Interfacing OCS2 with CoppeliaSim and other physical engine based simulations

[mszuyx]

While the provided dummy tests are very useful for studying mpc policy behaviors and weight tuning, they aren't rigorous validations since the generated states are based on the same dynamic models. Currently, we are trying to interface OCS2 with CoppeliaSim which so happens to simulate our robot behaviors well. The naive thought was to simply replace dummy_test with CoppeliaSim's ROS interface (this node will publish /mpc_observation and subscribe to /mpc_policy). We soon realized that by excluding dummy_test node, we also disable MRT ROS interface, which supposes to run along side with MPC (handling reset and timing control). Therefore, we wonder:

• Is MRT interface absolutely necessary or it can be replaced if a third party robot simulator handles the time control? (MPC computes traj based on simulation time)
• How do we enable state-based and feedback policy setting for the MPC solver and MRT? What are the pros and cons compared to time-based?
• Is it possible to provide a DummyXXbotNode.cpp or a MRT_ROS_Interface.cpp temple that allows people to take state observations from other ROS node (i.e., CoppeliaSim) while still having MRT to regulate the timing?

(Sorry, just started using OCS2, I hope the questions make sense. )

[mszuyx]

Dear team OCS2,

We have some news for you.

We found that by hacking MRT_ROS_Interface.cpp, and MRT_ROS_Dummy_Loop.cpp, we can "hijack" the mpc_observation and mpc_policy signals to establish the OCS2-CoppeliaSim interface:

https://www.youtube.com/watch?v=sWUfsK1tYkA

Here is the rqt_graph for this interface:

In this demo, we are able to complete the swing-up task which is not so easily achievable by a linear controller. It shows that at least the MPC is not complaining about this pseudo MRT interface and actually performs the task.
However, in this simulation demo, we hard coded the dynamic equations of the cart-pole model, which is not so different from the one used by OCS2 MPC.

In a second experiment shown below:
https://www.youtube.com/watch?v=zQrpfKmBaPI
The cart-pole dynamic was simulated by the Bullet physical engine. It uses the same OCS2-CoppeliaSim interface as the first video. But in this case, the controller failed almost instantly, rising an error message: "Integration terminated since the maximum number of function calls is reached. State at termination time " (hosted in IntegratoeBase.cpp).
From our observation, we suspect that this failure was caused by the discrepancy between the model used in the MPC and the actual behaviors of the simulated cart-pole. We wonder what's your thought on this?

If the root cause really is model discrepancy, our best option might be using the linear + feed-forward controller instead of pure feed-forward. To this end, can you provide some pointers on how to use the output from the linear + feed-forward controller (controllerType: 2)?
Using the control output of feed-forward controller (controllerType: 1) is easy, we just grab the values from inputTrajectory. But we are not sure where to get the output from controllerType: 2. -- Since it seems to be calculated in a post-processing step in readPolicyMsg() in MRT_ROS_Interface.cpp.

[rubengrandia]

Sorry for the delay in getting to your question. You are right that at the moment all examples use this DummyLoopNode structure and we have no examples yet that show how to best integrate with other simulations (or the robot).

Integrating your own physics
There are a few ways to go, depending on your needs.

1. Stay with the dummy loop setup, but provide a different dynamics. In this scenario, you would create a separate system dynamics object that you want to use as a simulator. (These dynamics should derive from ocs2::ControlledSystemBase) From this system dynamics, you can create a "TimeTriggeredRollout" and pass that rollout to the MRT. You would replace the default rollout with your custom rollout in this line:
   

   ocs2/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp

   Line 63 in b037d81

   mrt.initRollout(&cartPoleInterface.getRollout());

2. Integrate a "MRT_ROS_Interface" into your own simulation loop. In this case you would follow the example until here:
   

   ocs2/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp

   Line 64 in b037d81

   mrt.launchNodes(nodeHandle);

   
   You can embedd this MRT object into your own simulation loop, and it will handle the ROS communication with the MPC loop for you. You would then be calling setCurrentObservation(), updatePolicy(), etc. Your own simulation loop will probably look similar to the implementation of the Dummy loop, so I guess this is what you have current done: Adapting the dummy loop to your situation.

