Major problems in robot param & MPC control

[chalkchalk]

in gazebo_a1_mpc.yaml the a1_trunk_inertia is set to a1_trunk_inertia_xx: 0.0158533 a1_trunk_inertia_xy: 0.0 a1_trunk_inertia_xz: 0.0 a1_trunk_inertia_yz: 0.0 a1_trunk_inertia_yy: 0.0377999 a1_trunk_inertia_zz: 0.0456542 which is obviously copied from the urdf file of a1 robot description. However, such inertia param is only the trunk param. According to the urdf robot description, the main inertia of the robot is composed by the mass of leg motors, which contribute to more than 50% percent of the robot mass, and locate at farther postions from the robot COM. Therefore, the inertia param above needs carefully recalculating if the single-rigid-body model is used. I found that if they are multipled by 10 or 20, the angular prediction in MPC will work much better. (I havn't fine-tuned them)

============== Besides, the prediction horizon of MPC also has major problems. In file A1RobotControl.cpp, the mpc_dt is set to 2.5 ms, and the prediction step is set to 10. Therefore the prediction horizon is only 25 ms, which is way smaller than the description given by the original paper, and is unable to make resonable predctions.

The proposed controller was implemented on the MIT Cheetah 3 Robot [26] with a horizon length of one gait cycle (0.33 to 0.5 seconds), subdivided into between 10 and 16 timesteps, depending on the gait selected.

In fact, the time interval for mpc prediction does not have to be idential to the program update interval. In my practice, it can be set to 0.05 sec, which results in a 0.5 second prediction horizon. Such horizon allows better prediction performance and reduces noises.

I also notice that the A and B matrix in MPC control is not predicted but constant during every program update (constant root_rot_mat). The upper and lower bound of mpc is also constant instead of predicting future contacts. Maybe it will improve the performance of the MPC controller if such prediction is taken into account, just as in the original paper.

Thanks again for your contribution to the community. It helps me a lot. : )

[ShuoYangRobotics]

1. in convex MPC, an astonishing fact is - the actual value of inertia does not matter because when you tune the Q and R matrices, they implicitly account for the scale of the inertia.
2. the short horizon is due to a rudimentary usage of osqp. I initialize osqp solver within each control loop so it causes additional memory allocations which are not necessary. In our new implementation (new code), the problem is solved by a better osqp initialization logic. So we can use 0.05s as di and solve for 30 steps within 5ms.
3. I tried using the predicted contact sequence in the new implementation as well. No visble difference. The robot can run faster indeed in simulation. But I do not think about how to quantify how much improvement this can bring too the system.