Whole-body dynamics in gait optimization

[RobertoRoos]

WBD
I wanted to expand the optimizer to include whole-body dynamics, instead of only centroidal dynamics. The advantages are clear: the output is directly in joint torque which can be directly applied to the real robot, optimization can be done on the robot as well instead of the conceptual base and EE motion and things like physical constraints can be included more accurately.
This boils down to including generalized joint coordinates and torques in the NLP.

First concept
I extended some of the Towr classes such tat I can include a MuJoCo model for dynamics, kinematics and the related jacobians and created a new constraint for WBD. So far I have focused on the Hopper (monoped) since then I can easily include the end-effector position in generalized coordinates.
I got it (sort of) working:

NLP
Variables:

• End-effector position (3) [Still phase based]
• End-effector reaction forces (3) [Still phase based]
• Joint positions (3) [Constant polynomial]
• Joint torques (2) [Constant polynomial]

Constraints:

• Terrain (keep end-effector on the floor)
• Swing (keep swing EE positions in a straight line, doesn't do much for my 2D problem)
• Force (limit reaction forces and keep them zero during swing)
• DynamicTorque (keep WBD violation zero)

Problems
I am finding it very difficult to get the optimizer to converge to a solution. The above result is combination of polynomial durations and constraint dts that just happens to work. Moreover, adding a cost to the joint torques makes that no solution can be found.
I am curious if more people have working on this? Have there been similar experiences? Are there IPOPT solver settings that could be important?

[awinkler]

Hi Roberto! I think this is very interesting direction to explore 👍

I'm suspecting there are some conflicting constraints at work here, which make it hard for the solver to find a solution. A few thoughts to debug this:

• I would fix the step timings if you haven't already done so, not optimize over them.
• start with one jump forward (stance, swing, stance)
• Are the Cartesian endeffector states and forces still part of the optimization variables? Or are you now only optimizing over base state, joint angles and torques? I'm curious which constraints you used and how you formulated them if you don't have some variables available? A critical one might be the zero-endeffector-force-paramterization during swing phase. I'm not sure how you did this if you only have joint torques.
• Are you supplying the Jacobians (derivatives) of all these constraints as well? These might be very hard to write out analytically in terms of the joint angles. If you have wrong jacobians, that will also hinder the solver to converge. So a good test would be to try the above using IFOPT without your analytical Jacobians, only automatically derived numerical ones (change a setting in IFOPT). This takes a lot longer, but might converge and thereby hint at a bug in your Jacobians.

Alright, that would be my plan of attack, and intuitively i would think a single jump forward for a one-legged hopper should converge. But again, haven't tried this before, so curious to see what comes out.

Best
Alex

[RobertoRoos]

Hi Alex,

• Step timing is already disabled. (I actually never had it enabled, because I missed you need to enable it explicitly :p)
• I've tried only a single jump, the results are very similar to the gif in the first post. So the motion looks okay, but there is still the oscillation in the oscillation in the upper body.
• I updated the first post to include an outline of the NLP. Because this only a monoped I can still explicitly include the end-effector position.
• I am supplying the derivatives. They are determined numerically from MuJoCo, so that makes it easier. The IPOPT derivative checker finds no issues, so that seems good. Just tried the finite-difference Jacobian, and it seems to change little.

Thank you!

[awinkler]

Okey, thanks for clarifying the NLP layout and I see you already tried the above.

So you have some redundant variables you're optimizing over (joint torques vs foot forces & joint angles vs foot postion) and then linking those with additional constraints (WBD & Forward Kinematics/Inverse Kinamatics)? Because theoretically the solver would only need one of those variable sets and might have it easier to converge then. However, I understand the motivation for splitting it up, because it makes it easier formulating the Jacobian, so that makes sense.

[RobertoRoos]

No I started with redundant coordinates, but that didn't work well so I reduced it what it is now.

The foot forces (3 values) and joint torques (2 values) are separate. MuJoCo does not actually do ground physics, we rely on the implicit force. Then there is the end-effector position (3 values) and joint positions (3 values).

This is a niche notation that only works for a single EE, so I figured it would make a good start.

[RobertoRoos]

Small update. I strayed away from the Towr notation a little further by removing the splines. Instead, I tried the more basic direct collocation based on nodes and linear interpolation. In this notation no analytical acceleration is available, so instead I use trapezoidal integration between nodes to ensure valid dynamics.

To my surprise the results are better:

It is easier to converge to a valid solution and it is even capable of solving the torque cost. (However, the torque efficient solution looks stiff and awkward.)

I find it hard to explain why this would be more robust. My WIP hypothesis is that in this notation the dynamics constraints apply directly on the nodes, allowing better 'control' for the optimiser.