3. A third way is to use the ROS independent interface:
   

   ocs2/ocs2_mpc/include/ocs2_mpc/MPC_MRT_Interface.h

   Line 49 in b037d81

   class MPC_MRT_Interface final : public MRT_BASE {

   
   This interface has both the MPC and the MRT in one, giving you full control over both components. This is what we use when deploying the algorithms on hardware. I won't go into more detail for now. Just so you know it exists.

How to use the feedback policy
This can be done from the config file:

ocs2/ocs2_robotic_examples/ocs2_cartpole/config/mpc/task.info

Line 37 in b037d81

useFeedbackPolicy false

Set that one the "true". From there the MPC node should switch to publishing also the feedback matrices, and the MRT node will use them during the call to evaluatePolicy()

The MPC fails
A few things come to mind here

• The manually implemented dynamics in OCS2 is wrong. [I will look into this]
• Is your simulation using the same conventions and units as the OCS2 model? It might be worth it to look at the model errors between the MPC model and your simulation in isolation to rule such mistakes.
• Is the simulation loop synchronized with the MPC loop? In other words: is the simulation halted while a new MPC optimization is done? If not, a large delay in the pipeline can cause problems.

[mszuyx]

@rubengrandia Thank you for your in-depth response!

Updates:

We have recently resolved the MPC failure issue. Your guesses were right, we identify a bug in our pseudo MRT_ROS_Interface (we named it CoppeliaCartpoleNode.cpp, the equivalent to DummyCartpoleNode.cpp) that caused synchronization issue between CoppeliaSim and OCS2. Now both the physic engine simulated cart-pole and the cart-pole with hardcoded dynamic work with OCS2 (with the default physical parameters, i.e., cart mass: 2.0 kg, pole mass: 0.2 kg, pole length: 1.0 m)! (Here is a brief demo for the physic engine simulated cart-pole)

https://www.youtube.com/watch?v=kOZuncIXxXw

The pure feed forward mode didn't work so well with this physic engine simulated cart-pole (probably due to model discrepancy). But switching the control mode to linear + feed-forward controller (controllerType: 2) resolved the issue.

Current issues:

Now since the interface seems to work, we are moving on to adapt our own robot dynamic / physic with OCS2 MPC.

1. First, we leave the dynamic model (the EOM) of the cart pole the same, but changing the physical parameters to cart mass: 10.5 kg, pole mass: 40 kg, pole length: 0.3405 m in both the task.info and the CoppeliaSim.
2. We tuned the control weight matrix in task info to 0.01. The dummy test works fine with this setting.
3. However, OCS2 MPC failed to perform the swing up test / small angle balancing in CoppeliaSim.

We will provide more detail and information on this topic soon.

Follow-up questions

1. It seems like learning MPC_MRT_Interface.h is the key to the correct implementation and deployment of OCS2 MPC. To better understand it, we figured the double_integrator is a good place to start. Is there any other source / example that worth looking into?
2. Can you share some info on what kind of hardware you used to host the OCS2 MPC for your robot? Is it some kinds of real-time computer? Or just regular Ubuntu - ROS but assuming it is updating fast enough to be considered real-time?
3. The error "Integration terminated since the maximum number of function calls is reached. State at termination time" still occasionally occur when the control task fails. We suspect that it is due to some robot states making the non-linear dynamic model unsolvable to the MPC. What is your insight?

Thanks again! We will provide further update soon!

[rubengrandia]

Good to see it working for the default case!

I checked the dynamics implementation. That seems all fine. How are you defining the inertia of the pole in your simulation?
In OCS2 it is assumed that the center of mass is located at the middle of the pole (L/2), and that the Inertia around the center of mass of the pole is that of a pole with uniform mass distribution (I = m / 12 * L^2).

You might want to increase the terminal cost "Q_final" as well. Given that you say it already fails for small angles.

1. For the MPC_MRT_Interface you can start here:
   

   ocs2/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp

   Line 86 in b037d81

   TEST_F(DoubleIntegratorIntegrationTest, synchronousTracking) {

   
   After that, there is the test "asynchronousTracking" in the same file.

2. We mostly use regular Intel (laptop series) CPUs with Ubuntu + RT kernel patch, so soft real-time. With a multithreaded setup as in the "asynchronousTracking" example, you can have one thread executing the latest MPC policy and one creating running a new optimization. This way the optimization is not blocking the update of the commands.

3. The maximum integration reached error is indeed usually a sign that the current problem is 'hard'. The continuous time DDP algorithm takes an adaptive stepsize both in the forward and backward pass. If the problem becomes very stiff, this stepsize will go to zero, and we abort after reaching the maximum number of steps. We have a few other algorithms in the toolbox that you could try, but I would stick with this one for now.

[mszuyx]

@rubengrandia Thank you! This is super helpful!

Updates:

We fixed the issue with the heavier physical parameters.

• CoppliaSim uses a dt of 50 ms by default. So, running the simulation in real-time results in a update frequency of ~20 Hz. Therefore, we set the MPC and MRT desired frequency to 20 Hz to match with the CoppeliaSim's update rate. (Unfortunately, we cannot easily change the dt setting as the simulation needs to hook up with some VR applications and HMIs)
• It turns out, the simulation will miss a few steps of observation / policy exchanges at the start of the simulation. This issue remained undetected as the previous set of physical parameters result in a system dynamic that has a larger time constant. As the mass of the pole increases and the length of the pole decreases, the time constant becomes more critical and less tolerance to control delay.
• After setting the MPC and MRT desired frequency to a higher number (100 Hz), this problem disappears.

Here is an updated demo:
https://www.youtube.com/watch?v=Xn6ilTbPa9Y

We will move onto adapting the full dynamic of our robot in the current OCS2-CoppeliaSim interface. We will keep you updated!