-
Notifications
You must be signed in to change notification settings - Fork 1
/
ZmpPlaner.h
133 lines (105 loc) · 3.93 KB
/
ZmpPlaner.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#ifndef ZMPPLANER_H
#define ZMPPLANER_H
#include <deque>
//#define NEAR0 1e-8
// OpenHRP
//#include "hrpModel/Body.h"
//#include "hrpModel/Link.h"
//#include "hrpModel/JointPath.h"
//#include "hrpModel/ModelLoaderUtil.h"
//#include "hrpUtil/MatrixSolvers.h"
//#include "hrpUtil/uBlasCommonTypes.h"
#include "myfunc.h"
#include "spline.h"
#include "wuNewType.h"
//use preview control operater
#include "preview_control/PreviewControl.h"
class ZmpPlaner {
public:
ZmpPlaner();
//ZmpPlaner(FootType FT, double *prm);
~ZmpPlaner();
void setInit(vector2 &Ini);
void setInit(double &xIni, double &yIni);
//void PlanZMPnew(FootType FT, BodyPtr body, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp);
void PlanZMPnew(FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp);
//void calcSwingLegXYONew(FootType FT, BodyPtr body, vector2 swLegRef_p, Matrix3 object_ref_R);
void PlanZMPnew_toe(BodyPtr body, FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp);
void PlanZMPnew_toe_dynamic(BodyPtr body, FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp);
void calcSwingLegXYONew(FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R);
void calcSwingLegXYONew_toe(BodyPtr body, FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R);
//void calcWaistR( FootType FT, Matrix3 *R_ref);
Matrix3 calcWaistR( FootType FT, BodyPtr m_robot);
void atan2adjust(double &pre, double &cur);
void StopZMP(FootType FT, std::deque<vector2> &rfzmp, int count);
//capture point/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_
void PlanCP( BodyPtr m_robot, FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp, bool usePivot=0);
void PlanCPstop(BodyPtr m_robot ,FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, std::deque<vector2> &rfzmp);
void calcSwingLegCP( BodyPtr m_robot, FootType FT, Vector3 *p_ref, Matrix3 *R_ref, vector2 swLegRef_p, Matrix3 object_ref_R, bool usePivot);
void setw(double &wIn);
void getNextCom(Vector3 &cm_ref);
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_
/*
void ShiftZMP(FootType FT,double *prm, double *prmPre, BodyPtr body, std::deque<vector2> &rfzmp, int count, Vector3 cm, PreviewControl *PCOri, Vector3* p_Init);
void gaitChange(FootType FT,double *prm, double *prmPre, int count, BodyPtr body,Vector3* p_Init);
//pc
void setPreviewControl(double z);
bool calZmpErr(PreviewControl *PCIn, std::deque<vector2> &rfzmp,int tem, vector2 &offset);
*/
int step1Num;
int NomalPaceNum;
std::deque<Vector3> swingLegTraj;//x y theta
std::deque<vector2> swLegxy;
std::deque<double> Trajzd;
std::deque<Matrix3> swLeg_R;
std::deque<Matrix3> rot_pitch;
std::deque<double> index;
//for pitch
std::deque<Vector3> link_b_deque;
Vector3 link_b_front;
Vector3 link_b_rear;
double offsetZMPx;
double offsetZMPy;
bool stopOper;
int TsupNum,TdblNum;
int beforeUpNum;
double Tsup;
double Tdbl;
//for capture point
std::deque<vector2> cp_deque;
vector2 cp;//last cp of one step
double w;
vector2 cZMP;
vector2 cm_vel;
///
private:
//new
vector2 RLEG2LLEG;
vector2 LLEG2RLEG;
vector2 zmpInit;
vector2 offsetZMPr;
vector2 offsetZMPl;
std::deque<vector2> rfzmpCopy;
vector2 rzmpEnd;
double disfoot;
double displaceY;
double displaceNY;
double Now;
double xInit;
double yInit;
double Zup;
double Tv;
double dt;
int it;
double *zmptemp;
double pitch_angle;
double Tp;
double stopPoint;
double pitchMax;
double prmCache[3];
//for calc zmpErr
PreviewControl *PC;
vector2 limitZmpErr;
std::deque<vector2> rfzmpOri;
};
#endif