Issue about legged control failure on real Unitree GO1 (with load)

[AgIONX]

Hi! Thanks for the awesome works!

I have tried the quad_sdk in gazebo, and it works well for the primitive robot carrying nothing.
But when I used it with the same parameters (in nmpc_controller/scripts/main.m) on a real robot with additional sensors (which cause the imbalance of mass distribution and the center of gravity may being too forward), the robots tend to pitch down and fall.

So I think I should change the parameters in nmpc_controller/scripts/main.m or quad_utils/config/robot_name.yaml, especially the parameter.physics.inertia_body. But I don't have an exact introduction to such parameters modifications.

I want to know how to solve the problem through parameter modifications (of nmpc_controller or others), Thanks!

[elpimous]

Hi friend. @AgIONX
You re using sensors (foot)?

If you re using Noetic, would you mind sharing with me your code ?

I dont have success on my real robot.
What nodes do you launch for real ? Do you use t265 ?
Have a Nice day
Vincent

[AgIONX]

Hi, @elpimous
I'm using lidar & depth camera(no t265 and no for localization), and foot sensor havent been used.

The robot used is Unitree Go1, and I using Noetic on pc (connect the robot(Melodic) with wifi).

The estimator just read the imu_data in the low_state_msg from unitree_sdk with hardware_interface, I think you can use the t265 or IMU to gain the similar imu_data or pose/position_msg (with calibration to fix joint for legs).
I think the key is how to modify the hardware_interfaces to connect your localization or sensors msgs with the quad_msg usd in estimator.

The code used is quad_sdk-go1 and here for another version. I used the second version, and only launch robot_driver & local_planner for real robot.

I hope my answer can help you!

[elpimous]

Hey, thanks a lot for your answer 👍

[selvingarciag96]

Hi, @AgIONX. I am currently working on an implementation to solve a similar issue (very likely this as well).
Did you have any success or way to fix it?

Otherwise I'll keep you post it with a possible solution soon.

Thanks!

[ologandavid]

Hi @AgIONX,

If you're trying to solve the problem through parameter modification, I might also suggest updating your robot parameters in quad_utils/config/robot_name.yaml. Updating files in the NMPC is probably required to handle the change in the mass distribution of the robot.

Also, a good reference for modifying robot specific parameters can be found here: https://github.com/robomechanics/quad-sdk/wiki/Tutorial:-Adding-a-New-Type-of-Robot-to-Quad-SDK.

If you have any other questions, feel free to drop them here!

Best,
David Ologan

[AgIONX]

Hi, @AgIONX. I am currently working on an implementation to solve a similar issue (very likely this as well). Did you have any success or way to fix it?

Otherwise I'll keep you post it with a possible solution soon.

Thanks!

Hi, @selvingarciag96,
Thanks for comment! Sorry, I haven't been able to solve this issue yet. But I discovered a potential issue by modifying the interface file and monitoring the status feedback of the quad_sdk and Unitree_sdk. I think the issue may lie in the actual robot's control of motor commands.
the actual motors of Unitree_Go1 do not always perfectly execute the motor position commands published by quad_sdk, resulting in a non-constant position(angular) deviation. I adjusted the parameters of kp and kd in motor commands, but it didn't works. It seems that the pd control is not working on the actual motors of the robot.
Now the robot can execute stand commands with deviations, but once we pub twist commands, the foot control of the robot gradually diverges, leading to loss of control and fall. The offset setting in go1_interface.cpp is:

std::map<int, float> offset_q2u = { // Compensation offset  
     {FL_0, 0},  
     {FL_1, +MATH_PI/2},  
     {FL_2, -MATH_PI},  
     {FR_0, 0},  
     {FR_1, +MATH_PI/2},  
     {FR_2, -MATH_PI},  
     {RL_0, 0},  
     {RL_1, +MATH_PI/2},  
     {RL_2, -MATH_PI},  
     {RR_0, 0},  
     {RR_1, +MATH_PI/2},  
     {RR_2, -MATH_PI},  
};    
std::map<int, int> sign_q2u = { // Change sign for motion  
     {FL_0, 1},  
     {FL_1, -1},  
     {FL_2, 1},  
     {FR_0, 1},  
     {FR_1, -1},  
     {FR_2, 1},  
     {RL_0, 1},  
     {RL_1, -1},  
     {RL_2, 1},  
     {RR_0, 1},  
     {RR_1, -1},  
     {RR_2, 1},  
};  
int quad_to_unitree[4][3] = {  
  {FL_0, FL_1, FL_2},  
  {RL_0, RL_1, RL_2},  
  {FR_0, FR_1, FR_2},  
  {RR_0, RR_1, RR_2},  
};  

I think it may be necessary to set up additional closed-loop control between the actual feedback of the robot and the motor commands of quad_sdk, or dynamically adjust the offset.
I hope this information can be helpful to you. Please feel free to update me if you make any progress!

Thanks!

[AgIONX]

Hi @AgIONX,

If you're trying to solve the problem through parameter modification, I might also suggest updating your robot parameters in quad_utils/config/robot_name.yaml. Updating files in the NMPC is probably required to handle the change in the mass distribution of the robot.

Also, a good reference for modifying robot specific parameters can be found here: https://github.com/robomechanics/quad-sdk/wiki/Tutorial:-Adding-a-New-Type-of-Robot-to-Quad-SDK.

If you have any other questions, feel free to drop them here!

Best, David Ologan

Hi @ologandavid,
Thanks for your attention to this issue!
I have tried modifying the interface file and some robot parameters in robot_name.yaml, and I have found that there is a non-constant position deviation between the real motor state and the motor commands of quad_sdk in standing state. After launching local_planner, the controller can compensate for this deviation in gazebo simulation, but in the actual robot, the kp and kd parameters seems fail to function in the motor control, leading to robot fall.
It seems possible to fix the issue by eliminating the deviation in the robot_interface. I will change the issue title to "Issue about legged control failure on real Unitree GO1 (with load)".
Thanks!