forked from Technician13/MIT-Cheetah-Note
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathConvexMPCLocomotion
198 lines (169 loc) · 13.3 KB
/
ConvexMPCLocomotion
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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
参考论文：
[1] J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt and S. Kim, "Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 1-9, doi: 10.1109/IROS.2018.8594448.
[2] G. Bledt, M. J. Powell, B. Katz, J. Di Carlo, P. M. Wensing and S. Kim, "MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 2245-2252, doi: 10.1109/IROS.2018.8593885.
变量：
horizonLength：一个完整的步态周期由10个MPC段组成，即horizonLength个MPC段
iterationsBetweenMPC：每个MPC段由iterationsBetweenMPC次计算组成
dt：每次计算的时间长度(参考论文中给出的频率貌似是1kHz，所以这里的dt应该是1ms)
dtMPC = dt * iterationsBetweenMPC：每个MPC段的时间
x_comp_integral：z轴方向的加速度与x轴方向速度之间的关系参数，即x_drag
iterationCounter：从开始运行到当前一共进行iterationCounter次计算
firstSwing：标志位，如果当前腿处于支撑相，则为true；处于摆动相，则为false
函数：
>>>>>>> ConvexMPCLocomotion::ConvexMPCLocomotion(float _dt, int _iterations_between_mpc, MIT_UserParameters* parameters)
函数功能：类ConvexMPCLocomotion的构造函数，使用初始化列表，实例化：
trotting对象：对角步态，触地时间比例50%
bounding对象：跳跃步态，左前腿和右前腿先同时离地。左后腿和右后腿再同时离地，触地时间比例40%
pronking对象：四条腿先同时触地，再同时离地，触地时间比例40%
jumping对象：四条腿先同时触地，再同时离地，触地时间比例20%
galloping对象：奔跑步态，四条腿依次离地，再依次触地，触地时间比例40%
standing对象：站立步态，四条腿一直站立，触地时间比例100%
trotRunning对象：对角小跑步态，触地时间比例40%
walking对象：行走步态，四条腿依次离地，再依次触地，触地时间比例50%
walking2对象：对角慢走步态，触地时间比例70%
pacing对象：其实就是trotting换了个摆腿顺序
输出MPC计算的相关信息
调用setup_problem函数规定规划问题中的矩阵的维度。完成问题的初始化，其中摩擦系数定义为0.4,力的极限值定义为120
向量rpy_comp(3*1)和向量rpy_int(3*1)初始化为0向量
bool类型的数组firstSwing(4)所有元素都被初始化为true
调用initSparseMPC()函数
向量pBody_des(3*1)和向量vBody_des(3*1)和向量aBody_des(3*1)初始化为0向量
>>>>>>> void ConvexMPCLocomotion::initSparseMPC()
函数功能：创建机体惯性矩阵baseInertia
baseInertia = | 0.07 0 0 |
| 0 0.26 0 |
| 0 0 0.242 |
定义机体的质量mass = 9
定义力的极限值maxForce = 120
创建向量dtTraj，长度为horizonLength，每个元素为dtMPC
创建权重向量weights(12*1)
调用_sparseCMPC对象中的成员函数完成问题的配置
>>>>>>> void ConvexMPCLocomotion::recompute_timing(int iterations_per_mpc)
函数功能：根据形参iterations_per_mpc的值重新计算iterationsBetweenMPC和dtMPC
>>>>>>> void ConvexMPCLocomotion::updateMPCIfNeeded(int *mpcTable, ControlFSMData<float> &data, bool omniMode)
函数功能：判断iterationCounter对iterationsBetweenMPC是否等于零(从变量名来看这应该是一个计数器，检测当前是不是一个MPC段的开始状态)，如果等于0：
调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中，返回值的内容包括：
腿部触地估计向量seResult(4*1)
位置向量position(3*1)
机体坐标系下的速度向量vBody(3*1)
四元数orientation
机体坐标系下的旋转角速度omegaBody(3*1)
机体旋转矩阵rBody(3*3)
旋转欧拉角Roll/Yaw/Pitch向量rpy(3*1)
世界坐标系下的旋转角速度omegaWorld(3*1)
世界坐标系下的速度向量vWorld(3*1)
机体坐标系下的加速度向量aBody(3*1)
世界坐标系下的加速度向量aWorld(3*1)
对lcm通信框架的配置
将机体的位置向量position记录在指针p
设置机体坐标系下的期望速度向量v_des_robot(3*1)，存储_x_vel_des、_y_vel_des以及0,表示三个方向上的期望速度
设置世界坐标系下的期望速度向量，这里需要进行一次是否使用全方位模式的判断
如果当前的步态为站立步态(current_gait == 4)
定义数组trajInitial(12)，存放机体期望横滚角、期望俯仰角、期望偏航角、期望X坐标、期望Y坐标、机体高度(期望Z坐标)，以及6个0(期望的速度和角速度)
将其按照MPC段进行扩展
否则：
定义位置的误差max_pos_error = 0.1
定义xStart为机体在世界坐标系下的X方向的期望位置
定义yStart为机体在世界坐标系下的Y方向的期望位置
重新设定期望值xStart和yStart，使得二者与实际值之间的差距不大于max_pos_error
world_position_desired[0] = xStart;
world_position_desired[1] = yStart;
定义数组trajInitial(12)，存放机体期望横滚角、期望俯仰角、期望偏航角、期望X坐标、期望Y坐标、机体高度(期望Z坐标)，0,0,偏航角速度，世界坐标系下的X方向速度，世界坐标系下的Y方向速度，0
将其按照MPC段进行扩展，其中与位置有关的项，下一MPC段的值与当前MPC段的值之间的变化过程视为匀速运动；与速度有关的项，下一MPC段的值与当前MPC段的值之间视为不变
选择调用solveSparseMPC或者solveDenseMPC函数进行求解(稀疏或者稠密？？？)
>>>>>>> void ConvexMPCLocomotion::solveDenseMPC(int *mpcTable, ControlFSMData<float> &data)
函数功能：调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中
创建权重向量Q并用指针weights指向Q
估计的旋转角中的Yaw存储在变量yaw中
设置一个很小的数alpha
指针p指向位置向量position
指针v指向估计的世界坐标系下的速度向量vWorld
指针w指向估计的世界坐标系下的角速度向量omegaWorld
指针q指向四元数向量orientation
12维数组r存储机体中心到每个足端的向量的x、y、z值，存储方式：[x0 x1 x2 x3 y0 y1 y2 y3 z0 z1 z2 z3]
向量pxy_act(3*1)存储机体在xy平面上的实际位置向量(position向量的前两个元素)，第三个元素设置为0
向量pxy_des(3*1)存储机体在xy平面上的期望位置向量(world_position_desired向量，也就是在updateMPCIfNeeded函数中推导的xStart和yStart)，第三个元素设置为0
pz_err为z方向的偏差值
向量vxy(3*1)存储机体在xy平面上的实际速度向量(vWorld向量的前两个元素)，第三个元素设置为0
调用setup_problem函数完成MPC问题的设定
调用update_x_drag函数完成对于z轴方向的加速度与x轴方向速度之间的关系参数的设定，x_comp_integral即为这个系数
如果机体在x方向上的速度超过0.3,对系数x_comp_integral进行更新(这个更新算法不是很懂。。。。)
调用update_solver_settings函数对JCQP求解器进行配置
调用update_problem_data_floats函数完成MPC问题的求解
创建循环：
创建向量f(3*1)
对四条腿进行遍历，每次遍历，f存储对应的腿地面反作用力(GRF，也就是刚刚求解的MPC问题的结果)，调用get_solution函数完成反作用力的获取
f中现在存储的是在世界坐标系下的某条腿的地面反作用力的数值，需要将它转换到机体坐标系下的足端输出力，转换后的结果存储到f_ff中(3*4)
将四条腿的地面反作用力(4个f)存储到Fr_des中(3*4)
>>>>>>> void ConvexMPCLocomotion::solveSparseMPC(int *mpcTable, ControlFSMData<float> &data)
函数功能：求解MPC问题，逻辑和solveDenseMPC差不多
>>>>>>> void ConvexMPCLocomotion::_SetupCommand(ControlFSMData<float> & data)
函数功能：设置每项的期望值
根据当前机器人是Mini Cheetah还是Cheetah 3设置不同的机体高度
根据是否使用遥控器进行判断，如果使用遥控器：
实例化类rc_control_settings的对象rc_cmd，存储遥控数据
否则使用默认方式
_x_vel_des相当于是一个逐步加速的过程，逐渐从当前的_x_vel_des逼近x_vel_cmd，最后与x_vel_cmd相等
_y_vel_des与_x_vel_des处理方式相同
_yaw_des在一个dt时间间隔内认为是匀加速运动
_roll_des和_pitch_des均设置为0
>>>>>>> void ConvexMPCLocomotion::run(ControlFSMData<float>& data)
函数功能：关闭全方位模式
调用_SetupCommand函数，完成对期望值以及工作方式的设置
变量gaitNumber中存储当前的步态编号
调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中
四足机器人站立
根据不同的gaitNumber数值，实例化不同的类Gait的对象
调用setIterations函数分别计算当前步态和jumping步态的_iteration以及_phase，其中，_iteration为当前处于一个步态周期中的第几段，_phase为当前在一个步态周期中的百分比
检查jump行为是否被触发
检查jump动作
向量v_des_robot(3*1)存储期望速度，分别为_x_vel_des、_y_vel_des和0
向量v_des_world(3*1)存储在世界坐标系下的期望速度，这里需要进行一次是否使用全方位模式的判断
向量v_robot(3*1)存储当前在世界坐标系下的速度
判断当前机器人在x方向的速度的绝对值是否大于0.2,如果大于0.2：
更新初始的Pitch(这个更新算法不是很懂。。。。)
判断当前机器人在y方向的速度的绝对值是否大于0.1,如果大于0.1：
更新初始的Roll(这个更新算法不是很懂。。。。)
将初始的Pitch和初始的Roll进行范围限定
计算Roll的补偿值和Pitch的补偿值(这里的算法不是很懂。。。。)
向量pFoot(3*4)中存储四个足端在世界坐标系下的坐标
如果当前的步态不是站立步态：
设定世界坐标系下的目标位置(x方向的期望位置，y方向的期望位置，0偏航角)，为初始目标位置加上期望速度对时间的积分
判断是不是第一次运行，如果是第一次运行：
x方向的期望位置即为x方向的当前位置，y方向的期望位置即为y方向的当前位置，期望的偏航角即为当前的偏航角
设定四条腿的摆动轨迹，这里第一次运行的话，四足机器人将执行原地踏步的动作
初次运行标志为置为false
遍历四条腿，设定四条腿的摆动时长
数组side_sign存储腿部编号(-1：右侧的两条腿，1：左侧的两条腿)
创建循环，遍历四条腿：
判断是否是第一次摆动，进行摆动倒计时
设定摆动腿抬腿高度0.06
向量offset为y方向的偏移量
向量pRobotFrame为髋关节在机体坐标系下的修正后的结果
变量stance_time存储支撑持续时长
向量des_vel存储期望的速度(_x_vel_des、_y_vel_des、0.0)
最后计算得到四条腿的落足点的坐标，这里参考论文[2]的公式6有表述
计算次数计数器加1
设定支撑腿的Kp和Kd增益
向量contactStates调用getContactState函数存储四条腿当前处于支撑相的百分比，如果处于摆动相，则置0
向量swingStates调用getSwingState函数存储四条腿当前处于摆动相的百分比，如果处于支撑相，则置0
调用getMpcTable函数获取预测的接触状态列表，并使用指针mpcTable指向这个列表
调用函数updateMPCIfNeeded完成GRF的计算
创建向量se_contactState(4*1)存储估计的足端触地状态并初始化为全0向量
创建循环，遍历四条腿：
变量contactState存储这条腿当前处于支撑相的百分比(如果处于摆动相，则为0)
变量swingState存储这条腿当前处于摆动相的百分比(如果处于支撑相，则为0)
判断当前是否处于摆动相，如果处于v摆动相：
判断标志位firstSwing的值，可以判断是否刚刚从支撑相进入摆动相
调用computeSwingTrajectoryBezier函数规划摆动轨迹
向量pDesFootWorld(3*1)调用getPosition函数存储当前摆动腿足端在世界坐标系下的位置信息
向量vDesFootWorld(3*1)调用getVelocity函数存储当前摆动腿足端在世界坐标系下的速度信息
向量pDesLeg(3*1)存储当前足端位置坐标在髋关节坐标系下的坐标信息
向量vDesLeg(3*1)存储当前足端速度在髋关节坐标系下的表示方式
向量pFoot_des存储摆动腿足端在世界坐标系下的位置信息
向量vFoot_des存储摆动腿足端在世界坐标系下的速度信息
向量aFoot_des存储摆动腿足端在世界坐标系下的加速度信息
判断是否使用WBC控制，发送数据
否则，即这条腿处于支撑相：
firstSwing置true
与上文同样，定义pDesFootWorld、vDesFootWorld、pDesLeg和vDesLeg(这里怀疑源码有问题，因为当前是对支撑腿进行处理，而这里却调用了摆动腿的相关函数)