forked from Technician13/MIT-Cheetah-Note
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBalanceController
356 lines (296 loc) · 18.5 KB
/
BalanceController
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
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
变量：
NUM_CONSTRAINTS_QP：QP问题的约束个数（20）
NUM_VARIABLES_QP：QP问题变量个数（12）
函数：
>>>>>>> BalanceController::BalanceController()
BalanceController类的构造函数，实例化一个QProblem类的对象QProblemObj_qpOASES以及lcm类的对象lcm。
初始化用于QP问题的参数：
H_eigen（12×12）
A_eigen（20×12）
g_eigen（12×1）
xOpt_eigen（12×1）
yOpt_eigen（32×1）
初始化机器人本体和环境参数：
mass = 41 质量
inertia = 0.01 惯性
Ig 3×3的单位阵，赋值为 inertia * Ig
gravity 重力向量 3×1
direction_normal_flatGround 地面法向向量 3×1
direction_tangential_flatGround 地面切向向量 3×1
contact_state 接触状态向量 4×1 （初始状态四足着地，所以全部元素为1）
minNormalForces_feet GRF最小值向量 4×1
maxNormalForces_feet GRF最大值向量 4×1
初始化用于实际机器人运动的参数：
x_COM_world CoM在世界坐标系中的位置向量 3×1
xdot_COM_world CoM在世界坐标系中的速度向量 3×1
omega_b_world 机体在世界坐标系中的角速度向量 3×1
quat_b_world 四元数向量 4×1
R_b_world 旋转矩阵 3×3
p_feet 足端位置矩阵 3×4
R_yaw_act 偏航角旋转矩阵 3×3 （初始赋值为单位阵）
初始化用于机器人期望运动的参数：
x_COM_world_desired CoM在世界坐标系中的期望位置向量 3×1
xdot_COM_world_desired CoM在世界坐标系中的期望速度向量 3×1
xddot_COM_world_desired CoM在世界坐标系中的期望加速度向量 3×1
omega_b_world_desired 机体在世界坐标系中的期望角速度向量 3×1
omegadot_b_world_desired 机体在世界坐标系中的期望角加速度向量 3×1
R_b_world_desired 期望旋转矩阵 3×3
orientation_error 旋转偏差向量 3×1
error_x_rotated
error_dx_rotated
error_theta_rotated
error_dtheta_rotated
vbd_command_eigen (3×1)初始化为0
一些中间矩阵：
omegaHat
tempSkewMatrix3
tempVector3
A_control 6×(3*4)的矩阵
b_control 6×1的向量
b_control_Opt 6×1的向量
S_control 6×6的矩阵
W_control 12×12的矩阵
C_control 20×(3*4)的矩阵
C_times_f_control 20×1的向量
xOptPrev 12×1的向量
yOptPrev (12+20)×1的向量
xOpt_qpOASES 12×1的向量，赋值为0
xOpt_initialGuess 12×1的向量，赋值为0
xOpt_initialGuess 12×1的向量，其中第3、6、9和12个元素初值赋100
yOpt_qpOASES (12+20)×1的向量，元素全部为0
调用函数set_QPWeights()
调用函数set_RobotLimits()
调用函数set_worldData()
设置CoM在世界坐标系中的期望位置向量为[ -0.14, 0.0, 0.57]T
设置CoM在世界坐标系中的期望速度向量为[0, 0, 0]T
设置机体在世界坐标系中的期望角速度向量为[0, 0, 0]T
设置期望旋转矩阵为单位阵，即横滚角、俯仰角和偏航角的期望值均为0，机体不发生任何旋转行为
设置PD控制器的参数：
Kp_COMx = 0;
Kp_COMy = 0;
Kp_COMz = 0;
Kd_COMx = 0;
Kd_COMy = 0;
Kd_COMz = 0;
Kp_Base_roll = 0;
Kp_Base_pitch = 0;
Kp_Base_yaw = 0;
Kd_Base_roll = 0;
Kd_Base_pitch = 0;
Kd_Base_yaw = 0;
yaw_act 实际的机体偏航角，初始值为0
cpu_time = 0.001;
cpu_time_fixed = 0.001;
qp_exit_flag Qp问题退出标志位，初始值为-1.0;
qp_not_init QP问题未初始化标志位，初始值为1.0;
>>>>>>> BalanceController::set_QPWeights()
函数功能：将矩阵S_control、W_control设为单位阵，alpha_control赋值0.1
>>>>>>> BalanceController::set_RobotLimits()
函数功能：将向量minNormalForces_feet的元素全部设置为10,将向量maxNormalForces_feet的元素全部设置为160，即限定GRF范围
>>>>>>> BalanceController::set_worldData()
函数功能：将向量direction_normal_flatGround设置为[0,0,1]T，即地面法向与z轴正方向重合
将向量gravity设置为[0,0,9.81]T，即重力方向与z轴反方向重合，大小为9.81
将向量direction_tangential_flatGround设置为[0.7071,0.7071,0]T，即地面切向
设置mu_friction为0.05,即设定摩擦系数的值
>>>>>>> BalanceController::updateProblemData(double* xfb_in, double* p_feet_in,
double* p_des, double* p_act,
double* v_des, double* v_act,
double* O_err, double yaw_act_in)
函数功能：调用函数copy_Array_to_Eigen将输入参数xfb_in的第0～3位赋值给向量quat_b_world（4×1）
将输入参数xfb_in的第4～6位赋值给向量x_COM_world（3×1）
将输入参数xfb_in的第7～9位赋值给向量omega_b_world（3×1）
将输入参数xfb_in的第10～12位赋值给向量xdot_COM_world（3×1）
将输入参数p_feet_in赋值给矩阵p_feet（3×4）
将输入参数yaw_act_in赋值给变量yaw_act
偏航角旋转矩阵R_yaw_act的初始化：
rotZ(yaw) = | cos(yaw_act) -sin(yaw_act) 0 |
| sin(yaw_act) cos(yaw_act) 0 |
| 0 0 1 |
调用函数quaternion_to_rotationMatrix将四元数向量quat_b_world转换为旋转矩阵R_b_world
调用函数calc_PDcontrol()，得到 b_control (6×1)
调用函数update_A_control()，得到 A_control (6×12)
调用函数calc_H_qpOASES()，得到向量H_qpOASES (1×144)
调用函数calc_A_qpOASES()，得到向量A_qpOASES (1×240)
调用函数calc_g_qpOASES()，得到向量g_qpOASES (1×12)
调用函数update_log_variables，初始化优化问题的参数
>>>>>>> BalanceController::copy_Array_to_Eigen(Eigen::VectorXd& target,
double* source, int len, int startIndex)
函数功能：将输入数组source从角标为startIndex起，复制len个元素到向量target
>>>>>>> BalanceController::quaternion_to_rotationMatrix(Eigen::MatrixXd& R, Eigen::VectorXd& quat)
函数功能：将输入的四元数向量quat转换为旋转矩阵，应用Rodrigues公式：
| 1-2q2^2-2q3^2 2*q1*q2-2q0*q3 2*q1*q3+2q0*q2 |
| 2*q1*q2+2*q0*q3 1-2q1^2-2q3^2 2*q2*q3-2*q0*q1 |
| 2*q1*q3-2*q0*q2 2*q2*q3+2*q1*q0 1-2*q1^2-2*q2^2 |
>>>>>>> BalanceController::calc_PDcontrol()
函数功能：/*************旋转矩阵是正交阵，所以矩阵的转置等于逆矩阵*************/
将CoM的期望位置与实际位置之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_x_rotated
将CoM的期望速度与实际速度之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_dx_rotated
调用函数matrixLogRot将旋转矩阵转换为旋转向量orientation_error (3×1)
//参考论文High-slope terrain locomotion for torque-controlled quadruped robots
将机体的期望角速度与实际角速度之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_dtheta_rotated
两组PD控制器：
xddot_COM_world_desired = Kp_COM × error_x_rotated + Kd_COM × error_dx_rotated
omegadot_b_world_desired = Kp_Base × orientation_error + Kd_Base × error_dtheta_rotated
将Ig(转动惯量)赋值为：
| 0.35 0 0 |
| 0 2.1 0 |
| 0 0 2.1 |
矩阵II是将惯量矩阵Ig转换到世界坐标系下的表示，接下来b_control的计算均是在世界坐标系下完成
将向量b_control (6×1)赋值为期望的力和力矩
其中期望的力 = 质量 × （ 期望的加速度 + 重力加速度 ）（3×1）
期望的力矩 = 转动惯量 × 期望的角加速度 （3×1）
>>>>>>> BalanceController::matrixLogRot(const Eigen::MatrixXd& R, Eigen::VectorXd& omega)
函数功能：将输入的旋转矩阵R转换为旋转向量omega输出
>>>>>>> BalanceController::calc_constraint_check()
函数功能：对qp_controller_data对象中的成员赋值
>>>>>>> BalanceController::update_A_control()
函数功能：将矩阵A_control (6×12)赋值
调用函数crossMatrix，将着地腿的位置向量p_feet.col(i)转换为矩阵tempSkewMatrix3
| |
| R_yaw_act.transpose() R_yaw_act.transpose() R_yaw_act.transpose() R_yaw_act.transpose() |
| (3×3) (3×3) (3×3) (3×3) |
| R_yaw_act.transpose()* R_yaw_act.transpose()* R_yaw_act.transpose()* R_yaw_act.transpose()* |
| tempSkewMatrix3 tempSkewMatrix3 tempSkewMatrix3 tempSkewMatrix3 |
| (3×3) (3×3) (3×3) (3×3) |
>>>>>>> BalanceController::crossMatrix(Eigen::MatrixXd& R, const Eigen::VectorXd& omega)
函数功能：将输入向量omega转换为叉乘矩阵R输出
R = | 0 -omega(2) omega(1) |
| omega(2) 0 -omega(0) |
| -omega(1) omega(0) 0 |
>>>>>>> BalanceController::calc_H_qpOASES()
函数功能：计算H_eigen(12×12)
H_eigen = 2 * [ A.transpose * S * A + (alpha+0.001) * W ]
调用copy_Eigen_to_real_t函数，将矩阵H_eigen转换为向量H_qpOASES
>>>>>>> BalanceController::copy_Eigen_to_real_t(real_t* target, Eigen::MatrixXd& source, int nRows, int nCols)
函数功能：将输入矩阵的[0<=i<nRows , 0<=j<nCols]元素赋值给向量target[i+j]输出
>>>>>>> BalanceController::calc_A_qpOASES()
函数功能：计算C_control (20×12)
调用copy_Eigen_to_real_t函数，将矩阵C_control转换为向量A_qpOASES
>>>>>>> BalanceController::calc_g_qpOASES()
函数功能：计算g_eigen(12×1)
调用copy_Eigen_to_real_t函数，将向量g_eigen转换为向量g_qpOASES
>>>>>>> BalanceController::update_log_variables(double* p_des, double* p_act, double* v_des, double* v_act, double* O_err)
函数功能：调用函数copy_Eigen_to_double将CoM期望位置向量x_COM_world_desired (3×1)赋值给数组p_des (3×1)
将CoM实际位置向量x_COM_world (3×1)赋值给数组p_act (3×1)
将CoM期望速度向量xdot_COM_world_desired (3×1)赋值给数组v_des (3×1)
将CoM实际速度向量xdot_COM_world (3×1)赋值给数组v_act (3×1)
将旋转向量orientation_error (3×1)赋值给数组O_err (3×1)
将对应变量赋值给qp_controller_data类中对应的数据成员
>>>>>>> BalanceController::copy_Eigen_to_double(double* target, Eigen::VectorXd& source, int length)
函数功能：将输入向量source的前length个元素赋值给数组target
>>>>>>> BalanceController::update_log_variables(double* p_des, double* p_act, double* v_des, double* v_act, double* O_err)
函数功能：对优化问题的参数进行赋值
qp_controller_data.p_des = x_COM_world_desired （p_des）
qp_controller_data.p_act = x_COM_world（p_act）
qp_controller_data.v_des = xdot_COM_world_desired（v_des）
qp_controller_data.v_act = xdot_COM_world（v_act）
qp_controller_data.O_err = orientation_error（O_err）
qp_controller_data.omegab_des = omega_b_world_desired
qp_controller_data.omegab_act = omega_b_world
>>>>>>> BalanceController::SetContactData(double* contact_state_in, double* min_forces_in, double* max_forces_in)
函数功能：设置与触地相关的参数
调用copy_Array_to_Eigen函数,分别将：
数组contact_state_in赋值给向量contact_state
数组min_forces_in赋值给向量minNormalForces_feet
数组max_forces_in赋值给向量maxNormalForces_feet
调用函数calc_lb_ub_qpOASES()以及calc_lbA_ubA_qpOASES()完成力上下界的配置
>>>>>>> BalanceController::calc_lb_ub_qpOASES()
函数功能：计算 lb_qpOASES和ub_qpOASES，二者均为向量，作用是限定限定每条腿足端三个方向分力的范围
>>>>>>> BalanceController::calc_lbA_ubA_qpOASES()
函数功能：计算 lbA_qpOASES和ubA_qpOASES，二者分别对应论文
High-slope terrain locomotion for torque-controlled quadruped robots中公式(8)的d_和d-
>>>>>>> BalanceController::set_desired_swing_pos(double* pFeet_des_in)
函数功能：将输入数组pFeet_des_in的十二个元素赋值给数组qp_controller_data.pfeet_des
>>>>>>> BalanceController::set_actual_swing_pos(double* pFeet_act_in)
函数功能：将输入数组pFeet_act_in的十二个元素赋值给qp_controller_data.pfeet_act
>>>>>>> BalanceController::solveQP_nonThreaded(double* xOpt)
函数功能：QProblemObj_qpOASES对象的实例化并求解，将初始化的返回值存储到变量qp_exit_flag中
决策变量的结果存储在数组xOpt_qpOASES中 (12)
决策变量和约束的结果存储在数组yOpt_qpOASES中 (12+20)
将边界对象guessedBounds和约束对象guessedConstraints赋值
调用函数copy_real_t_to_Eigen()，将求解得到的数组xOpt_qpOASES赋值给向量xOpt_eigen (12×1)
b_control_Opt 机体坐标系下，通过优化结果反推的合力和合力矩向量 (6×1)
qp_controller_data.b_control_Opt就是b_control_Opt
将向量xOpt_eigen赋值给向量xOptPrev
xOpt为函数输入数组 (12),存储每个足端对地面施加的力 (GRF等大反向)在机体坐标系下的表示
重置QP问题
调用函数calc_constraint_check()，完成对qp_controller_data对象中的成员的赋值
qp_controller_data_publish对象就是qp_controller_data对象
>>>>>>> BalanceController::copy_real_t_to_Eigen(Eigen::VectorXd& target, real_t* source, int len)
函数功能：将输入数组source的前len个元素赋值给向量target输出
>>>>>>> BalanceController::calc_constraint_check()
函数功能：C_times_f_control 为约束矩阵C_control与优化后的结果向量xOpt_eigen的乘积
将qp_controller_data对象中的成员进行赋值：
qp_controller_data.xOpt = xOpt_eigen
qp_controller_data.lbA = lbA_qpOASES
qp_controller_data.ubA = ubA_qpOASES
qp_controller_data.C_times_f = C_times_f_control
qp_controller_data.b_control = b_control
>>>>>>> BalanceController::verifyModel(double* vbd_command)
函数功能：计算根据优化结果反推的在机体坐标系下的加速度 (合加速度减去重力加速度)
>>>>>>> BalanceController::publish_data_lcm()
函数功能使用lcm对象发布结果 (qp_controller_data_publish对象)
>>>>>>> BalanceController::set_PDgains(double* Kp_COM_in, double* Kd_COM_in,
double* Kp_Base_in, double* Kd_Base_in)
函数功能：设定PD控制器的参数
>>>>>>> BalanceController::set_desiredTrajectoryData(double* rpy_des_in, double* p_des_in, double* omegab_des_in, double* v_des_in)
函数功能：将输入数组p_des_in赋值给CoM期望位置向量x_COM_world_desired (3×1)
调用函数rpyToR()，根据输入数组rpy_des_in计算旋转向量R_b_world_desired
根据输入数组omegab_des_in，给机体在世界坐标系中的期望角速度向量omega_b_world_desired (3×1)赋值
根据输入数组v_des_in，给CoM在世界坐标系中的期望速度向量xdot_COM_world_desired (3×1)赋值
>>>>>>> BalanceController::rpyToR(Eigen::MatrixXd& R, double* rpy_in)
函数功能：根据输入数组rpy_in (横滚角、俯仰角、偏航角)计算旋转矩阵R输出
>>>>>>> BalanceController::set_wrench_weights(double* COM_weights_in, double* Base_weights_in)
函数功能：给S_control矩阵对角线元素赋值，表示权重的矩阵
>>>>>>> BalanceController::set_QP_options(double use_hard_constraint_pitch_in)
函数功能：设定QP问题求解方式
>>>>>>> BalanceController::set_QPWeights()
函数功能：将矩阵S_control和W_control设定为单位阵， alpha_control设定为0.1，权重矩阵和权重系数的初始化
>>>>>>> BalanceController::set_worldData()
函数功能：设定环境相关的参数：
地面的法向方向 direction_normal_flatGround << 0, 0, 1
重力大小及方向 gravity << 0, 0, 9.81
地面的切向方向 direction_tangential_flatGround << 0.7071, 0.7071, 0
摩擦系数 mu_friction = 0.05
>>>>>>> BalanceController::set_friction(double mu_in)
函数功能：根据输入变量mu_in给变量mu_friction赋值
>>>>>>> BalanceController::set_alpha_control(double alpha_control_in)
函数功能：根据输入变量alpha_control_in给变量alpha_control赋值
>>>>>>> BalanceController::set_mass(double mass_in)
函数功能：根据输入变量mass_in给变量mass赋值
>>>>>>> BalanceController::set_RobotLimits()
函数功能：设定足端法向分力的最小值10和最大值160
>>>>>>> BalanceController::getQPFinished()
函数功能：QP问题是否求解完毕，返回标志位
>>>>>>> BalanceController::print_QPData()
函数功能：调用print_real_t()函数打印QP问题参数：
H_qpOASES、A_qpOASES、g_qpOASES、lb_qpOASES、ub_qpOASES、lbA_qpOASES、ubA_qpOASES
>>>>>>> BalanceController::print_real_t(real_t* matrix, int nRows, int nCols)
函数功能：将输入数组matrix打印成矩阵形式 (nRows×nCols)
>>>>>>> BalanceController::matrixExpOmegaCross(const Eigen::VectorXd& omega, Eigen::MatrixXd& R)
函数功能：罗徳里格斯公式的代码实现
###############################################################################################################################
总结：
主要的函数调用关系：
updateProblemData
|___calc_PDcontrol
|___update_A_control
|___calc_H_qpOASES
|___calc_A_qpOASES
|___calc_g_qpOASES
|___update_log_variables
SetContactData
|___calc_lb_ub_qpOASES
|___calc_lbA_ubA_qpOASES
solveQP_nonThreaded
|___calc_constraint_check
此工程主要完成了基于QP的足端GRF的生成。本质上是通过使用qp_OASES求解器求解如下的二次规划问题：
min 0.5 * x.transpose * H_qpOASES * x + x.transpose * g_qpOASES
s.t. lbA_qpOASES <= A_qpOASES * x <= ubA_qpOASES
lb_qpOASES <= x<= ub_qpOASES
其中：
g_qpOASES <<<< -2 * ( A_control.transpose * S_control * b_control - xOptPrev.transpose * alpha_control )
A_qpOASES <<<< C_control
H_qpOASES <<<< 2 * [ A_control.transpose * S_control * A_control + (alpha_control + 1e-3) * W_control ]
注："<<<<"符号此处表示矩阵或向量与数组之间的转换
END