search

loco-3d / crocoddyl

Crocoddyl is an optimal control library for robot control under contact sequence. Its solver is based on various efficient Differential Dynamic Programming (DDP)-like algorithms
BSD 3-Clause "New" or "Revised" License
830 stars 170 forks source link

Issue on non-constant angular momentum during aerial phase #979

Closed zzhou387 closed 3 years ago

zzhou387 commented 3 years ago

Hi folks,

Thanks for maintaining this awesome codebase first. I have been trying to generate some trajectories that involve aerial phases such as jumping or backflipping using crocoddyl. Since there is no contact force during the aerial phase, the angular momentum is supposed to be constant. The attached gif is one example I generated using crocoddyl: A1_jumping

However, when I plotted out the trajectory of angular momentum, it is not always constant: examp_1

I was thinking it might be a numerical issue when integrating the dynamics, so I changed the timestep from 0.005 to 0.001. However, it didn't improve too much. It became even worse when the legs are jiggling during the aerial phase: A1_bonding examp_2

I am wondering whether you guys have ever noticed this issue before or maybe there is something wrong with my current optimization formulation. FYI, I only have EE and CoM position trackings during the aerial phase, and no contact model is added for both cases. I also double checked the contact forces are always zero during aerial phase.

Thanks a lot, Ziyi

cmastalli commented 3 years ago

Hi Ziyi,

Thank you for being part of Crocoddyl community!

Would you let us know more details about your setup? Particularly I am interested to know about:

If this is the case, it would be interesting to report results for RK4 integrator with JMinvJt_damping equals zero.

Cheers, Carlos

proyan commented 3 years ago

Could you please try after reinstalling the current devel version of pinocchio, and then reinstalling crocoddyl? I'm afraid if that might be because of https://github.com/stack-of-tasks/pinocchio/issues/1473, but just have to be sure.

zzhou387 commented 3 years ago

Hi Ziyi,

Thank you for being part of Crocoddyl community!

Would you let us know more details about your setup? Particularly I am interested to know about:

  • The value that you used for JMinvJt_damping
  • The integrator that you used it.

If this is the case, it would be interesting to report results for RK4 integrator with JMinvJt_damping equals zero.

Cheers, Carlos

Thanks for your reply. The JMinvJt_damping has always been zero. I tried RK4 instead of Euler and it does help a lot. Now the AM is constant during the aerial phase. Here is the plotting for the second bonding example: Figure_11

Seems like the integration scheme matters a lot when integrating the momentum terms. thanks again!

zzhou387 commented 3 years ago

Could you please try after reinstalling the current devel version of pinocchio, and then reinstalling crocoddyl? I'm afraid if that might be because of stack-of-tasks/pinocchio#1473, but just have to be sure.

Thanks for letting me know this bug in analytical jacobian of centroidal momentum. I have fixed the non-constant issue by switching to RK4, but I'll do some benchmarks on the previous Euler case once I get a chance to reinstall everthing. I'll update you here.

jcarpent commented 3 years ago

A quick feedback. Your issue is not due to stack-of-tasks/pinocchio#1473 but rather to the drift induced by poor quality integrators. Using advanced scheme will help for sure. This is a well-known problem in Numercial Mechanics.

jcarpent commented 3 years ago

@zzhou387 What is the renderer you have been using so far?

zzhou387 commented 3 years ago

@zzhou387 What is the renderer you have been using so far?

This is in rviz.

cmastalli commented 3 years ago

A quick feedback. Your issue is not due to stack-of-tasks/pinocchio#1473 but rather to the drift induced by poor quality integrators. Using advanced scheme will help for sure. This is a well-known problem in Numercial Mechanics.

Yes, indeed, I suspect this is the explanation, reason why I asked you to report results with RK4.

Generally speaking, numerical integrators inject energy in the systems.

Note that our Euler and RK4 integrators are the best that they could be (i.e., simpletic schemes) as we need to keep the Markovian structure in DDP-like algorithms.

cmastalli commented 3 years ago

@zzhou387 What is the renderer you have been using so far?

@jcarpent --- please have a look on an Rviz plugin that I developed: https://github.com/loco-3d/whole_body_state_rviz_plugin

Indeed, I am planning to develop a Python callback function (to avoid ROS dependencies at compile time) as I am very unsatisfied with GepettoViewer (e.g., cannot display motions at real-time, code under development). Note that we are heavily using the plugin in our developments.

jcarpent commented 3 years ago

On our side, we are about to add the support of RVIZ in Python for easy visualisation within Pinocchio. This will help everybody, without having to write C++ code.

cmastalli commented 3 years ago

On our side, we are about to add the support of RVIZ in Python for easy visualisation within Pinocchio. This will help everybody, without having to write C++ code.

We could indeed develop this in Pinocchio (rather than Crocoddyl). If you like this solution, I will be keen to work on it asap. Let's schedule a meeting if this is in your interest.

jcarpent commented 3 years ago

Yes, sure.