├── BalanceController ├── ConvexMPCLocomotion ├── FootSwingTrajectory ├── Gait ├── LegController ├── PositionVelocityEstimator ├── Quadruped ├── README.md ├── RobotState ├── SolverMPC ├── WBC ├── BodyOriTask ├── BodyPosTask ├── BodyPostureTask ├── ContactSpec ├── KinWBC ├── LinkPosTask ├── LocomotionCtrl ├── SingleContact ├── WBC ├── WBC_Ctrl └── WBIC ├── convexMPC_interface └── ref ├── Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control.pdf ├── High-slope Terrain Locomotion for Torque-Controlled Quadruped.pdf ├── MIT Cheetah 3 Design and Control of a Robust, Dynamic Quadruped Robot.pdf ├── Probabilistic Contact Estimation and Impact Detection for State Estimation of Quadruped Robots.pdf ├── Rigid Body Dynamics Algorithms.pdf └── highly dynamic quadruped locomotion via whole-body impulse control and model predictive control.pdf /BalanceController: -------------------------------------------------------------------------------- 1 | 变量： 2 | NUM_CONSTRAINTS_QP：QP问题的约束个数（20） 3 | NUM_VARIABLES_QP：QP问题变量个数（12） 4 | 5 | 函数： 6 | >>>>>>> BalanceController::BalanceController() 7 | BalanceController类的构造函数，实例化一个QProblem类的对象QProblemObj_qpOASES以及lcm类的对象lcm。 8 | 初始化用于QP问题的参数： 9 | H_eigen（12×12） 10 | A_eigen（20×12） 11 | g_eigen（12×1） 12 | xOpt_eigen（12×1） 13 | yOpt_eigen（32×1） 14 | 初始化机器人本体和环境参数： 15 | mass = 41 质量 16 | inertia = 0.01 惯性 17 | Ig 3×3的单位阵，赋值为 inertia * Ig 18 | gravity 重力向量 3×1 19 | direction_normal_flatGround 地面法向向量 3×1 20 | direction_tangential_flatGround 地面切向向量 3×1 21 | contact_state 接触状态向量 4×1 （初始状态四足着地，所以全部元素为1） 22 | minNormalForces_feet GRF最小值向量 4×1 23 | maxNormalForces_feet GRF最大值向量 4×1 24 | 初始化用于实际机器人运动的参数： 25 | x_COM_world CoM在世界坐标系中的位置向量 3×1 26 | xdot_COM_world CoM在世界坐标系中的速度向量 3×1 27 | omega_b_world 机体在世界坐标系中的角速度向量 3×1 28 | quat_b_world 四元数向量 4×1 29 | R_b_world 旋转矩阵 3×3 30 | p_feet 足端位置矩阵 3×4 31 | R_yaw_act 偏航角旋转矩阵 3×3 （初始赋值为单位阵） 32 | 初始化用于机器人期望运动的参数： 33 | x_COM_world_desired CoM在世界坐标系中的期望位置向量 3×1 34 | xdot_COM_world_desired CoM在世界坐标系中的期望速度向量 3×1 35 | xddot_COM_world_desired CoM在世界坐标系中的期望加速度向量 3×1 36 | omega_b_world_desired 机体在世界坐标系中的期望角速度向量 3×1 37 | omegadot_b_world_desired 机体在世界坐标系中的期望角加速度向量 3×1 38 | R_b_world_desired 期望旋转矩阵 3×3 39 | orientation_error 旋转偏差向量 3×1 40 | 41 | 42 | error_x_rotated 43 | error_dx_rotated 44 | error_theta_rotated 45 | error_dtheta_rotated 46 | vbd_command_eigen (3×1)初始化为0 47 | 一些中间矩阵： 48 | omegaHat 49 | tempSkewMatrix3 50 | tempVector3 51 | A_control 6×(3*4)的矩阵 52 | b_control 6×1的向量 53 | b_control_Opt 6×1的向量 54 | S_control 6×6的矩阵 55 | W_control 12×12的矩阵 56 | C_control 20×(3*4)的矩阵 57 | C_times_f_control 20×1的向量 58 | xOptPrev 12×1的向量 59 | yOptPrev (12+20)×1的向量 60 | xOpt_qpOASES 12×1的向量，赋值为0 61 | xOpt_initialGuess 12×1的向量，赋值为0 62 | xOpt_initialGuess 12×1的向量，其中第3、6、9和12个元素初值赋100 63 | yOpt_qpOASES (12+20)×1的向量，元素全部为0 64 | 调用函数set_QPWeights() 65 | 调用函数set_RobotLimits() 66 | 调用函数set_worldData() 67 | 设置CoM在世界坐标系中的期望位置向量为[ -0.14, 0.0, 0.57]T 68 | 设置CoM在世界坐标系中的期望速度向量为[0, 0, 0]T 69 | 设置机体在世界坐标系中的期望角速度向量为[0, 0, 0]T 70 | 设置期望旋转矩阵为单位阵，即横滚角、俯仰角和偏航角的期望值均为0，机体不发生任何旋转行为 71 | 设置PD控制器的参数： 72 | Kp_COMx = 0; 73 | Kp_COMy = 0; 74 | Kp_COMz = 0; 75 | 76 | Kd_COMx = 0; 77 | Kd_COMy = 0; 78 | Kd_COMz = 0; 79 | 80 | Kp_Base_roll = 0; 81 | Kp_Base_pitch = 0; 82 | Kp_Base_yaw = 0; 83 | 84 | Kd_Base_roll = 0; 85 | Kd_Base_pitch = 0; 86 | Kd_Base_yaw = 0; 87 | yaw_act 实际的机体偏航角，初始值为0 88 | cpu_time = 0.001; 89 | cpu_time_fixed = 0.001; 90 | qp_exit_flag Qp问题退出标志位，初始值为-1.0; 91 | qp_not_init QP问题未初始化标志位，初始值为1.0; 92 | 93 | >>>>>>> BalanceController::set_QPWeights() 94 | 函数功能：将矩阵S_control、W_control设为单位阵，alpha_control赋值0.1 95 | 96 | >>>>>>> BalanceController::set_RobotLimits() 97 | 函数功能：将向量minNormalForces_feet的元素全部设置为10,将向量maxNormalForces_feet的元素全部设置为160，即限定GRF范围 98 | 99 | >>>>>>> BalanceController::set_worldData() 100 | 函数功能：将向量direction_normal_flatGround设置为[0,0,1]T，即地面法向与z轴正方向重合 101 | 将向量gravity设置为[0,0,9.81]T，即重力方向与z轴反方向重合，大小为9.81 102 | 将向量direction_tangential_flatGround设置为[0.7071,0.7071,0]T，即地面切向 103 | 设置mu_friction为0.05,即设定摩擦系数的值 104 | 105 | >>>>>>> BalanceController::updateProblemData(double* xfb_in, double* p_feet_in, 106 | double* p_des, double* p_act, 107 | double* v_des, double* v_act, 108 | double* O_err, double yaw_act_in) 109 | 函数功能：调用函数copy_Array_to_Eigen将输入参数xfb_in的第0～3位赋值给向量quat_b_world（4×1） 110 | 将输入参数xfb_in的第4～6位赋值给向量x_COM_world（3×1） 111 | 将输入参数xfb_in的第7～9位赋值给向量omega_b_world（3×1） 112 | 将输入参数xfb_in的第10～12位赋值给向量xdot_COM_world（3×1） 113 | 将输入参数p_feet_in赋值给矩阵p_feet（3×4） 114 | 将输入参数yaw_act_in赋值给变量yaw_act 115 | 偏航角旋转矩阵R_yaw_act的初始化： 116 | rotZ(yaw) = | cos(yaw_act) -sin(yaw_act) 0 | 117 | | sin(yaw_act) cos(yaw_act) 0 | 118 | | 0 0 1 | 119 | 调用函数quaternion_to_rotationMatrix将四元数向量quat_b_world转换为旋转矩阵R_b_world 120 | 调用函数calc_PDcontrol()，得到 b_control (6×1) 121 | 调用函数update_A_control()，得到 A_control (6×12) 122 | 调用函数calc_H_qpOASES()，得到向量H_qpOASES (1×144) 123 | 调用函数calc_A_qpOASES()，得到向量A_qpOASES (1×240) 124 | 调用函数calc_g_qpOASES()，得到向量g_qpOASES (1×12) 125 | 调用函数update_log_variables，初始化优化问题的参数 126 | 127 | >>>>>>> BalanceController::copy_Array_to_Eigen(Eigen::VectorXd& target, 128 | double* source, int len, int startIndex) 129 | 函数功能：将输入数组source从角标为startIndex起，复制len个元素到向量target 130 | 131 | >>>>>>> BalanceController::quaternion_to_rotationMatrix(Eigen::MatrixXd& R, Eigen::VectorXd& quat) 132 | 函数功能：将输入的四元数向量quat转换为旋转矩阵，应用Rodrigues公式： 133 | | 1-2q2^2-2q3^2 2*q1*q2-2q0*q3 2*q1*q3+2q0*q2 | 134 | | 2*q1*q2+2*q0*q3 1-2q1^2-2q3^2 2*q2*q3-2*q0*q1 | 135 | | 2*q1*q3-2*q0*q2 2*q2*q3+2*q1*q0 1-2*q1^2-2*q2^2 | 136 | 137 | >>>>>>> BalanceController::calc_PDcontrol() 138 | 函数功能：/*************旋转矩阵是正交阵，所以矩阵的转置等于逆矩阵*************/ 139 | 将CoM的期望位置与实际位置之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_x_rotated 140 | 将CoM的期望速度与实际速度之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_dx_rotated 141 | 调用函数matrixLogRot将旋转矩阵转换为旋转向量orientation_error (3×1) 142 | //参考论文High-slope terrain locomotion for torque-controlled quadruped robots 143 | 将机体的期望角速度与实际角速度之间的偏差向量转移到机体坐标系中（只考虑偏航角），赋值给error_dtheta_rotated 144 | 两组PD控制器： 145 | xddot_COM_world_desired = Kp_COM × error_x_rotated + Kd_COM × error_dx_rotated 146 | omegadot_b_world_desired = Kp_Base × orientation_error + Kd_Base × error_dtheta_rotated 147 | 将Ig(转动惯量)赋值为： 148 | | 0.35 0 0 | 149 | | 0 2.1 0 | 150 | | 0 0 2.1 | 151 | 矩阵II是将惯量矩阵Ig转换到世界坐标系下的表示，接下来b_control的计算均是在世界坐标系下完成 152 | 将向量b_control (6×1)赋值为期望的力和力矩 153 | 其中期望的力 = 质量 × （ 期望的加速度 + 重力加速度 ）（3×1） 154 | 期望的力矩 = 转动惯量 × 期望的角加速度 （3×1） 155 | 156 | >>>>>>> BalanceController::matrixLogRot(const Eigen::MatrixXd& R, Eigen::VectorXd& omega) 157 | 函数功能：将输入的旋转矩阵R转换为旋转向量omega输出 158 | 159 | >>>>>>> BalanceController::calc_constraint_check() 160 | 函数功能：对qp_controller_data对象中的成员赋值 161 | 162 | >>>>>>> BalanceController::update_A_control() 163 | 函数功能：将矩阵A_control (6×12)赋值 164 | 调用函数crossMatrix，将着地腿的位置向量p_feet.col(i)转换为矩阵tempSkewMatrix3 165 | | | 166 | | R_yaw_act.transpose() R_yaw_act.transpose() R_yaw_act.transpose() R_yaw_act.transpose() | 167 | | (3×3) (3×3) (3×3) (3×3) | 168 | | R_yaw_act.transpose()* R_yaw_act.transpose()* R_yaw_act.transpose()* R_yaw_act.transpose()* | 169 | | tempSkewMatrix3 tempSkewMatrix3 tempSkewMatrix3 tempSkewMatrix3 | 170 | | (3×3) (3×3) (3×3) (3×3) | 171 | 172 | >>>>>>> BalanceController::crossMatrix(Eigen::MatrixXd& R, const Eigen::VectorXd& omega) 173 | 函数功能：将输入向量omega转换为叉乘矩阵R输出 174 | R = | 0 -omega(2) omega(1) | 175 | | omega(2) 0 -omega(0) | 176 | | -omega(1) omega(0) 0 | 177 | 178 | >>>>>>> BalanceController::calc_H_qpOASES() 179 | 函数功能：计算H_eigen(12×12) 180 | H_eigen = 2 * [ A.transpose * S * A + (alpha+0.001) * W ] 181 | 调用copy_Eigen_to_real_t函数，将矩阵H_eigen转换为向量H_qpOASES 182 | 183 | >>>>>>> BalanceController::copy_Eigen_to_real_t(real_t* target, Eigen::MatrixXd& source, int nRows, int nCols) 184 | 函数功能：将输入矩阵的[0<=i>>>>>> BalanceController::calc_A_qpOASES() 187 | 函数功能：计算C_control (20×12) 188 | 调用copy_Eigen_to_real_t函数，将矩阵C_control转换为向量A_qpOASES 189 | 190 | >>>>>>> BalanceController::calc_g_qpOASES() 191 | 函数功能：计算g_eigen(12×1) 192 | 调用copy_Eigen_to_real_t函数，将向量g_eigen转换为向量g_qpOASES 193 | 194 | >>>>>>> BalanceController::update_log_variables(double* p_des, double* p_act, double* v_des, double* v_act, double* O_err) 195 | 函数功能：调用函数copy_Eigen_to_double将CoM期望位置向量x_COM_world_desired (3×1)赋值给数组p_des (3×1) 196 | 将CoM实际位置向量x_COM_world (3×1)赋值给数组p_act (3×1) 197 | 将CoM期望速度向量xdot_COM_world_desired (3×1)赋值给数组v_des (3×1) 198 | 将CoM实际速度向量xdot_COM_world (3×1)赋值给数组v_act (3×1) 199 | 将旋转向量orientation_error (3×1)赋值给数组O_err (3×1) 200 | 将对应变量赋值给qp_controller_data类中对应的数据成员 201 | 202 | >>>>>>> BalanceController::copy_Eigen_to_double(double* target, Eigen::VectorXd& source, int length) 203 | 函数功能：将输入向量source的前length个元素赋值给数组target 204 | 205 | >>>>>>> BalanceController::update_log_variables(double* p_des, double* p_act, double* v_des, double* v_act, double* O_err) 206 | 函数功能：对优化问题的参数进行赋值 207 | qp_controller_data.p_des = x_COM_world_desired （p_des） 208 | qp_controller_data.p_act = x_COM_world（p_act） 209 | qp_controller_data.v_des = xdot_COM_world_desired（v_des） 210 | qp_controller_data.v_act = xdot_COM_world（v_act） 211 | qp_controller_data.O_err = orientation_error（O_err） 212 | qp_controller_data.omegab_des = omega_b_world_desired 213 | qp_controller_data.omegab_act = omega_b_world 214 | 215 | >>>>>>> BalanceController::SetContactData(double* contact_state_in, double* min_forces_in, double* max_forces_in) 216 | 函数功能：设置与触地相关的参数 217 | 调用copy_Array_to_Eigen函数,分别将： 218 | 数组contact_state_in赋值给向量contact_state 219 | 数组min_forces_in赋值给向量minNormalForces_feet 220 | 数组max_forces_in赋值给向量maxNormalForces_feet 221 | 调用函数calc_lb_ub_qpOASES()以及calc_lbA_ubA_qpOASES()完成力上下界的配置 222 | 223 | >>>>>>> BalanceController::calc_lb_ub_qpOASES() 224 | 函数功能：计算 lb_qpOASES和ub_qpOASES，二者均为向量，作用是限定限定每条腿足端三个方向分力的范围 225 | 226 | >>>>>>> BalanceController::calc_lbA_ubA_qpOASES() 227 | 函数功能：计算 lbA_qpOASES和ubA_qpOASES，二者分别对应论文 228 | High-slope terrain locomotion for torque-controlled quadruped robots中公式(8)的d_和d- 229 | 230 | >>>>>>> BalanceController::set_desired_swing_pos(double* pFeet_des_in) 231 | 函数功能：将输入数组pFeet_des_in的十二个元素赋值给数组qp_controller_data.pfeet_des 232 | 233 | >>>>>>> BalanceController::set_actual_swing_pos(double* pFeet_act_in) 234 | 函数功能：将输入数组pFeet_act_in的十二个元素赋值给qp_controller_data.pfeet_act 235 | 236 | >>>>>>> BalanceController::solveQP_nonThreaded(double* xOpt) 237 | 函数功能：QProblemObj_qpOASES对象的实例化并求解，将初始化的返回值存储到变量qp_exit_flag中 238 | 决策变量的结果存储在数组xOpt_qpOASES中 (12) 239 | 决策变量和约束的结果存储在数组yOpt_qpOASES中 (12+20) 240 | 将边界对象guessedBounds和约束对象guessedConstraints赋值 241 | 调用函数copy_real_t_to_Eigen()，将求解得到的数组xOpt_qpOASES赋值给向量xOpt_eigen (12×1) 242 | b_control_Opt 机体坐标系下，通过优化结果反推的合力和合力矩向量 (6×1) 243 | qp_controller_data.b_control_Opt就是b_control_Opt 244 | 将向量xOpt_eigen赋值给向量xOptPrev 245 | xOpt为函数输入数组 (12),存储每个足端对地面施加的力 (GRF等大反向)在机体坐标系下的表示 246 | 重置QP问题 247 | 调用函数calc_constraint_check()，完成对qp_controller_data对象中的成员的赋值 248 | qp_controller_data_publish对象就是qp_controller_data对象 249 | 250 | >>>>>>> BalanceController::copy_real_t_to_Eigen(Eigen::VectorXd& target, real_t* source, int len) 251 | 函数功能：将输入数组source的前len个元素赋值给向量target输出 252 | 253 | >>>>>>> BalanceController::calc_constraint_check() 254 | 函数功能：C_times_f_control 为约束矩阵C_control与优化后的结果向量xOpt_eigen的乘积 255 | 将qp_controller_data对象中的成员进行赋值： 256 | qp_controller_data.xOpt = xOpt_eigen 257 | qp_controller_data.lbA = lbA_qpOASES 258 | qp_controller_data.ubA = ubA_qpOASES 259 | qp_controller_data.C_times_f = C_times_f_control 260 | qp_controller_data.b_control = b_control 261 | 262 | >>>>>>> BalanceController::verifyModel(double* vbd_command) 263 | 函数功能：计算根据优化结果反推的在机体坐标系下的加速度 (合加速度减去重力加速度) 264 | 265 | >>>>>>> BalanceController::publish_data_lcm() 266 | 函数功能使用lcm对象发布结果 (qp_controller_data_publish对象) 267 | 268 | >>>>>>> BalanceController::set_PDgains(double* Kp_COM_in, double* Kd_COM_in, 269 | double* Kp_Base_in, double* Kd_Base_in) 270 | 函数功能：设定PD控制器的参数 271 | 272 | >>>>>>> BalanceController::set_desiredTrajectoryData(double* rpy_des_in, double* p_des_in, double* omegab_des_in, double* v_des_in) 273 | 函数功能：将输入数组p_des_in赋值给CoM期望位置向量x_COM_world_desired (3×1) 274 | 调用函数rpyToR()，根据输入数组rpy_des_in计算旋转向量R_b_world_desired 275 | 根据输入数组omegab_des_in，给机体在世界坐标系中的期望角速度向量omega_b_world_desired (3×1)赋值 276 | 根据输入数组v_des_in，给CoM在世界坐标系中的期望速度向量xdot_COM_world_desired (3×1)赋值 277 | 278 | >>>>>>> BalanceController::rpyToR(Eigen::MatrixXd& R, double* rpy_in) 279 | 函数功能：根据输入数组rpy_in (横滚角、俯仰角、偏航角)计算旋转矩阵R输出 280 | 281 | >>>>>>> BalanceController::set_wrench_weights(double* COM_weights_in, double* Base_weights_in) 282 | 函数功能：给S_control矩阵对角线元素赋值，表示权重的矩阵 283 | 284 | >>>>>>> BalanceController::set_QP_options(double use_hard_constraint_pitch_in) 285 | 函数功能：设定QP问题求解方式 286 | 287 | >>>>>>> BalanceController::set_QPWeights() 288 | 函数功能：将矩阵S_control和W_control设定为单位阵， alpha_control设定为0.1，权重矩阵和权重系数的初始化 289 | 290 | >>>>>>> BalanceController::set_worldData() 291 | 函数功能：设定环境相关的参数： 292 | 地面的法向方向 direction_normal_flatGround << 0, 0, 1 293 | 重力大小及方向 gravity << 0, 0, 9.81 294 | 地面的切向方向 direction_tangential_flatGround << 0.7071, 0.7071, 0 295 | 摩擦系数 mu_friction = 0.05 296 | 297 | >>>>>>> BalanceController::set_friction(double mu_in) 298 | 函数功能：根据输入变量mu_in给变量mu_friction赋值 299 | 300 | >>>>>>> BalanceController::set_alpha_control(double alpha_control_in) 301 | 函数功能：根据输入变量alpha_control_in给变量alpha_control赋值 302 | 303 | >>>>>>> BalanceController::set_mass(double mass_in) 304 | 函数功能：根据输入变量mass_in给变量mass赋值 305 | 306 | >>>>>>> BalanceController::set_RobotLimits() 307 | 函数功能：设定足端法向分力的最小值10和最大值160 308 | 309 | >>>>>>> BalanceController::getQPFinished() 310 | 函数功能：QP问题是否求解完毕，返回标志位 311 | 312 | >>>>>>> BalanceController::print_QPData() 313 | 函数功能：调用print_real_t()函数打印QP问题参数： 314 | H_qpOASES、A_qpOASES、g_qpOASES、lb_qpOASES、ub_qpOASES、lbA_qpOASES、ubA_qpOASES 315 | 316 | >>>>>>> BalanceController::print_real_t(real_t* matrix, int nRows, int nCols) 317 | 函数功能：将输入数组matrix打印成矩阵形式 (nRows×nCols) 318 | 319 | >>>>>>> BalanceController::matrixExpOmegaCross(const Eigen::VectorXd& omega, Eigen::MatrixXd& R) 320 | 函数功能：罗徳里格斯公式的代码实现 321 | 322 | 323 | ############################################################################################################################### 324 | 总结： 325 | 主要的函数调用关系： 326 | updateProblemData 327 | |___calc_PDcontrol 328 | |___update_A_control 329 | |___calc_H_qpOASES 330 | |___calc_A_qpOASES 331 | |___calc_g_qpOASES 332 | |___update_log_variables 333 | SetContactData 334 | |___calc_lb_ub_qpOASES 335 | |___calc_lbA_ubA_qpOASES 336 | solveQP_nonThreaded 337 | |___calc_constraint_check 338 | 339 | 此工程主要完成了基于QP的足端GRF的生成。本质上是通过使用qp_OASES求解器求解如下的二次规划问题： 340 | min 0.5 * x.transpose * H_qpOASES * x + x.transpose * g_qpOASES 341 | s.t. lbA_qpOASES <= A_qpOASES * x <= ubA_qpOASES 342 | lb_qpOASES <= x<= ub_qpOASES 343 | 其中： 344 | g_qpOASES <<<< -2 * ( A_control.transpose * S_control * b_control - xOptPrev.transpose * alpha_control ) 345 | A_qpOASES <<<< C_control 346 | H_qpOASES <<<< 2 * [ A_control.transpose * S_control * A_control + (alpha_control + 1e-3) * W_control ] 347 | 注："<<<<"符号此处表示矩阵或向量与数组之间的转换 348 | 349 | END 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | -------------------------------------------------------------------------------- /ConvexMPCLocomotion: -------------------------------------------------------------------------------- 1 | 参考论文： 2 | [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. 3 | [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. 4 | 5 | 6 | 变量： 7 | horizonLength：一个完整的步态周期由10个MPC段组成，即horizonLength个MPC段 8 | iterationsBetweenMPC：每个MPC段由iterationsBetweenMPC次计算组成 9 | dt：每次计算的时间长度(参考论文中给出的频率貌似是1kHz，所以这里的dt应该是1ms) 10 | dtMPC = dt * iterationsBetweenMPC：每个MPC段的时间 11 | x_comp_integral：z轴方向的加速度与x轴方向速度之间的关系参数，即x_drag 12 | iterationCounter：从开始运行到当前一共进行iterationCounter次计算 13 | firstSwing：标志位，如果当前腿处于支撑相，则为true；处于摆动相，则为false 14 | 15 | 函数： 16 | >>>>>>> ConvexMPCLocomotion::ConvexMPCLocomotion(float _dt, int _iterations_between_mpc, MIT_UserParameters* parameters) 17 | 函数功能：类ConvexMPCLocomotion的构造函数，使用初始化列表，实例化： 18 | trotting对象：对角步态，触地时间比例50% 19 | bounding对象：跳跃步态，左前腿和右前腿先同时离地。左后腿和右后腿再同时离地，触地时间比例40% 20 | pronking对象：四条腿先同时触地，再同时离地，触地时间比例40% 21 | jumping对象：四条腿先同时触地，再同时离地，触地时间比例20% 22 | galloping对象：奔跑步态，四条腿依次离地，再依次触地，触地时间比例40% 23 | standing对象：站立步态，四条腿一直站立，触地时间比例100% 24 | trotRunning对象：对角小跑步态，触地时间比例40% 25 | walking对象：行走步态，四条腿依次离地，再依次触地，触地时间比例50% 26 | walking2对象：对角慢走步态，触地时间比例70% 27 | pacing对象：其实就是trotting换了个摆腿顺序 28 | 输出MPC计算的相关信息 29 | 调用setup_problem函数规定规划问题中的矩阵的维度。完成问题的初始化，其中摩擦系数定义为0.4,力的极限值定义为120 30 | 向量rpy_comp(3*1)和向量rpy_int(3*1)初始化为0向量 31 | bool类型的数组firstSwing(4)所有元素都被初始化为true 32 | 调用initSparseMPC()函数 33 | 向量pBody_des(3*1)和向量vBody_des(3*1)和向量aBody_des(3*1)初始化为0向量 34 | 35 | >>>>>>> void ConvexMPCLocomotion::initSparseMPC() 36 | 函数功能：创建机体惯性矩阵baseInertia 37 | baseInertia = | 0.07 0 0 | 38 | | 0 0.26 0 | 39 | | 0 0 0.242 | 40 | 定义机体的质量mass = 9 41 | 定义力的极限值maxForce = 120 42 | 创建向量dtTraj，长度为horizonLength，每个元素为dtMPC 43 | 创建权重向量weights(12*1) 44 | 调用_sparseCMPC对象中的成员函数完成问题的配置 45 | 46 | >>>>>>> void ConvexMPCLocomotion::recompute_timing(int iterations_per_mpc) 47 | 函数功能：根据形参iterations_per_mpc的值重新计算iterationsBetweenMPC和dtMPC 48 | 49 | >>>>>>> void ConvexMPCLocomotion::updateMPCIfNeeded(int *mpcTable, ControlFSMData &data, bool omniMode) 50 | 函数功能：判断iterationCounter对iterationsBetweenMPC是否等于零(从变量名来看这应该是一个计数器，检测当前是不是一个MPC段的开始状态)，如果等于0： 51 | 调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中，返回值的内容包括： 52 | 腿部触地估计向量seResult(4*1) 53 | 位置向量position(3*1) 54 | 机体坐标系下的速度向量vBody(3*1) 55 | 四元数orientation 56 | 机体坐标系下的旋转角速度omegaBody(3*1) 57 | 机体旋转矩阵rBody(3*3) 58 | 旋转欧拉角Roll/Yaw/Pitch向量rpy(3*1) 59 | 世界坐标系下的旋转角速度omegaWorld(3*1) 60 | 世界坐标系下的速度向量vWorld(3*1) 61 | 机体坐标系下的加速度向量aBody(3*1) 62 | 世界坐标系下的加速度向量aWorld(3*1) 63 | 对lcm通信框架的配置 64 | 将机体的位置向量position记录在指针p 65 | 设置机体坐标系下的期望速度向量v_des_robot(3*1)，存储_x_vel_des、_y_vel_des以及0,表示三个方向上的期望速度 66 | 设置世界坐标系下的期望速度向量，这里需要进行一次是否使用全方位模式的判断 67 | 如果当前的步态为站立步态(current_gait == 4) 68 | 定义数组trajInitial(12)，存放机体期望横滚角、期望俯仰角、期望偏航角、期望X坐标、期望Y坐标、机体高度(期望Z坐标)，以及6个0(期望的速度和角速度) 69 | 将其按照MPC段进行扩展 70 | 否则： 71 | 定义位置的误差max_pos_error = 0.1 72 | 定义xStart为机体在世界坐标系下的X方向的期望位置 73 | 定义yStart为机体在世界坐标系下的Y方向的期望位置 74 | 重新设定期望值xStart和yStart，使得二者与实际值之间的差距不大于max_pos_error 75 | world_position_desired[0] = xStart; 76 | world_position_desired[1] = yStart; 77 | 定义数组trajInitial(12)，存放机体期望横滚角、期望俯仰角、期望偏航角、期望X坐标、期望Y坐标、机体高度(期望Z坐标)，0,0,偏航角速度，世界坐标系下的X方向速度，世界坐标系下的Y方向速度，0 78 | 将其按照MPC段进行扩展，其中与位置有关的项，下一MPC段的值与当前MPC段的值之间的变化过程视为匀速运动；与速度有关的项，下一MPC段的值与当前MPC段的值之间视为不变 79 | 选择调用solveSparseMPC或者solveDenseMPC函数进行求解(稀疏或者稠密？？？) 80 | 81 | >>>>>>> void ConvexMPCLocomotion::solveDenseMPC(int *mpcTable, ControlFSMData &data) 82 | 函数功能：调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中 83 | 创建权重向量Q并用指针weights指向Q 84 | 估计的旋转角中的Yaw存储在变量yaw中 85 | 设置一个很小的数alpha 86 | 指针p指向位置向量position 87 | 指针v指向估计的世界坐标系下的速度向量vWorld 88 | 指针w指向估计的世界坐标系下的角速度向量omegaWorld 89 | 指针q指向四元数向量orientation 90 | 12维数组r存储机体中心到每个足端的向量的x、y、z值，存储方式：[x0 x1 x2 x3 y0 y1 y2 y3 z0 z1 z2 z3] 91 | 向量pxy_act(3*1)存储机体在xy平面上的实际位置向量(position向量的前两个元素)，第三个元素设置为0 92 | 向量pxy_des(3*1)存储机体在xy平面上的期望位置向量(world_position_desired向量，也就是在updateMPCIfNeeded函数中推导的xStart和yStart)，第三个元素设置为0 93 | pz_err为z方向的偏差值 94 | 向量vxy(3*1)存储机体在xy平面上的实际速度向量(vWorld向量的前两个元素)，第三个元素设置为0 95 | 调用setup_problem函数完成MPC问题的设定 96 | 调用update_x_drag函数完成对于z轴方向的加速度与x轴方向速度之间的关系参数的设定，x_comp_integral即为这个系数 97 | 如果机体在x方向上的速度超过0.3,对系数x_comp_integral进行更新(这个更新算法不是很懂。。。。) 98 | 调用update_solver_settings函数对JCQP求解器进行配置 99 | 调用update_problem_data_floats函数完成MPC问题的求解 100 | 创建循环： 101 | 创建向量f(3*1) 102 | 对四条腿进行遍历，每次遍历，f存储对应的腿地面反作用力(GRF，也就是刚刚求解的MPC问题的结果)，调用get_solution函数完成反作用力的获取 103 | f中现在存储的是在世界坐标系下的某条腿的地面反作用力的数值，需要将它转换到机体坐标系下的足端输出力，转换后的结果存储到f_ff中(3*4) 104 | 将四条腿的地面反作用力(4个f)存储到Fr_des中(3*4) 105 | 106 | >>>>>>> void ConvexMPCLocomotion::solveSparseMPC(int *mpcTable, ControlFSMData &data) 107 | 函数功能：求解MPC问题，逻辑和solveDenseMPC差不多 108 | 109 | >>>>>>> void ConvexMPCLocomotion::_SetupCommand(ControlFSMData & data) 110 | 函数功能：设置每项的期望值 111 | 根据当前机器人是Mini Cheetah还是Cheetah 3设置不同的机体高度 112 | 根据是否使用遥控器进行判断，如果使用遥控器： 113 | 实例化类rc_control_settings的对象rc_cmd，存储遥控数据 114 | 否则使用默认方式 115 | _x_vel_des相当于是一个逐步加速的过程，逐渐从当前的_x_vel_des逼近x_vel_cmd，最后与x_vel_cmd相等 116 | _y_vel_des与_x_vel_des处理方式相同 117 | _yaw_des在一个dt时间间隔内认为是匀加速运动 118 | _roll_des和_pitch_des均设置为0 119 | 120 | >>>>>>> void ConvexMPCLocomotion::run(ControlFSMData& data) 121 | 函数功能：关闭全方位模式 122 | 调用_SetupCommand函数，完成对期望值以及工作方式的设置 123 | 变量gaitNumber中存储当前的步态编号 124 | 调用data对象中的_stateEstimator对象成员的getResult()函数，返回值存储到seResult中 125 | 四足机器人站立 126 | 根据不同的gaitNumber数值，实例化不同的类Gait的对象 127 | 调用setIterations函数分别计算当前步态和jumping步态的_iteration以及_phase，其中，_iteration为当前处于一个步态周期中的第几段，_phase为当前在一个步态周期中的百分比 128 | 检查jump行为是否被触发 129 | 检查jump动作 130 | 向量v_des_robot(3*1)存储期望速度，分别为_x_vel_des、_y_vel_des和0 131 | 向量v_des_world(3*1)存储在世界坐标系下的期望速度，这里需要进行一次是否使用全方位模式的判断 132 | 向量v_robot(3*1)存储当前在世界坐标系下的速度 133 | 判断当前机器人在x方向的速度的绝对值是否大于0.2,如果大于0.2： 134 | 更新初始的Pitch(这个更新算法不是很懂。。。。) 135 | 判断当前机器人在y方向的速度的绝对值是否大于0.1,如果大于0.1： 136 | 更新初始的Roll(这个更新算法不是很懂。。。。) 137 | 将初始的Pitch和初始的Roll进行范围限定 138 | 计算Roll的补偿值和Pitch的补偿值(这里的算法不是很懂。。。。) 139 | 向量pFoot(3*4)中存储四个足端在世界坐标系下的坐标 140 | 如果当前的步态不是站立步态： 141 | 设定世界坐标系下的目标位置(x方向的期望位置，y方向的期望位置，0偏航角)，为初始目标位置加上期望速度对时间的积分 142 | 判断是不是第一次运行，如果是第一次运行： 143 | x方向的期望位置即为x方向的当前位置，y方向的期望位置即为y方向的当前位置，期望的偏航角即为当前的偏航角 144 | 设定四条腿的摆动轨迹，这里第一次运行的话，四足机器人将执行原地踏步的动作 145 | 初次运行标志为置为false 146 | 遍历四条腿，设定四条腿的摆动时长 147 | 数组side_sign存储腿部编号(-1：右侧的两条腿，1：左侧的两条腿) 148 | 创建循环，遍历四条腿： 149 | 判断是否是第一次摆动，进行摆动倒计时 150 | 设定摆动腿抬腿高度0.06 151 | 向量offset为y方向的偏移量 152 | 向量pRobotFrame为髋关节在机体坐标系下的修正后的结果 153 | 变量stance_time存储支撑持续时长 154 | 向量des_vel存储期望的速度(_x_vel_des、_y_vel_des、0.0) 155 | 最后计算得到四条腿的落足点的坐标，这里参考论文[2]的公式6有表述 156 | 计算次数计数器加1 157 | 设定支撑腿的Kp和Kd增益 158 | 向量contactStates调用getContactState函数存储四条腿当前处于支撑相的百分比，如果处于摆动相，则置0 159 | 向量swingStates调用getSwingState函数存储四条腿当前处于摆动相的百分比，如果处于支撑相，则置0 160 | 调用getMpcTable函数获取预测的接触状态列表，并使用指针mpcTable指向这个列表 161 | 调用函数updateMPCIfNeeded完成GRF的计算 162 | 创建向量se_contactState(4*1)存储估计的足端触地状态并初始化为全0向量 163 | 创建循环，遍历四条腿： 164 | 变量contactState存储这条腿当前处于支撑相的百分比(如果处于摆动相，则为0) 165 | 变量swingState存储这条腿当前处于摆动相的百分比(如果处于支撑相，则为0) 166 | 判断当前是否处于摆动相，如果处于v摆动相： 167 | 判断标志位firstSwing的值，可以判断是否刚刚从支撑相进入摆动相 168 | 调用computeSwingTrajectoryBezier函数规划摆动轨迹 169 | 向量pDesFootWorld(3*1)调用getPosition函数存储当前摆动腿足端在世界坐标系下的位置信息 170 | 向量vDesFootWorld(3*1)调用getVelocity函数存储当前摆动腿足端在世界坐标系下的速度信息 171 | 向量pDesLeg(3*1)存储当前足端位置坐标在髋关节坐标系下的坐标信息 172 | 向量vDesLeg(3*1)存储当前足端速度在髋关节坐标系下的表示方式 173 | 向量pFoot_des存储摆动腿足端在世界坐标系下的位置信息 174 | 向量vFoot_des存储摆动腿足端在世界坐标系下的速度信息 175 | 向量aFoot_des存储摆动腿足端在世界坐标系下的加速度信息 176 | 判断是否使用WBC控制，发送数据 177 | 否则，即这条腿处于支撑相： 178 | firstSwing置true 179 | 与上文同样，定义pDesFootWorld、vDesFootWorld、pDesLeg和vDesLeg(这里怀疑源码有问题，因为当前是对支撑腿进行处理，而这里却调用了摆动腿的相关函数) 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | -------------------------------------------------------------------------------- /FootSwingTrajectory: -------------------------------------------------------------------------------- 1 | 类外实现的函数： 2 | >>>>>>> template 3 | void FootSwingTrajectory::computeSwingTrajectoryBezier(T phase, T swingTime) 4 | 函数功能：_p存储通过调用Interpolate::cubicBezier>函数生成实时的足端位置坐标向量，引入的参数为类FootSwingTrajectory中的私有数据成员_p0，_pf以及phase，三者分别表示 5 | 摆动相的起始位置，终点位置以及此时摆动相持续时长占完整时长的比例（0到1之间）。_p0，_pf以及phase三者的赋值操作在类内实现。 6 | 所使用的摆动相足端位置方程为：( yf - y0 ) * x^3 + 3 * ( yf - y0 ) * x^2 * ( 1 - x ) + y0 7 | _v存储通过调用Interpolate::cubicBezierFirstDerivative>函数生成实时的足端速度向量，引入的参数为类FootSwingTrajectory中的私有数据成员_p0，_pf，phase以及函数 8 | 的输入参数swingTime，其中swingTime应该表示的是采样间隔时间 9 | 所使用的摆动相足端速度方程为：[ ( yf - y0 ) * 6 * x * ( 1 - x ) ] / swingTime 10 | _a存储通过调用Interpolate::cubicBezierSecondDerivative>函数生成实时的足端加速度向量，引入的参数为类FootSwingTrajectory中的私有数据成员_p0，_pf，phase以 11 | 及函数的输入参数swingTime 12 | 所使用的摆动相足端加速度方程为：[ ( yf - y0 ) * ( 6 - 12 * x ) ] / ( swingTime * swingTime ) 13 | 以 phase = 0.5 作为分界，即认为当 phase = 0.5 时摆动腿足端达到最高点，以此为界将足端轨迹分为上升阶段和下降阶段两部分进行规划，此处完成的是足端竖直方向的规划 14 | 15 | 这里我的理解是：if判断之前完成足端水平方向上的规划，if判断之后完成足端竖直方向上的规划 16 | 17 | 类内实现的函数： 18 | >>>>>>> FootSwingTrajectory() 19 | 函数功能：类FootSwingTrajectory的构造函数，完成对成员函数的初始化 20 | 21 | >>>>>>> setInitialPosition(Vec3 p0) 22 | 函数功能：根据输入参数p0完成对成员函数_p0的赋值 23 | 24 | >>>>>>> setFinalPosition(Vec3 pf) 25 | 函数功能：根据输入参数pf完成对成员函数_pf的赋值 26 | 27 | >>>>>>> setHeight(T h) 28 | 函数功能：根据输入参数h完成对成员函数_height的赋值 29 | 30 | >>>>>>> getPosition() 31 | 函数功能：返回数据成员_p 32 | 33 | >>>>>>> getVelocity() 34 | 函数功能：返回数据成员_v 35 | 36 | >>>>>>> getAcceleration() 37 | 函数功能：返回数据成员_a 38 | 39 | ############################################################################################################################### 40 | 总结： 41 | 对摆动腿的足端位置、速度和加速度通过三次贝塞尔曲线插值进行规划 42 | -------------------------------------------------------------------------------- /Gait: -------------------------------------------------------------------------------- 1 | 参考论文： 2 | [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. 3 | 4 | 变量： 5 | currentIteration：当前迭代次数 6 | iterationsPerMPC：每段迭代次数 7 | _nIterations：一步中有多少段(论文中给出的是10～16) 8 | _iteration：当前处于一个步态周期中的第几段 9 | _phase：当前在一个步态周期中的百分比 10 | 11 | 函数： 12 | >>>>>>> OffsetDurationGait(int nSegment, Vec4 offset, Vec4 durations, const std::string& name) 13 | 函数功能：类OffsetDurationGait的构造函数 14 | 将形参name赋值给对象中的成员_name 15 | 分配内存，建立_mpc_table数组(nSegment * 4) 16 | 数组_offsetsFloat存储offsets中每个元素对与nSegment相除之后的值(映射到0-1之间) 17 | 数组_durationsFloat存储durations中每个元素对与nSegment相除之后的值(映射到0-1之间) 18 | _stance存储数组durations中的第0个元素 19 | _swing存储nSegment与_stance之差 20 | 21 | >>>>>>> MixedFrequncyGait::MixedFrequncyGait(int nSegment, Vec4 periods, float duty_cycle, const std::string &name) 22 | 函数功能：类MixedFrequncyGait的构造函数 23 | 将形参name赋值给对象中的成员_name 24 | 将形参_duty_cycle赋值给对象中的成员_duty_cycle 25 | 分配内存，建立_mpc_table数组(nSegment * 4) 26 | 将形参periods赋值给对象中的成员_periods 27 | 将形参nSegment赋值给对象中的成员_nIterations 28 | 将对象中的成员_iteration初始化为0 29 | 将对象中的成员_phase初始化为全0数组(4) 30 | 31 | >>>>>>> OffsetDurationGait::~OffsetDurationGait() 32 | 函数功能：类OffsetDurationGait的析构函数 33 | 释放掉构造函数中为_mpc_table申请的内存区域 34 | 35 | >>>>>>> MixedFrequncyGait::~MixedFrequncyGait() 36 | 函数功能：类MixedFrequncyGait的析构函数 37 | 释放掉构造函数中为_mpc_table申请的内存区域 38 | 39 | >>>>>>> Vec4 OffsetDurationGait::getContactState() 40 | 函数功能：获取每条腿当前的状态占支撑相的百分比(函数名称的意思是获取接触状态，但是有文不对题的嫌疑) 41 | 创建四维数组progress，progress中的每个元素存储_phase与_offsetsFloat中每个元素之差，_phase是当前进行的进度(百分比)，_offsetsFloat是每条腿的相位差位于一个周期的百分比。这里举个例子：以walk步态为例，walk步态中_offsetsFloat=[0.00 0.25 0.50 0.75]，假设此时进行的进度_phase=[0.80 0.80 0.80 0.80],即此时机体的状态相比于起始状态已经过了0.8个步态周期，则progress=[0.80 0.55 0.30 0.05]，在walk步态中_durationsFloat=[0.75 0.75 0.75 0.75]。按照源码中接下来的处理，可以得到progress=[0.00 0.55/0.75 0.30/0.75 0.05/0.75]，所表达的意义是：第一个元素所代表的腿已经进入摆动相，因此置0；后三个元素所代表的是这三条腿目前处于支撑相的百分比 42 | 返回矩阵progress 43 | 44 | >>>>>>> Vec4 MixedFrequncyGait::getContactState() 45 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::getContactState()差不多 46 | 47 | >>>>>>> Vec4 OffsetDurationGait::getSwingState() 48 | 函数功能：获取每条腿当前的状态占摆动相的百分比 49 | 这里举个例子：以walk步态为例，walk步态中_offsetsFloat=[0.00 0.25 0.50 0.75]，_durationsFloat=[0.75 0.75 0.75 0.75]，所以swing_offset=[0.75 1.00 1.25 1.50]=[0.75 0.00 0.25 0.50]。数组swing_duration中存储在当前步态下，摆动相占一个步态周期的百分比，因此walk步态下，swing_duration=[0.25 0.25 0.25 0.25]。假设此时进行的进度_phase=[0.80 0.80 0.80 0.80],即此时机体的状态相比于起始状态已经过了0.8个步态周期，则progress=[0.05 0.80 0.55 0.30]。按照源码中接下来的处理，可以得到progress=[0.05/0.25 0.00 0.00 0.00]，所表达的意义是：第一个元素所代表的腿已经进入摆动相，目前处于摆动相的0.05/0.25位置处；后三个元素所代表的是这三条腿目前处于支撑相，因此置0 50 | 返回矩阵progress 51 | 52 | >>>>>>> Vec4 MixedFrequncyGait::getSwingState() 53 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::getSwingState()差不多 54 | 55 | >>>>>>> int* OffsetDurationGait::getMpcTable() 56 | 函数功能：在数组_mpc_table中存储每一段中每条腿的接触状态。所谓“段”就是将一个完整的步态周期分成若干段进行MPC规划，相当于是对一个完整的连续动作的离散化过程。 57 | 这里的接触状态是根据当前在总段数中的进度来判断的，如果触地，置1,否则置0 58 | 返回数组_mpc_table 59 | 60 | >>>>>>> int* MixedFrequncyGait::getMpcTable() 61 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::getMpcTable()差不多 62 | 63 | >>>>>>> void OffsetDurationGait::setIterations(int iterationsPerMPC, int currentIteration) 64 | 函数功能：_iteration为当前处于一个步态周期中的第几段 65 | _phase为当前在一个步态周期中的百分比 66 | 67 | >>>>>>> void MixedFrequncyGait::setIterations(int iterationsBetweenMPC, int currentIteration) 68 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::setIterations(int iterationsPerMPC, int currentIteration)差不多 69 | 70 | >>>>>>> int OffsetDurationGait::getCurrentGaitPhase() 71 | 函数功能：返回参数_iteration 72 | 73 | >>>>>>> int MixedFrequncyGait::getCurrentGaitPhase() { 74 | 函数功能：返回0 75 | 76 | >>>>>>> float OffsetDurationGait::getCurrentSwingTime(float dtMPC, int leg) 77 | 函数功能：返回摆动相的时间 78 | 79 | >>>>>>> float MixedFrequncyGait::getCurrentSwingTime(float dtMPC, int leg) 80 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::getCurrentSwingTime(float dtMPC, int leg)差不多 81 | 82 | >>>>>>> float OffsetDurationGait::getCurrentStanceTime(float dtMPC, int leg) 83 | 函数功能：返回支撑相的时间 84 | 85 | >>>>>>> float MixedFrequncyGait::getCurrentStanceTime(float dtMPC, int leg) { 86 | 函数功能：目前不是太懂，但是逻辑上与OffsetDurationGait::getCurrentStanceTime(float dtMPC, int leg)差不多 87 | 88 | >>>>>>> void OffsetDurationGait::debugPrint() 89 | 函数功能：空函数 90 | 91 | >>>>>>> void MixedFrequncyGait::debugPrint() 92 | 函数功能：空函数 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /LegController: -------------------------------------------------------------------------------- 1 | >>>>>>> template 2 | void LegControllerCommand::zero() 3 | 函数功能：将结构体LegControllerCommand中所有的参数置零 4 | 向量类型参数：tauFeedForward：输出的力矩（3*1） 5 | forceFeedForward：输出的力（3*1） 6 | qDes：期望的关节位置（3*1） 7 | qdDes：期望的关节速度（3*1） 8 | pDes：期望的足端位置（3*1） 9 | vDes：期望的足端速度（3*1） 10 | 矩阵类型参数：kpCartesian：笛卡尔空间比例系数，用来对足端行为进行控制（3*3） 11 | kdCartesian：笛卡尔空间微分系数，用来对足端行为进行控制（3*3） 12 | kpJoint：关节空间比例系数，用来对关节行为进行控制（3*3） 13 | kdJoint：关节空间微分系数，用来对关节行为进行控制（3*3） 14 | 15 | >>>>>>> template 16 | void LegControllerData::zero() 17 | 函数功能：将结构体LegControllerData中所有的参数置零 18 | 向量类型参数：q：实际的关节位置（3*1） 19 | qd：实际的关节速度（3*1） 20 | p：实际的足端位置（3*1） 21 | v：实际的足端速度（3*1） 22 | tauEstimate：关节力矩的估计值（3*1） 23 | 矩阵类型参数：J：Jacobian矩阵（3*3） 24 | 25 | >>>>>>> template 26 | void LegController::zeroCommand() 27 | 函数功能：commands为定义的4个元素的LegControllerCommand结构体数组 28 | for循环遍历调用4个元素中的zero()函数 29 | 将_legsEnabled变量置为false 30 | 31 | >>>>>>> template 32 | void LegController::edampCommand(RobotType robot, T gain) 33 | 函数功能：根据输入参数robot是Cheetah 3还是mini-cheetah，对微分系数矩阵kdCartesian或kdJoint使用输入参数gain进行对角赋值 34 | 35 | >>>>>>> template 36 | void LegController::updateData(const SpiData* spiData) 37 | 函数功能：datas为定义的4个元素的LegControllerData结构体数组 38 | 使用输入参数spiData中的成员对datas中的q和qd成员赋值 39 | 调用函数computeLegJacobianAndPosition，计算四条腿各自的Jacobian矩阵以及足端位置坐标 40 | 通过正运动学方法计算足端的速度 41 | 42 | >>>>>>> template 43 | void LegController::updateData(const TiBoardData* tiBoardData) 44 | 函数功能：datas为定义的4个元素的LegControllerData结构体数组 45 | 使用输入参数tiBoardData中的成员对datas中的q、qd、p和v成员赋值 46 | 调用函数computeLegJacobianAndPosition，计算四条腿各自的Jacobian矩阵以及足端位置坐标 47 | 使用输入参数tiBoardData中的关节力矩成员tau对datas中的tauEstimate成员赋值 48 | 49 | >>>>>>> template 50 | void LegController::updateCommand(SpiCommand* spiCommand) 51 | 函数功能：用commands中的成员更新spiCommand中的成员 52 | 53 | >>>>>>> template 54 | void LegController::updateCommand(TiBoardCommand* tiBoardCommand) 55 | 函数功能：用commands中的成员更新tiBoardCommand中的成员 56 | 57 | >>>>>>> template 58 | void LegController::setLcm(leg_control_data_lcmt *lcmData, leg_control_command_lcmt *lcmCommand) 59 | 函数功能：更新lcmData和lcmCommand中的成员，本质上就是将LegControllerCommand结构体和LegControllerData结构体中的成员分类存储到lcmData结构体和lcmCommand结构体中的对应成员中 60 | 61 | >>>>>>> template 62 | void computeLegJacobianAndPosition(Quadruped& quad, Vec3& q, Mat3* J, Vec3* p, int leg) 63 | 函数功能：将输入参数quad的abadLink长度、hipLink长度、kneeLink长度以及补偿量kneeLinkY分别存储在l1、l2、l3以及l4中 64 | 标志位sideSign用以区分左侧的腿和右侧的腿（左正右负），因为设计到腿部正运动学的足端在y方向上的坐标 65 | J为Jacobian矩阵，联系关节空间和足端笛卡尔空间 66 | p为足端在笛卡尔坐标系下的位置坐标，已知连杆长度和关节角度，通过正运动学求解得到足端位置坐标 67 | | l3 * sin(2 + 3) + l2 * sin(2) | 68 | p = | (l1 + l4) * sideSign * cos(1) + [l3 * cos(2 + 3) + l2 * cos(2)] * sin(1) | 69 | | (l1 + l4) * sideSign * sin(1) - [l3 * cos(2 + 3) + l2 * cos(2)] * cos(1) | 70 | 有关参考方向：以左前腿为例，从右前腿的位置看去： 71 | 对于髋关节，逆时针方向为正方向 72 | 对于膝关节，逆时针方向为正方向 73 | 从机体头部看去： 74 | 对于肩关节，逆时针方向为正方向 75 | 76 | ############################################################################################################################### 77 | 总结： 78 | 主要的参数成员： 79 | LegControllerCommand 80 | |___Vec3 81 | | |___tauFeedForward, 82 | | |___forceFeedForward 83 | | |___qDes 84 | | |___qdDes 85 | | |___pDes 86 | | |___vDes 87 | | 88 | |___Mat3 89 | |___kpCartesian 90 | |___kdCartesian 91 | |___kpJoint 92 | |___kdJoint 93 | LegControllerData 94 | |___Vec3 95 | | |___q 96 | | |___qd 97 | | |___p 98 | | |___v 99 | | |___tauEstimate 100 | | 101 | |___Mat3 102 | | |___J 103 | | 104 | |___Quadruped 105 | |___quadruped 106 | 107 | 此工程主要完成了机器人腿部状态量的数据处理和计算，构建Jacobian矩阵完成正运动学和逆运动学的计算，通过LCM完成数据的打包和通信 108 | 109 | END 110 | -------------------------------------------------------------------------------- /PositionVelocityEstimator: -------------------------------------------------------------------------------- 1 | 参考论文： 2 | [1]Camurri M, Fallon M, Bazeille S, et al. Probabilistic contact estimation and impact detection for state estimation of quadruped robots[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 1023-1030. 3 | 4 | 变量： 5 | Rbod：机体坐标系到世界坐标系的旋转矩阵(3*3) 6 | p_rel：机体坐标系下的足端位置坐标(3*1) 7 | p_f：世界坐标系下的足端位置坐标(3*1) 8 | _ps：以每个足端为基准，位于机体中心的世界坐标系的位置(12*1) 9 | trust：速度检测的信任度 10 | trust_window：速度检测的阈值 11 | 12 | 函数： 13 | >>>>>>> template 14 | void LinearKFPositionVelocityEstimator::setup() 15 | 函数功能：矩阵的初始化 16 | 17 | >>>>>>> template 18 | void LinearKFPositionVelocityEstimator::run() 19 | 函数功能：构建线性Kalman滤波框架： 20 | 从源码中的第152行开始： 21 | 构建28*1的列向量y，按照顺序存储_ps，_vs和pzs，其中： 22 | _ps：以每个足端为基准，位于机体中心的世界坐标系的位置(12*1) 23 | _vs：每个足端对于当前机体速度的检测值，这里可以参考论文[1]中的计算方法，与此同时，与上一时刻滤波得到的最终的速度值之间进行加权，权重系数为trust， 24 | 说白了也就是现在我到底信任谁的问题，是更信任上一时刻的最终滤波后的结果速度还是更信任此时通过腿部估计的速度，在二者之间进行加权，权重系数与 25 | trust相关，而trust与当前所处的一个完整步态周期的百分比有关，源码中设定速度检测的阈值trust_window为0.2,在一个完整的步态周期中，phase为估计 26 | 的着地概率，phase处于0-0.2时，trust为phase/0.2;phase处于0.2-0.8时，trust为1;phase处于0.8-1的时，trust为(1-phase)/0.2。也就是说着地概率 27 | 很小或者很大的时候，trust与这个概率有关，而其余时候都是1。trust越大，也就越相信此时通过腿部估计的速度。(12*1) 28 | pzs：p0为上一时刻滤波得到的最终的位置值，pzs中的每个元素存储一个数值，这个数值是上一时刻滤波得到的最终的位置值中的第三个元素，也就是机体高度与当 29 | 前时刻对应位置的足端在世界坐标系下的位置的第三个元素，也就是足端与机体中心之间在竖直方向上的高度差的差值。如果这个值大于零，意味着当前时刻 30 | 这个足端与机体中心的竖直方向的距离小于上一时刻滤波得到的最终的机体高度值。(4*1) 31 | _xhat = _A * _xhat + _B * a：动态系统的状态空间模型，表示状态的预测。其中_xhat为机器人状态向量(18*1)，前三个元素为机体的位置，之后三个元素为机体的速度， 32 | 最后的十二个元素分为四组。每组为每个足端在初始坐标系下的绝对位置。a为加速度向量(3*1)。 33 | Pm = _A * _P * _A.transpose + Q：协方差矩阵的预测，其中_P为系统的协方差矩阵，为100倍的单位阵;Q为过程噪声矩阵。 34 | ey = y - _C * _xhat：得到偏差。 35 | S = _C * Pm * _C.transpose() + R：R为系统的观测噪声矩阵。 36 | 状态更新以及协方差矩阵更新。 37 | 38 | -------------------------------------------------------------------------------- /Quadruped: -------------------------------------------------------------------------------- 1 | 参数：bodyId：机体-----5 2 | 3 | 4 | 5 | 6 | >>>>>>> template 7 | bool Quadruped::buildModel(FloatingBaseModel& model) 8 | 函数功能：输入参数model为一个FloatingBaseModel类的对象 9 | 创建向量bodyDims (3×1)，存储机体的长_bodyLength、宽_bodyWidth和高_bodyHeight 10 | 调用函数addBase(_bodyInertia)，添加机体相关属性 11 | 调用函数addGroundContactBoxPoints(5, bodyDims) 12 | 13 | >>>>>>> template 14 | void FloatingBaseModel::addBase(const SpatialInertia &inertia) 15 | 函数功能：eye6 单位阵 (6×6) 16 | zero6 零矩阵 (6×6) 17 | SpatialInertia类的对象zeroInertia实例化，zeroInertia->_inertia = zero6 18 | _nDof 赋值为6,表示机体有六个空间自由度 19 | 使用push_back()函数，循环六次 (六个自由度？？？)向定义的九个向量中压入值： 20 | _parents向量：存储父元件，六个元素均为0 21 | _gearRatios向量：存储传动比，前五个元素均为0，最后一个元素为1 22 | _jointTypes向量：存储关节类型，前五个元素均为Nothing，最后一个元素为FloatingBase。 23 | 枚举类型JointType：Prismatic滑动副，Revolute转动副，FloatingBase机体，Nothing不是运动副 24 | _jointAxes向量：存储关节轴线，六个元素均为X。 25 | 枚举类型CoordinateAxis：X，Y，Z 26 | _Xtree向量： 27 | _Ibody向量：前五个元素均为zeroInertia，最后一个元素为inertia (addBase函数输入参数) 28 | _Xrot向量： 29 | _Irot向量： 30 | _bodyNames向量：向量中的前五个元素均为N/A，最后一个元素为FloatingBase 31 | 调用addDynamicsVars(6)函数，完成动力学变量的配置 32 | 33 | >>>>>>> template 34 | void FloatingBaseModel::addDynamicsVars(int count) 35 | 函数功能：配置动力学变量 36 | 37 | >>>>>>> template 38 | void FloatingBaseModel::addGroundContactBoxPoints(int bodyId, const Vec3 &dims) 39 | 函数功能：将机体视作长方体的盒子(box)，此函数完成box八个角点在以机体中心点为原点的坐标系框架内的坐标的计算 40 | 调用addGroundContactPoint()函数 41 | 42 | >>>>>>> template 43 | int FloatingBaseModel::addGroundContactPoint(int bodyID, const Vec3 &location, bool isFoot) 44 | 函数功能： 45 | 46 | >>>>>>> template 47 | Vec3 withLegSigns(const Eigen::MatrixBase& v, int legID) 48 | 函数功能：输入参数v是机械结构相对于机器人机体中心点在x、y和z三个方向上的距离，根据此机械结构所属的腿不同，将其转换为相对于机体中心点为原点的位置坐标输出。 49 | 50 | >>>>>>> template 51 | Mat3 vectorToSkewMat(const Eigen::MatrixBase& v) 52 | 函数功能：输入三维向量v，输出v所对应的反对称矩阵 53 | 参考：https://blog.csdn.net/u012200261/article/details/56494754/ 54 | 55 | >>>>>>> template 56 | auto createSXform(const Eigen::MatrixBase& R,const Eigen::MatrixBase& r) 57 | 函数功能：输出矩阵X (6×6) 58 | X = | R 0 | 59 | | -R*vectorToSkewMat(r) R | 60 | 61 | >>>>>>> template 62 | Mat3 coordinateRotation(CoordinateAxis axis, T theta) 63 | 函数功能：根据枚举类型的输入参数axis(X/Y/Z)以及旋转角theta，输出旋转矩阵R(3×3) 64 | 65 | 66 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MIT-Cheetah-Note 2 | ## MIT Cheetah仿真平台源码笔记以及一部分第三方组件 3 | ### 仓库说明 4 | >在看MIT Cheetah仿真平台的源码的过程中所做的笔记的整理，为了更好地理解源码，因此对应代码中提到的部分参考论文，手动整理了这份笔记，希望与同好交流，笔记中的错误之处欢迎指正。 5 | 6 | ### 文件结构 7 | `ref` 文件夹中存储相关的论文和电子书的pdf格式文件。 8 | `BalanceController` 对应源码中BalanceController相关文件的剖析,主要是调用qpOASES求解器完成对四足机器人GRF的优化求解过程。 9 | `Quadruped` 对应源码中Quadruped相关文件的剖析，主要是构建四足机器人的动力学和运动学模型。 10 | `LegController` 对应源码中LegController相关文件的剖析，主要是四足机器人腿部运动学的参数处理以及正、逆运动学的计算。 11 | `FootSwingTrajectory` 对应源码中FootSwingTrajectory相关文件的剖析，主要是对摆动腿的足端位置、速度和加速度通过三次贝塞尔曲线插值进行规划。 12 | `convexMPC_interface`对应源码中convexMPC_interface相关文件的剖析，主要是为MPC问题的求解和参数赋值提供接口。 13 | `RobotState`对应源码中RobotState相关文件的剖析，主要是提供一个存储机器人状态信息和打印状态信息的对象。 14 | `SolverMPC` 对应源码中SolverMPC相关文件的剖析，主要是构建一个MPC问题并完成求解。 15 | `Gait` 对应源码中Gait相关文件的剖析，主要是对机器人行走步态的分析，涉及到摆动相和支撑相的分析。 16 | `ConvexMPCLocomotion` 对应源码中ConvexMPCLocomotion相关文件的剖析，主要是将步态的控制与MPC控制器完成结合。 17 | `PositionVelocityEstimator` 对应源码中PositionVelocityEstimator相关文件的剖析，主要是构建一个线性卡尔曼滤波器，完成对于机体位置和速度的估计。 18 | `WBC` 目录中存储对源码中所有与WBC相关的文件的剖析。 19 | 20 | ### 其他 21 | 原始项目地址：[https://github.com/mit-biomimetics/Cheetah-Software](https://github.com/mit-biomimetics/Cheetah-Software) 22 | qpOASES求解器地址：[https://github.com/coin-or/qpOASES](https://github.com/coin-or/qpOASES) 23 | 24 | ## Loading...... 25 | -------------------------------------------------------------------------------- /RobotState: -------------------------------------------------------------------------------- 1 | 变量： 2 | p:机器人位置>>>>>>>3*1矩阵 3 | v:机器人速度>>>>>>>3*1矩阵 4 | w:机器人角速度>>>>>>>3*1矩阵 5 | R:旋转矩阵>>>>>>>3*3矩阵 6 | R_yaw:仅考虑偏航情况下的旋转矩阵>>>>>>>3*3矩阵 7 | r_feet:足端位置坐标矩阵>>>>>>>3*4矩阵 8 | I_body:机器人惯性矩阵>>>>>>>3*3矩阵 9 | q:四元数 10 | yaw:偏航角 11 | 12 | 函数： 13 | >>>>>>> void RobotState::set(flt* p_, flt* v_, flt* q_, flt* w_, flt* r_,flt yaw_) 14 | 函数功能：将形参赋值给对应的参数 15 | 16 | >>>>>>> void RobotState::print() 17 | 函数功能：打印参数信息 18 | -------------------------------------------------------------------------------- /SolverMPC: -------------------------------------------------------------------------------- 1 | 参考论文： 2 | [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. 3 | [2] High-slope terrain locomotion for torque-controlled quadruped robots Focchi M., del Prete A., Havoutis I., Featherstone R., Caldwell D.G., Semini C.(2017) Autonomous Robots, 41 (1) , pp. 259-272. 4 | 5 | 变量： 6 | Matrix x_0 >>>>>>> 初始状态向量 7 | Matrix I_world >>>>>>> 世界坐标系下的惯性矩阵 8 | Matrix A_ct >>>>>>> 参考文献[1]中的公式15中的矩阵A 9 | Matrix B_ct_r >>>>>>> 参考文献[1]中的公式15中的矩阵B 10 | Matrix full_weight >>>>>>> 权重向量 11 | Matrix X_d >>>>>>> MPC预测跨度中每个采样点的参考状态(期望值) 12 | 13 | 函数： 14 | >>>>>>> void solve_mpc(update_data_t* update, problem_setup* setup) 15 | 函数功能：求解MPC问题 16 | 根据形参update和setup的成员更新rs的成员 17 | 创建rpy矩阵(3*1)存储机体的横滚角、俯仰角和偏航角 18 | 构建初始状态向量x_0 19 | 根据此笔记参考论文[1]中的公式15计算世界坐标系下的惯性矩阵 20 | 调用ct_ss_mats函数，计算A_ct和B_ct_r 21 | 调用c2qp函数，生成QP问题的矩阵A_qp和B_qp 22 | S功能存疑？？？ 23 | 使用形参update结构体中的traj数组更新参考状态向量X_d 24 | U_b为约束上限 25 | f_block为构建约束不等式中的系数矩阵的元素 26 | fmat为约束不等式中的系数矩阵，可以看作是一个对角阵，对角线上的元素是f_block。这里可以参考参考此笔记参考论文[2]中的公式(8) 27 | 根据此笔记的参考论文[1]中的公式(31)(32)计算矩阵qH和qg 28 | 实例化QpProblem对象jcqp 29 | 根据形参update中的use_jcqp成员的值进行判断： 30 | 如果use_jcqp为1： 31 | jcqp.A = fmat >>>>>>> 20h*12h 32 | jcqp.P = qH >>>>>>> 12h*12h 33 | jcqp.q = qg >>>>>>> 12h*1 34 | jcqp.u = U_b >>>>>>> 20h*1 35 | jcqp.l = 0向量 >>>>>>> 20h*1 36 | 如果use_jcqp不为1： 37 | 调用matrix_to_real函数，分别将： 38 | qH矩阵转换为数组H_qpoases 39 | qg矩阵转换为数组g_qpoases 40 | fmat矩阵转换为数组A_qpoases 41 | U_b矩阵转换为数组ub_qpoases 42 | 数组lb_qpoases全部为0.0 43 | num_constraints为约束不等式的数目，值为20*horizon：预测跨度为horizon，每一步需要对四个足端接触力进行约束，每个力的约束有五项约束 44 | num_variables为状态量的数目，值为12*horizon：预测跨度为horizon，每一步的状态量为12个 45 | new_vars = num_variables，new_cons = num_constraints 46 | 字符数组con_elim为全0数组，数组长度为num_constraints 47 | 字符数组var_elim为全0数组，数组长度为num_variables 48 | 构建循环体，循环次数为num_constraints次： 49 | 对约束不等式的上下界进行遍历检查，如果两个边界都在0附近，意味着这一采样点的这一条腿处于悬空状态，所以GRF为0，则进行接下来的循环体。如果两个边界有一个不在0附近，则直接进行下一次循环 50 | 两个边界值都为零的行所对应的数组起始元素位置地址存储在c_row中 51 | 遍历A_qpoases数组从c_row开始的num_variables个元素，相当于遍历fmat矩阵第i行的所有元素 52 | 调用函数near_one判断遍历的元素是否在1附近，如果在1附近，即定位悬空腿的z方向的力对应的约束矩阵A_qpoases的系数 53 | new_vars中存储着地腿的变量数目(3的倍数) 54 | new_cons中存储着地腿的约束数目(5的倍数) 55 | 将全0且长度为num_variables的数组var_elim中，悬空腿对应的3个元素置1；将全0且长度为num_constraints的数组con_elim中，悬空腿对应的5个元素置1 56 | 构建长度为new_vars的数组var_ind 57 | 构建长度为new_cons的数组con_ind 58 | 构建循环体，循环次数为num_variables次 59 | var_ind存储着地腿的变量序号 60 | 构建循环体，循环次数为num_constraints次 61 | con_ind存储着地腿的约束序号 62 | 构建循环体，循环次数为new_vars次 63 | 数组g_red存储g_qpoases中位于着地腿编号位置的数值 64 | 数组H_red存储H_qpoases中位于着地腿编号位置的数值 65 | 构建循环体，循环次数为new_cons次 66 | 数组A_red存储A_qpoases中位于着地腿编号位置的数值 67 | 构建循环体，循环次数为new_cons次 68 | 数组ub_red存储ub_qpoases中位于着地腿编号位置的数值 69 | 数组lb_red存储lb_qpoases中位于着地腿编号位置的数值 70 | 上面这几个步骤总结一下： 71 | 最初构建的QP问题，包括了着地腿和悬空腿，但是最后只能对着地腿进行规划，因此，根据约束不等式中上界数组ub_qpoases和下界数组lb_qpoases中的元素进行遍历，如果发现在某一位置二者同时很接近0,则认为在这一采样点处，这个方向的力不存在，也就意味着在这一时刻这条腿悬空。接下来根据悬空腿和着地腿的编号，将原先QP问题中的数组中对应的悬空腿的部分剔除，最后只剩下着地腿的部分。 72 | 如果use_jcqp为0： 73 | 构建基于qpOASES求解器的求解方法，参考：https://blog.csdn.net/weixin_40709533/article/details/86064148 74 | 完成求解，在数组q_red中存储原始解 75 | 对原始解进行处理：将悬空腿对应的最终解置0.0,着地腿和悬空腿按照最初顺序存储在q_soln中 76 | 否则(源码注释中给出的此时use_jcqp为2)： 77 | 与use_jcqp为0的情况大同小异，只是采用了另一种方式求解QP问题 78 | 如果use_jcqp为1： 79 | 与use_jcqp为0的情况大同小异，只是采用了另一种方式求解QP问题 80 | 81 | >>>>>>> void ct_ss_mats(Matrix I_world, fpt m, Matrix r_feet, Matrix R_yaw, Matrix& A, Matrix& B, float x_drag) 82 | 函数功能：计算参考文献[1]中的矩阵A和B 83 | 调用函数cross_mat，为计算力矩做准备 84 | x_drag存疑：z方向的机体速度 = x_drag * x方向的机体速度 - 9.8 + z方向的合力/质量 85 | 86 | >>>>>>> inline Matrix cross_mat(Matrix I_inv, Matrix r) 87 | 函数功能：将形参r从向量转换为叉乘矩阵，并与形参I_inv相乘 88 | 调用时，I_inv为机体在世界坐标系下的惯性矩阵，r为足端坐标的叉乘矩阵 89 | 90 | >>>>>>> void c2qp(Matrix Ac, Matrix Bc,fpt dt,s16 horizon) 91 | 函数功能：生成QP问题的矩阵 92 | ABC为25*25的方阵，其中左上角13*13的块存储Ac，右上角13*12的块存储Bc，并与采样时间间隔dt相乘 93 | expmm相当于是求解一个齐次微分方程的解，具体的原理参考：https://blog.csdn.net/HaoZiHuang/article/details/105024959 94 | 拆分expmm矩阵，左上角的13*13的块赋值给Adt，右上角的13*12的块赋值给Bdt 95 | 至此，完成系数矩阵的离散化 96 | 定义包含20个元素的矩阵数组powerMats，其中每个元素是接下来长度为horizon预测跨度中，每个采样点的Adt(相对于初始状态，而不是上一状态) 97 | A_qp中从上到下依次为：Adt、Adt*Adt、Adt*Adt*Adt ......，即每一采样时刻相对于初始状态的状态转移矩阵 98 | B_qp同理为下三角矩阵 99 | 100 | >>>>>>> void matrix_to_real(qpOASES::real_t* dst, Matrix src, s16 rows, s16 cols) 101 | 函数功能：将rows*cols维度的矩阵转换为数组dst，逐行扫描 102 | 103 | >>>>>>> s8 near_zero(fpt a) 104 | 函数功能：判断输入参数a的范围，如果a大于-0.01且小于0.01,返回1；否则返回0 105 | 106 | >>>>>>> s8 near_one(fpt a) 107 | 函数功能：判断输入参数a的范围，如果a大于0.99且小于1.01,返回1；否则返回0 108 | 109 | >>>>>>> mfp* get_q_soln() 110 | 函数功能：返回MPC问题的解向量q_soln 111 | -------------------------------------------------------------------------------- /WBC/BodyOriTask: -------------------------------------------------------------------------------- 1 | 机体角度任务，声明BodyOriTask类，公有继承自Task类 2 | 3 | 4 | >>>>>>> BodyOriTask::BodyOriTask(const FloatingBaseModel* robot) 5 | 函数功能：构造函数，初始化参数，设定任务的维度为3 6 | 设定任务的雅可比矩阵Jt_维度为dim_task_ x dim_config(即3x18) 7 | 将Jt_对应位置的块设为单位阵 8 | 设定任务的JtDotQdot_为维度为dim_task_(3)的向量 9 | 设定任务的_Kp_kin为维度为dim_task_(3)的全1向量(好像没用) 10 | 设定任务的_Kp为维度为dim_task_(3)的全50向量 11 | 设定任务的_Kd为维度为dim_task_(3)的全1向量 12 | 13 | >>>>>>> BodyOriTask::~BodyOriTask() 14 | 函数功能：析构函数，空函数 15 | 16 | >>>>>>> bool BodyOriTask::_UpdateCommand(const void* pos_des, const DVec& vel_des, const DVec& acc_des) 17 | 函数功能：生成加速度指令，对应《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》论文中的公式(22) 18 | ori_cmd = pos_des，为机体的期望角度 19 | link_ori = _robot_sys->_state.bodyOrientation，为机体的当前角度 20 | op_cmd_ = _Kp * ori_err_so3 + _Kd * vel_err + TK::acc_des_ 21 | 22 | >>>>>>> bool BodyOriTask::_UpdateTaskJacobian() 23 | 函数功能：更新任务的雅可比矩阵Jt_ 24 | 这里假设Jt_分为从左至右六部分(因为Jt_的维度为3x18，所以每部分为3x3的方阵) 25 | 更新结束后，Jt_的六个方阵从左至右依次为：R.transpose，0，0，0，0，0 26 | //TODO ？ 27 | 为什么用R.transpose作为第一部分？？？ 28 | 29 | >>>>>>> bool BodyOriTask::_UpdateTaskJDotQdot() 30 | 函数功能：空函数，直接返回TRUE -------------------------------------------------------------------------------- /WBC/BodyPosTask: -------------------------------------------------------------------------------- 1 | 机体位置任务，声明BodyPosTask类，公有继承自Task类 2 | 3 | 4 | >>>>>>> BodyPosTask::BodyPosTask(const FloatingBaseModel* robot) 5 | 函数功能：构造函数，初始化参数，设定任务的维度为3 6 | 设定任务的雅可比矩阵Jt_维度为dim_task_ x dim_config(即3x18) 7 | 将Jt_对应位置的块设为单位阵 8 | 设定任务的JtDotQdot_为维度为dim_task_(3)的向量 9 | 设定任务的_Kp_kin为维度为dim_task_(3)的全1向量(好像没用) 10 | 设定任务的_Kp为维度为dim_task_(3)的全50向量 11 | 设定任务的_Kd为维度为dim_task_(3)的全1向量 12 | 13 | >>>>>>> BodyOriTask::~BodyOriTask() 14 | 函数功能：析构函数，空函数 15 | 16 | >>>>>>> bool BodyPosTask::_UpdateCommand(const void* pos_des, const DVec& vel_des, const DVec& acc_des) 17 | 函数功能：生成加速度指令，对应《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》论文中的公式(22) 18 | pos_cmd = pos_des，为机体的期望位置 19 | link_pos = _robot_sys->_state.bodyPosition，为机体的当前位置 20 | quat = _robot_sys->_state.bodyOrientation，为表示机体旋转的四元数 21 | Rot = ori::quaternionToRotationMatrix(quat)，将四元数转换为旋转向量 22 | op_cmd_ = _Kp * (pos_cmd[i] - link_pos) + _Kd[ * (vel_des_ - curr_vel) + acc_des_ 23 | 24 | >>>>>>> bool BodyPosTask::_UpdateTaskJacobian() 25 | 函数功能：更新任务的雅可比矩阵Jt_ 26 | 这里假设Jt_分为从左至右六部分(因为Jt_的维度为3x18，所以每部分为3x3的方阵) 27 | 更新结束后，Jt_的六个方阵从左至右依次为：0，R.transpose，0，0，0，0 28 | //TODO ？ 29 | 为什么用R.transpose作为第二部分？？？ 30 | 31 | >>>>>>> bool BodyPosTask::_UpdateTaskJDotQdot() 32 | 函数功能：空函数，直接返回TRUE -------------------------------------------------------------------------------- /WBC/BodyPostureTask: -------------------------------------------------------------------------------- 1 | 机体位姿任务，声明BodyPostureTask类，公有继承自Task类 2 | 3 | 内容等价于机体角度任务(BodyOriTask)和机体位置任务(BodyPosTask)的综合 -------------------------------------------------------------------------------- /WBC/ContactSpec: -------------------------------------------------------------------------------- 1 | 接触相关 2 | 3 | 4 | -------------------------------------------------------------------------------- /WBC/KinWBC: -------------------------------------------------------------------------------- 1 | 参考论文：[1]Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control 2 | 3 | 4 | >>>>>>> bool KinWBC::FindConfiguration(const DVec& curr_config , const std::vector*>& task_list, 5 | const std::vector*>& contact_list , DVec& jpos_cmd, 6 | DVec& jvel_cmd) 7 | 函数功能：对应[1]论文中的公式(16)～(24)，计算得到jpos_cmd和jvel_cmd 8 | 形参中的第一项curr_config为19维的向量，传入时索引0-5为0，索引6-8为第一条腿的 9 | 关节角度，索引9-11为第二条腿的关节角度，索引12-14为第三条腿的关节角度，索引15-17 10 | 为第四条腿的关节角度，索引18为0 11 | 12 | 初始化Nc为18 x 18的单位阵，作为contact Jacobian(Jc)的投影矩阵 13 | 14 | 访问足端接触列表的首元素，更新Jc 15 | 调用getContactJacobian获取触地腿足端的Jacobian(3 x 18)作为Jc的初始值 16 | 更新行数num_rows为Jc的行数作为num_rows的初始值 17 | 遍历足端接触列表的剩余元素 18 | 调用getContactJacobian获取触地腿足端的Jacobian(3 x 18)作为Jc_i 19 | num_new_rows为Jc_i的行数 20 | 将Jc_i从尾部接入Jc 21 | 更新num_rows为(num_rows+num_new_rows) 22 | 调用_BuildProjectionMatrix函数计算Jc的投影矩阵Nc，对应[1]论文中的公式(20)中的第一个 23 | 24 | 访问任务列表中的首任务，更新Jt(task Jacobian) 25 | JtPre = Jt * Nc，对应[1]论文中的公式(19)中的第一个 26 | 调用_PseudoInverse求解JtPre的伪逆矩阵JtPre_pinv 27 | delta_q = JtPre_pinv * (task->getPosError())，对应[1]论文中的公式(16)中的i=1的情况 28 | qdot = JtPre_pinv * (task->getDesVel())，对应[1]论文中的公式(17)中的i=1的情况 29 | 调用_BuildProjectionMatrix计算JtPre的投影矩阵，对应[1]论文中的公式(20)中的第二个 30 | N_pre = Nc * N_nx，对应[1]论文中的公式(19)中的第二个 31 | 遍历任务列表的剩余任务 32 | 具体流程同上 33 | 34 | 对于构型空间中的关节元素的位置指令：jpos_cmd = curr_config + delta_q，对应[1]论文中的公式(24) 35 | 对于构型空间中的关节元素的速度指令：jvel_cmd = qdot 36 | 37 | >>>>>>> void KinWBC::_BuildProjectionMatrix(const DMat& J, DMat& N) 38 | 函数功能：求解J的投影矩阵N 39 | 调用_PseudoInverse求解J的伪逆矩阵J_pinv 40 | N = I_mtx(18 x 18) - J_pinv * J得到J的投影矩阵N 41 | 42 | >>>>>>> void KinWBC::_PseudoInverse(const DMat J, DMat& Jinv) 43 | 函数功能：求解J的伪逆矩阵Jinv 44 | 调用pseudoInverse求解J的伪逆矩阵Jinv -------------------------------------------------------------------------------- /WBC/LinkPosTask: -------------------------------------------------------------------------------- 1 | 机体摆动腿足端任务，声明LinkPosTask类，公有继承自Task类 2 | 3 | 数据成员：link_idx_ ------ 足端标号(FR = 9：右前腿 | FL = 11：左前腿 | HR = 13：右后腿 | HL = 15：左后腿) 4 | //TODO ？ 5 | virtual_depend_ ------ 目前还不知道是啥意思？？？ 6 | 7 | >>>>>>> LinkPosTask::LinkPosTask(const FloatingBaseModel* robot, int link_idx, bool virtual_depend) 8 | 函数功能：构造函数，初始化参数，设定任务的维度为3 9 | 设定任务的雅可比矩阵Jt_维度为dim_task_ x dim_config(即3x18) 10 | 设定任务的JtDotQdot_为维度为dim_task_(3)的向量 11 | 设定任务的_Kp_kin为维度为dim_task_(3)的全1向量(好像没用) 12 | 设定任务的_Kp为维度为dim_task_(3)的全100向量 13 | 设定任务的_Kd为维度为dim_task_(3)的全5向量 14 | 15 | >>>>>>> LinkPosTask::~LinkPosTask() 16 | 函数功能：析构函数，空函数 17 | 18 | >>>>>>> LinkPosTask::_UpdateCommand(const void* pos_des, const DVec& vel_des, const DVec& acc_des) 19 | 函数功能：生成加速度指令，对应《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》论文中的公式(22) 20 | pos_cmd = pos_des，为机体摆动腿足端的期望位置 21 | 调用浮动基座模型的接口获取摆动腿足端的当前位置和当前速度 22 | op_cmd_ = _Kp * pos_err_ + _Kd * (vel_des_ - robot_sys_->_vGC) + acc_des_ 23 | 24 | >>>>>>> LinkPosTask::_UpdateTaskJacobian() 25 | 函数功能：更新任务的雅可比矩阵Jt_ 26 | 调用浮动基座模型的接口获取摆动腿足端的雅可比矩阵 27 | 28 | >>>>>>> LinkPosTask::_UpdateTaskJDotQdot() 29 | 函数功能：更新任务的JDotQdot 30 | 调用浮动基座模型的接口获取摆动腿足端的JDotQdot 31 | 用于《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》论文中的公式(18) -------------------------------------------------------------------------------- /WBC/LocomotionCtrl: -------------------------------------------------------------------------------- 1 | 参考论文：[1]Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control 2 | 3 | 4 | >>>>>>> void LocomotionCtrl::_ContactTaskUpdate(void* input, ControlFSMData & data) 5 | 函数功能：更新任务以及接触相关 6 | 调用_ParameterSetup函数完成所有Kp、Kd参数的初始化 7 | 调用_CleanUp完成接触列表与任务列表的清空 8 | _quat_des为机体期望姿态的四元数表示 9 | 调用_body_ori_task->UpdateTask函数激活机体姿态任务 10 | 调用_body_pos_task->UpdateTask函数激活机体位置任务 11 | 将激活完成后的机体姿态任务和机体位置任务按顺序装载进任务列表 12 | 遍历四条腿 13 | 如果当前腿为支撑腿 14 | 调用_foot_contact[leg]->setRFDesired函数完成GRF的初始化 15 | 调用_foot_contact[leg]->UpdateContactSpec函数激活接触相关的参数 16 | 否则，即如果当前腿为摆动腿 17 | 调用_foot_task[leg]->UpdateTask函数激活摆动腿足端任务 18 | 将激活完成后的摆动腿足端任务装载进任务列表 19 | 20 | >>>>>>> void LocomotionCtrl::_ParameterSetup(const MIT_UserParameters* param) 21 | 函数功能：访问形参param，完成所有Kp、Kd参数的初始化 22 | 包括机体位置任务中的Kp、Kd参数、 23 | 机体姿态任务中的Kp、Kd参数、 24 | 摆动腿足端任务中的Kp、Kd参数、 25 | 所有关节的Kp、Kd参数 26 | 27 | >>>>>>> void LocomotionCtrl::_CleanUp() 28 | 函数功能：清空接触列表与任务列表 29 | 30 | >>>>>>> void LocomotionCtrl::_LCM_PublishData() 31 | 函数功能：依赖LCM通信库，完成WBC指令的传递 32 | 遍历四条腿 33 | 清空对应的的_Fr_result(3 x 1)，这里的_Fr_result对应论文[1]中公式(25)中的fr 34 | 如果当前腿为支撑腿，将对应的_Fr_result赋值为_Fr 35 | 如果当前腿为支撑腿，将对应的触地状态contact_est赋值为1 36 | 如果当前腿为摆动腿，将对应的触地状态contact_est赋值为0 37 | 遍历三个方向[X->Y->Z] 38 | 赋值对应的足端在对应方向上的位置和速度 39 | 遍历四条腿 40 | 赋值Fr_des，这里的Fr_des对应论文[1]中公式(25)中的frMPC 41 | 赋值Fr，这里的Fr_des对应论文[1]中公式(25)中的fr 42 | 赋值足端位置指令、足端速度指令和足端加速度指令 43 | 赋值关节位置指令、速度指令 44 | 赋值关节位置、速度 45 | 赋值机体期望位置、期望速度、期望姿态 46 | 赋值机体位置、速度、姿态、角速度 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /WBC/SingleContact: -------------------------------------------------------------------------------- 1 | >>>>>>> SingleContact::SingleContact(const FloatingBaseModel* robot, int pt) 2 | 函数功能：摩擦锥约束块的实现 3 | 设定摩擦系数mu为0.4 4 | Uf_为6 x 3的摩擦锥约束块 5 | 6 | >>>>>>> bool SingleContact::_UpdateInequalityVector() 7 | 函數功能：摩擦锥约束块的边界的实现 8 | ieq_vec_为6 x 1的向量，表示摩擦锥约束块的边界 9 | ieq_vec_的第六个元素为-_max_Fz，即-1500 -------------------------------------------------------------------------------- /WBC/WBC: -------------------------------------------------------------------------------- 1 | 声明WBC类，作为父类 2 | 3 | 4 | 数据成员：num_act_joint_ ------ 被驱动的关节数目 5 | num_qdot_ ------ 总自由度 6 | Sa_ ------ 动态矩阵，表示被驱动的关节与总自由度之间的关系，即行数为num_act_joint_，列数为num_qdot_ 7 | Sv_ ------ 动态矩阵，表示被机体的自由度与总自由度之间的关系，即行数为6，列数为num_qdot_ 8 | A_ ------ 拉格朗日动力学中的惯性部分 9 | Ainv_ ------ 拉格朗日动力学中的惯性部分的逆 10 | cori_ ------ 拉格朗日动力学中的科氏力部分 11 | grav_ ------ 拉格朗日动力学中的重力部分 12 | b_updatesetting_ ------ 拉格朗日动力学参数更新完成的标志位(更新完成置1) 13 | b_internal_constraint_ 14 | 15 | 16 | >>>>>>> WBC::WBC(size_t num_qdot) 17 | 函数功能：构造函数，初始化参数 18 | num_qdot：机器人的自由度(机体自由度(6) + 关节自由度(12)) 19 | 20 | 通过初始化列表完成数据成员的初始化 21 | num_act_joint_ = num_qdot - 6 ------ 关节自由度数目 22 | num_qdot_ = num_qdot ------ 总自由度 23 | 24 | 初始化Sa_和Sv_ 25 | 26 | >>>>>>> virtual ~WBC() {} 27 | 函数功能：析构函数，空函数 28 | 29 | >>>>>>> virtual void UpdateSetting(const DMat& A, const DMat& Ainv, const DVec& cori, const DVec& grav, void* extra_setting = NULL) = 0 30 | 函数功能：更新拉格朗日动力学参数 31 | 32 | >>>>>>> virtual void MakeTorque(DVec& cmd, void* extra_input = NULL) = 0 33 | 函数功能：计算扭矩？ 34 | 35 | >>>>>>> void _WeightedInverse(const DMat& J, const DMat& Winv, DMat& Jinv, double threshold = 0.0001) 36 | 函数功能：目前还不知道 37 | -------------------------------------------------------------------------------- /WBC/WBC_Ctrl: -------------------------------------------------------------------------------- 1 | >>>>>>> void WBC_Ctrl::run(void* input, ControlFSMData & data) 2 | 函数功能：WBC功能模块的最顶层入口 3 | 被调用时的形参传递为： 4 | input为LocomotionCtrlData类型，包含机体的位置期望、速度期望、加速度期望、姿态期望、 5 | 足端位置期望、足端速度期望、足端加速度期望、GRF、足端触地状态 6 | 调用_UpdateModel函数完成模型参数的更新 7 | 调用_ContactTaskUpdate函数完成任务以及接触相关的更新 8 | 调用_ComputeWBC完成WBC中的所有计算 9 | 调用_UpdateLegCMD函数完成腿部指令的更新 10 | 调用_LCM_PublishData函数完成指令的传递 11 | 12 | >>>>>>> void WBC_Ctrl::_UpdateModel(const StateEstimate & state_est, 13 | const LegControllerData * leg_data) 14 | 函数功能：更新WBC计算过程中使用到的模型参数 15 | 更新机体姿态信息、机体位置信息、机体速度、机体角速度、关节角度、关节角速度 16 | 更新拉格朗日动力学中的惯性项、重力项、科氏力项以及惯性项的逆 17 | 18 | >>>>>>> void WBC_Ctrl::_ComputeWBC() 19 | 函数功能：完成WBC中的所有的计算 20 | 调用_kin_wbc->FindConfiguration完成对机器人构型空间中的项的操作 21 | 调用_wbic->UpdateSetting更新机体动力学参数 22 | 调用_wbic->MakeTorque完成WBC中的所有计算过程，最终得到前馈扭矩 23 | 24 | >>>>>>> void WBC_Ctrl::_UpdateLegCMD(ControlFSMData & data) 25 | 函数功能：更新腿部指令 26 | 遍历四条腿 27 | 将对应腿部指令的结构体清空 28 | 遍历关节[肩关节->髋关节->膝关节] 29 | 赋值前馈扭矩 30 | 赋值期望角度 31 | 赋值期望角速度 32 | 赋值关节的Kp和Kd参数 33 | 遍历四条腿 34 | 对膝关节的期望角度限幅 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /WBC/WBIC: -------------------------------------------------------------------------------- 1 | 参考论文：[1]Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control 2 | 3 | 声明WBIC_ExtraData类 4 | 5 | 声明WBIC类，公有继承自WBC类 6 | 7 | 数据成员：_dim_floating ------ 浮动基座的自由度 8 | _eye ------ 动态矩阵，单位阵 9 | _eye_floating ------ 动态矩阵，单位阵 10 | 11 | 12 | >>>>>>> WBIC::WBIC(size_t num_qdot, const std::vector*>* contact_list, const std::vector*>* task_list) 13 | 函数功能：构造函数，初始化参数 14 | _dim_floating初始化为6 15 | _eye初始化为num_qdot x num_qdot的单位阵 16 | _eye_floating初始化为_dim_floating x _dim_floating(即6x6)的单位阵 17 | 18 | >>>>>>> virtual ~WBIC() 19 | 函数功能：析构函数，空函数 20 | 21 | >>>>>>> virtual void UpdateSetting(const DMat& A, const DMat& Ainv, const DVec& cori, const DVec& grav, void* extra_setting = NULL) 22 | 函数功能：更新拉格朗日动力学参数 23 | 24 | >>>>>>> void WBIC::MakeTorque(DVec& cmd, void* extra_input) 25 | 函数功能：调用_SetOptimizationSize设置优化问题的问题规模 26 | 调用_SetCost设置优化问题的目标函数 27 | 判断rf的维度，如果大于零，即有支撑腿 28 | 调用_ContactBuilding完成接触Jacobian、摩擦锥等的初始化 29 | 调用_SetInEqualityConstraint完成摩擦锥约束的构建 30 | 对应论文[1]中的公式(23)，计算得到_Jc的伪逆JcBar 31 | 对应论文[1]中的公式(21)的第二个，计算得到qddot_pre 32 | 对应论文[1]中的公式(20)的第一个，计算得到Npre 33 | 否则，即没有支撑腿 34 | qddot_pre初始化为维度为18的全零向量 35 | Npre初始化18 x 18的单位阵 36 | 37 | 遍历任务列表 38 | 获取当前任务的Task Jacobian -> Jt 39 | 获取当前任务的JtDotQdot，用在论文[1]中的公式(18) 40 | 获取当前任务的加速度指令，就是论文[1]中的公式(22)的计算结果 41 | 对应论文[1]中的公式(19)中的第一个，计算得到JtPre 42 | 对应论文[1]中的公式(23)，计算得到JtPre的伪逆JtBar 43 | 对应论文[1]中的公式(18)，更新qddot_pre 44 | 对应论文[1]中的公式(19)中的第二个，更新Npre 45 | 46 | 调用_SetEqualityConstraint完成等式约束的构建 47 | 调用solve_quadprog求解优化问题，求解的结果为z 48 | 对应论文[1]中的公式(25)的第二组约束，更新qddot_pre 49 | 调用_GetSolution，获取所有关节的扭矩 50 | 将优化求解器的计算结果存储到_data->_opt_result 51 | 52 | >>>>>>> void WBIC::_SetOptimizationSize() 53 | 函数功能：设置优化问题的规模 54 | _dim_rf初始化为0 55 | _dim_Uf初始化为0 56 | _dim_rf表示支撑的维度，大小是(3 * 支撑腿的数目) 57 | _dim_Uf表示总摩擦锥的维度，大小是(6 * 支撑腿的数目) 58 | _dim_opt表示优化问题的整体维度(6 + 3 * 支撑腿的数目) 59 | _dim_eq_cstr应该表示等式约束的维度，大小是6 60 | 61 | 以下为对优化求解器所需变量的设置： 62 | 矩阵G初始化为_dim_opt x _dim_opt的全零矩阵 63 | 向量g0初始化为长度为_dim_opt的全零向量 64 | 矩阵CE初始化为_dim_opt x _dim_eq_cstr的全零矩阵 65 | 向量ce0初始化为长度为_dim_eq_cstr的全零向量 66 | 矩阵_dyn_CE初始化为_dim_eq_cstr x _dim_opt的全零矩阵 67 | 向量_dyn_ce0初始化为维度为_dim_eq_cstr的向量 68 | 69 | 判断_dim_rf的值，如果大于零，即有支撑腿 70 | 矩阵CI初始化为_dim_opt x _dim_Uf的全零矩阵 71 | 向量ci0初始化为维度为_dim_Uf的全零向量 72 | 矩阵_dyn_CI初始化为_dim_Uf x _dim_opt的全零矩阵 73 | 向量_dyn_ci0初始化为维度为_dim_Uf的向量 74 | 矩阵_Jc初始化为_dim_rf x 18的矩阵 75 | 向量_JcDotQdot初始化为维度为_dim_rf的向量 76 | 向量_Fr_des初始化为维度为_dim_rf的向量，承接MPC的计算结果 77 | 矩阵_Uf初始化为_dim_Uf x _dim_rf的全零矩阵，表示总摩擦锥约束 78 | 向量_Uf_ieq_vec初始化为维度为_dim_Uf的向量，表示摩擦锥约束的边界 79 | 否则，即没有支撑腿 80 | 矩阵CI初始化为_dim_opt x 18的矩阵 81 | 向量ci0初始化为0的单元素向量 82 | 83 | >>>>>>> void WBIC::_SetCost() 84 | 函数功能：设置优化问题的目标函数 85 | idx_offset初始化为0 86 | 矩阵G的左上角6 x 6的块为元素全为0.1的对角阵 87 | 矩阵G的右下角(3 * contactNum) x (3 * contactNum)的块为元素全为1.0的对角阵 88 | 89 | >>>>>>> void WBIC::_ContactBuilding() 90 | 函数功能：完成接触Jacobian、摩擦锥等的初始化 91 | 感觉这里的操作与FindConfiguration函数中的操作有重复 92 | 93 | 访问足端接触列表的首元素，更新Jc，这里的Jc应该是对应任务的Jc，维度是3 x 18 94 | 访问足端接触列表的首元素，更新JcDotQdot 95 | 访问足端接触列表的首元素，更新Uf，摩擦锥约束块，维度是6 x 3 96 | 访问足端接触列表的首元素，更新Uf_ieq_vec，摩擦锥约束的边界，维度是6 x 1 97 | 访问足端接触列表的首元素，调用getDim函数，将接触力的维度(3)赋值给dim_accumul_rf 98 | 访问足端接触列表的首元素，调用getDimRFConstraint函数，将摩擦锥约束块的维度(6)赋值给dim_accumul_uf 99 | 更新_Jc，从索引(0,0)开始的3行18列的块设置为Jc 100 | 更新_JcDotQdot，前3个元素设置为JcDotQdot 101 | 更新_Uf，这个应该是总摩擦锥约束，从索引(0,0)开始的6行3列的块设置为Uf 102 | 更新_Uf_ieq_vec，这个应该是总摩擦锥约束边界，前6个元素设置为Uf_ieq_vec 103 | 更新_Fr_des，承接MPC的计算结果 104 | 105 | 遍历其余的足端接触列表中的元素 106 | 具体流程同上 107 | 108 | >>>>>>> void WBIC::_SetInEqualityConstraint() 109 | 函数功能：设定不等式约束，满足求解器要求的格式 110 | 构建摩擦锥约束，也是整体优化问题中唯一的一组不等式约束 111 | //TODO ？ 112 | 这里_dyn_ci0 = _Uf_ieq_vec - _Uf * _Fr_des是不是有问题 113 | 对照论文[1]中的公式(25)中的第四组约束，这里为什么最后乘的是MPC计算得到的fr_MPC而不是fr 114 | 115 | >>>>>>> void WBIC::_SetEqualityConstraint(const DVec& qddot) 116 | 函数功能：设定等式约束，满足求解器要求的格式 117 | 对应论文[1]中的公式(25)中的第一组约束 118 | 这里Sv_的维度是6 x 18 119 | Sv_从索引(0,0)开始的6 x 6的块为单位阵，其余元素都是0 120 | Sv_的意义是一个选择矩阵，选中了机体对应的项 121 | 对应原文中的"floating base selection matrix" 122 | 123 | >>>>>>> void WBIC::_GetSolution(const DVec& qddot, DVec& cmd) 124 | 函数功能：得到扭矩指令 125 | 判断rf的维度，如果大于零，即有支撑腿 126 | 对应论文[1]中的公式(25)的第三组约束，更新_Fr 127 | 对应论文[1]中的公式(26)计算得到整体扭矩向量tot_tau 128 | 否则，即没有支撑腿 129 | 对应论文[1]中的公式(26)中没有最后一项的情况，计算得到整体扭矩向量tot_tau 130 | 将按引用传递的第一个形参qddot赋值为对应论文[1]中的公式(25)的第二组约束更新后的qddot_pre，在MakeTorque函数中完成 131 | 将按引用传递的第二个形参cmd赋值为整体扭矩向量tot_tau中的后12个元素，对应所有关节的扭矩 132 | 133 | -------------------------------------------------------------------------------- /convexMPC_interface: -------------------------------------------------------------------------------- 1 | 变量： 2 | K_NUM_LEGS = 4 : 机器人腿数目 3 | K_MAX_GAIT_SEGMENTS = 36 : 一个步态周期被分为36段 4 | 5 | 函数： 6 | >>>>>>> void initialize_mpc() 7 | 函数功能：以默认属性创建互斥锁problem_cfg_mt和update_mt，前者在设置问题时上锁，后者在更新问题数值时上锁 8 | 9 | >>>>>>> void setup_problem(double dt, int horizon, double mu, double f_max) 10 | 函数功能：将形参赋值给problem_configuration结构体中对应的成员 11 | dt:采样时间间隔 12 | horizon:MPC控制器的预测长度 13 | mu:摩擦系数 14 | f_max:力的极限值 15 | 调用resize_qp_mats函数，根据horizon的值，更新QP问题中矩阵的维度 16 | 17 | >>>>>>> inline void mfp_to_flt(flt* dst, mfp* src, s32 n_items) 18 | 函数功能：内联函数，通过指针完成赋值（数组赋值） 19 | 20 | >>>>>>> inline void mint_to_u8(u8* dst, mint* src, s32 n_items) 21 | 函数功能：内联函数，通过指针完成赋值（数组赋值） 22 | 23 | >>>>>>> void update_problem_data(double* p, double* v, double* q, double* w, double* r, double yaw, double* weights, double* state_trajectory, double alpha, int* gait) 24 | 函数功能：更新问题 25 | 调用函数solve_mpc完成求解 26 | 27 | >>>>>>> double get_solution(int index) 28 | 函数功能：获取解向量中的第index个值 29 | -------------------------------------------------------------------------------- /ref/Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control.pdf -------------------------------------------------------------------------------- /ref/High-slope Terrain Locomotion for Torque-Controlled Quadruped.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/High-slope Terrain Locomotion for Torque-Controlled Quadruped.pdf -------------------------------------------------------------------------------- /ref/MIT Cheetah 3 Design and Control of a Robust, Dynamic Quadruped Robot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/MIT Cheetah 3 Design and Control of a Robust, Dynamic Quadruped Robot.pdf -------------------------------------------------------------------------------- /ref/Probabilistic Contact Estimation and Impact Detection for State Estimation of Quadruped Robots.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/Probabilistic Contact Estimation and Impact Detection for State Estimation of Quadruped Robots.pdf -------------------------------------------------------------------------------- /ref/Rigid Body Dynamics Algorithms.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/Rigid Body Dynamics Algorithms.pdf -------------------------------------------------------------------------------- /ref/highly dynamic quadruped locomotion via whole-body impulse control and model predictive control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Technician13/MIT-Cheetah-Note/6189e00f025f6ad881f1c0a2a9d7ea91116c40b0/ref/highly dynamic quadruped locomotion via whole-body impulse control and model predictive control.pdf --------------------------------------------------------------------------------