search

machines-in-motion / kino_dynamic_opt

Kino-dynamic optimization algorithm for multiped robots
BSD 3-Clause "New" or "Revised" License
38 stars 9 forks source link

second order IK #44

Open righetti opened 3 years ago

righetti commented 3 years ago

@jviereck @majidkhadiv @avadesh02 @Neotriple

I added my (temporary code) to do 2nd order IK that takes a set of desired trajectories and solves the pb, 1) with a weighted inverse or 2) with a nullspace projection to enforce feet movements first

A temporary jupyter notebook and some data files show how it works

This is work in progress and needs to be integrated in the full optimizer but I think I manage to get nicer tracking and more control over what needs to be optimized.

it's in the branch ludo/second_order_ik

jviereck commented 3 years ago

Thanks for providing the initial code for the 2nd order IK!

I've pushed a commit into the branch (hope that's okay) which does the integration into the optimization framework. By default the second order IK will be used.

While optimizing a static walk and a trotting trajectory, the linear momentum trajectories started to oscillate. I managed to remove this effect by lowering the p_com_tracking and mom_tracking by a factor of 10. Now the tracking looks good on my examples.

In addition, I've noticed that the orientation of the robot was oscillating. Reducing the p_orient_tracking helped with reducing the frequency of the oscillation but not the amplitude. I've therefore added a new damping on the orientation which can be tuned using d_orient_tracking.

majidkhadiv commented 3 years ago

I am unable to reproduce the jump and trot trajectories I had for Solo8 with the 2nd order IK. But I believe that it is a matter of tuning and since we have less DOFs for Solo8, tuning is a bit more tricky. So, if @jviereck could have a better tracking with 2nd order, I can also try to make demos work on Solo8.

@righetti One thing that I think would be problematic is projection to the null space of all the feet, while we should only do that for the stance foot. Specially for jumping, minJerk trajectories for swing feet are not good and then it does not seem to be a good idea to do the projection to the null space of the swing foot. What do you think?

@righetti @jviereck One other thing that is quite important to me is that we should not let the kinematic optimiser to change the momentum in the air (it should be a free fall motion). I think this is very simple to implement, if you agree I can modify both first and second order IK adding this feature to it.

To proceed, we need to do the following steps:

  1. move the parameters of the 2nd order IK to the config files for each demo.
  2. add torque constraints to the 2nd order solver (or we can first merge it to the master and I can work on that afterward)

Let me know how you want to proceed, I have time to work on any of these problems...

jviereck commented 3 years ago

I took a closer look at the second order optimization for the trotting task on solo12. What I find is that the com and lmom are better tracked when doing a dyn-kin-dyn optimization compared to using first order optimization. The amom generated by the dyn optimizer in y is also lower. Still the tracking of the generated second order plan in a simulator leads to wobbeling of the base when both legs are in the air.

Here are the plots:

First order optimization: image

Second order optimization: image

jviereck commented 3 years ago

@righetti @jviereck One other thing that is quite important to me is that we should not let the kinematic optimiser to change the momentum in the air (it should be a free fall motion). I think this is very simple to implement, if you agree I can modify both first and second order IK adding this feature to it.

Is there a way to restrict the invkin to generate only momentum along the possible axis in general? For instance, when two feet are on the ground, it should only generate momentum along the line between the feet.

To proceed, we need to do the following steps:

  • move the parameters of the 2nd order IK to the config files for each demo.

That sounds good. Could you have a look at this?

  • add torque constraints to the 2nd order solver (or we can first merge it to the master and I can work on that afterward)

Maybe it's better to merge the changes first and then add new features.

righetti commented 3 years ago

Thanks all for checking the code and integrating it with the kino-dyn, a few things:

majidkhadiv commented 3 years ago

@righetti @jviereck One other thing that is quite important to me is that we should not let the kinematic optimiser to change the momentum in the air (it should be a free fall motion). I think this is very simple to implement, if you agree I can modify both first and second order IK adding this feature to it.

Is there a way to restrict the invkin to generate only momentum along the possible axis in general? For instance, when two feet are on the ground, it should only generate momentum along the line between the feet.

I think you mean "not generate angular momentum along that line", in that case we should put an equality constraint to make the angular momentum around that line zero. I think it is a good idea and I will look into it when I started to implement fixed angular momentum in air phase.

To proceed, we need to do the following steps:

  • move the parameters of the 2nd order IK to the config files for each demo.

That sounds good. Could you have a look at this?

Yes I will do it next week...

  • add torque constraints to the 2nd order solver (or we can first merge it to the master and I can work on that afterward)

Maybe it's better to merge the changes first and then add new features.

Sounds good to me, I will start working on it after merging current code to master...

majidkhadiv commented 3 years ago

Thanks all for checking the code and integrating it with the kino-dyn, a few things:

  • I wrote this mainly for fun so we can ignore this if it does not provide any improvement

I think it is very useful, thanks for doing that. I wanted to start doing that, now I can continue working on this and improve it over time...

  • Yes the foot tracking is probably not the best priority / we can easily play with hierarchies or a weighted inverse and see what works best. It is also easy to get rid of some control directions if needed (@majidkhadiv you are right we should ensure momentum is conserved during flying phase)

Sounds good, I think I also will work a bit on having a general implementation that we can change hierarchies depending on task, it could be useful.

  • ideally we should not have prescribed swing foot trajectories / maybe there is a smart cost design that could work instead - but I think it would be easier to do that considering the horizon (because we want to ensure that the feet hit the ground at the right time/place)

I agree, we need horizon for that. I have some ideas in mind to investigate for that...

  • the way I compute the reference accelerations for the tasks can have issues because the splines sometimes have messy derivatives - we would keep this in mind

Good point!

majidkhadiv commented 3 years ago

@righetti @jviereck I would like to rebase this branch with master and then add the parameters of the 2nd order IK to the config files. There are many conflicts in CMakeLists.txt and I am not confident resolving them. Can someone who is more confident rebase the branch with master such that I can proceed?

jviereck commented 3 years ago

I take a look at rebasing it now.

jviereck commented 3 years ago

@majidkhadiv I tried to rebase as well but this leads to many rebase conflicts. Instead, I merged the current master into the second order IK. The result is pushed to ludo/second_order_ik branch.

majidkhadiv commented 3 years ago

I added the second order IK parameters to the config files and adapted all blmc robots config files in the demo folder based on the changes. I have not managed yet (not tried so much) to make any of the demos available in the repo work using second order IK. That is why, I set the default IK to be the first order one for now... I will try to see if I can make any of the demos work using the second order IK, but the code is ready to be merged. Let me know what you think

jviereck commented 3 years ago

If the code is ready to be merged, can you do a PR?