ethz-adrl / towr

A light-weight, Eigen-based C++ library for trajectory optimization for legged robots.
http://wiki.ros.org/towr
BSD 3-Clause "New" or "Revised" License
925 stars 230 forks source link

Whole-body dynamics in gait optimization #71

Open RobertoRoos opened 4 years ago

RobertoRoos commented 4 years ago

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: hopper_towr

NLP
Variables:

Constraints:

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 commented 4 years ago

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:

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 commented 4 years ago

Hi Alex,

Thank you!

awinkler commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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. dynamic_constraints

To my surprise the results are better: monoped_direct_collocation 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.