Joint level controller for NMPC for quadruped

[lwygzh]

Hi,

First, I want to thank the contributors of this library for creating this awesome package!

I am trying to create a NMPC controller for a quadruped. And I am a bit confused on what kind of joint level controller I should have to produce the final control torques. I have tried two things:

• directly applying the generated torques from the NMPC controller
• using the the generated torques from the NMPC controller plus a PD controller that follows the generated state trajectory

I noticed that in the first case the performance is not that well when the computation delay is introduced to the quadruped system. The second controller does improve upon the first controller but it seems to be very sensitive to the cost function weights, amount of delay, and number of solver iterations (there are probably more factors coming into play, but these are the ones I tuned the most).

So I was wondering if anyone sees any issues in the joint level controllers I have?

Two other options I want to try are

• to incorporate the computed ground reaction forces, but I'm not sure how should I integrate them into the joint controller
• add the joint accelerations into the state, then compute the desired feedforward torques using $q$, $v$, $\dot{v}$, and $J^TF$, then add a feedback controller using a PD controller that follows the generated state trajectory (I'm more inclined to this solution, however, I'm not sure how to create an action model that has the joint acceleration in its states.)

Thanks in advance for the help!

[cmastalli]

Hi @lwygzh !

Your question is a bit tangential to our project, but I'm happy to share experiences with you.

There is no general answer when designing a low-level controller, this depends on the robot itself.
Your current comments don't make sense to me.

1. Both controllers can easily deteriorate with communication delays. If you want to account for it, then you need an MPC that introduces these delays inside the dynamics.
2. PD gains cannot sort out communication delays. They are still tracking joint references that have been computed with no-delayed dynamics.
3. PD gains cannot increase sensibility to cost weights. You have hand-tuned them. Instead, D gains amplify high-frequency signals, i.e., encoder noise. You need to be careful with this.

If we have perfect dynamics, then an MPC predicts perfect torque commands.
In this situation, the MPC feedforward torques will completely drive the system as the P and D errors would be zero.
Now, we can say that PD controllers aim at correcting unexpecting dynamics (e.g., joint friction, discretisation errors).
But, you might aim at reducing these as much as possible, as we typically wish to have a compliant robot.

Regarding contact force integration, you might first read our works

• Agile Maneuvers in Legged Robots: a Predictive Control Approach, and
• Inverse-Dynamics MPC via Nullspace Resolution

From the first paper, you can see that introducing whole-body controllers (or PD controller) lead to different control policies.
This means in practice: errors in the angular momentum and swing-foot trajectories.
We use contact forces in our whole-body controller.

Contact forces might be implicit or explicit decision variables.
They are explicit if we formulate our MPC via inverse dynamics as described in the second paper.
Similar comment for the generalised accelerations.

Integration of the contact forces/accelerations requires a controller with inverse dynamics.
You can introduce feedback actions for the contact forces and accelerations by rollouting the inverse-dynamics policy.

From the above comment, you might now be aware that we don't need to add the generalised acceleration (I guess this is what you mean with "joint") as these variables are implicit.
You can easily obtain them using Crocoddyl v2 (via diffData.multibody.joint.a)

I hope this helps.

[lwygzh]

Thank you for the response and sharing the two papers!

For the controllers I tried out, to put some numbers to it, I used a MPC integration time step of 5 ms, a MPC planning frequency of 20 Hz, 20 knot points. So in this case the preview horizon is 100 ms. When there is a delay caused by the MPC computation, the joint level controller still has access to the old plan. Do you think this counts as introducing the delays into the MPC formulation?

[cmastalli]

No, this doesn't count. Delays affect the dynamics per se.
However, I am concerned with your setup.
You should increase the horizon.

For reference, we use:

• time step: 10 ms
• number of nodes: 80-180
• horizon: 0.8-1.8 s
• MPC frequency: 50 Hz

It is worth telling you that you can achieve this computation time by enabling multi-threading support.
This feature is not yet been released in our binaries from pip, conda or robotpkg.

[lwygzh]

By delays affect the dynamics, do you mean that the MPC plans a certain state trajectory, and because of the delay, when the new MPC plan is applied, the state already deviated from the new planned state trajectory?

If that is the case, do have any suggestions on how to handle this? Should the MPC plan start from a future estimated state assuming that the old plan was followed?

In your opinion, is it easier to let the MPC take delay into account or use a more robust joint level controller? If possible, can you give an example or point me to a paper that provides a more robust joint level controller?

If I compile from source will multi-threading support be available? If so can you provide or point me to what options I should set for the build process.

If you can give me a ball-park estimation of what kind of computation time I should expect with the setting you suggested that would be great!