The desired body trajectory of legged robot example is floating upwards

[edward9503]

Hi,

thanks for the contribution of ocs2.

Recently I was running the legged robot example provided in the ocs2 directory. A green desired body trajectory will be generated when setting the desired position and yaw angle. However, after moving the robot forward and backward several times, this green trajectory started to float upwards, although the robot motion seemed still correct and logical. I was wondering is that a bug? Or perhaps is it an issue with some dependency versions (like eigen3)? I am using Ubuntu 20.04, eigen3 3.3.7, and I can catkin build the whole ocs2 project without any problems.

Here are some pictures and videos: Pic:

Video: https://user-images.githubusercontent.com/39060488/140451959-137e472c-dd27-4414-b1a7-a7e289d618c3.mp4

Thank you in advance.

Best

[rubengrandia]

Hi,

Thank you for reporting. In the example you are running here, the "physics" of the simulation are identical to the MPC model. In this model we enforce the contacts with a zero velocity constraint. As you can imagine, such a constraint will cause drift over longer integration times. The issue that you show here is that the command is still published in world frame, but the notion of where the ground is has drifted in z-direction.

In an extended formulation we are adding a proper notion of where the terrain is in world frame and will integrate it in the optimization, simulation, and command side.

[edward9503]

Thanks a lot!