Dear legged robotics group,
thanks for providing the OCS2.
Recently, we are going to use OCS2 to generate an online walking trajectory for Anymal C and simulate it in the RaiSim. We have developed our WBC to track the reference trajectory. At the very beginning, we employed the MPC solver of OCS2 only as a pure trajectory generator (meaning there was no robot's state feedback obtained from RaiSim into the MPC). But then, we realized that there was a drift in the z-direction, as mentioned in #24. So we tried to find the solution.
During figuring out a solution, we found that #23 mentioned that we could add some feedback terms to the MPC. Moreover, paper "A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation" also mentioned that we could obtain the state information from the robot and insert it into the SLQ-MPC solver, as shown in figure 3. Therefore, we tried to obtain the state from RaiSim and insert it into the MPC solver.
We've found that there is a user-defined modification function interface in file "ocs2_ros_interface/src/mrt/MRT_ROS_Dummy_Loop.cpp". In my opinion, the "observation_" term should be the real robot state that is the input for the MPC. We replaced the "observation_" with the state and input vector that gathered from the anymal in RaiSim. The RaiSim and WBC were ruining at 1kHz, mrtDesiredFrequency was 1khz, and mpcDesiredFrequency was -1 Hz (which was 250Hz in my pc). We tried replacing the "observation_" with measured data in 1kHz. It turned out that even though I set the target position, the robot was standing still on the ground without any motion at first, then it would drift, as shown below:
![final_61b9f9f92733e70169c019ff_12003](https://user-images.githubusercontent.com/39060488/146204605-579c3700-99e0-4d65-a65d-208bd4cf9a0b.gif)
I thought maybe we were updating the "observation_" too frequently, so I set the updating frequency to 1Hz. But in this way, the robot in rviz (on the left hand side) will “flash” from one place to the other, which will again make the robot in RaiSim unstable, as shown here:
![final_61b9fb2e64bdc400c8ab2751_866206](https://user-images.githubusercontent.com/39060488/146205411-50891355-79cd-43e6-b0ab-6a9e4f054dba.gif)
Of course, we have tried different mrtDesiredFrequency, mpcDesiredFrequency and "observation_" updating frequency, but the robot was still unstable. We started to consider that maybe the way we used the measured data for the MPC solver was not correct, because by doing our way, the desired state and input vector seems not continuous, which will, in turn, cause the unstable tracking performance. And also, if we update the "observation_" too frequently, then the generated reference trajectory is too close to the current robot state after each time's MPC update, which won't lead the robot to move. So I was wondering:
- How to use the robot's measured data for the MPC solver properly?
- How should we solve the drift problem in z-direction if we want to use the MPC as a pure trajectory planner (without state feedback)?
Many thanks
Best
Shengzhi