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
845 stars 173 forks source link

About whole body manipulation with cop constraint #880

Closed ddliu365 closed 3 years ago

ddliu365 commented 4 years ago

As the issue mention in #792, for the whole body manipulation example in /crocoddyl/examples/whole_body_manipulation.ipynb, should we add the zmp constraint while moving the manipulator with whole body motion for balancing torso?

Maybe the friction constraint is enough, but when should we consider adding the zmp constraint for torso balancing?

cmastalli commented 4 years ago

Yes, the friction constraint is not enough. But the CoP is not enough neither.

What we need is the contact wrench cone.

cmastalli commented 4 years ago

Note that Crocoddyl can penalize the contact wrench cone.

ddliu365 commented 4 years ago

I check the paper contact wrench cone from http://arxiv.org/abs/1501.04719.

Friction cone and zmp are parallel concepts to guarantee the stability, while contact wrench cone includes them as well as the bounds on the yaw torque.

Is this understanding correct?

Is there any issue if I add the contact wrench cone cost for the whole body manipulation? I see in the example code you just used the friction cone while julesser @julesser used both of them. I am confused the case when I should use these three different costs.

I am appreciated if you can provide some explanation.

cmastalli commented 4 years ago

Is this understanding correct?

Yes, it is correct.

Is there any issue if I add the contact wrench cone cost for the whole body manipulation? I see in the example code you just used the friction cone while julesser @julesser used both of them. I am confused the case when I should use these three different costs.

No, there is not. Just note that you need to remove the friction cone cost (or CoP one) in this case.

We developed the contact wrench cone after this examples, this is the reason why we haven't yet integrated it. We certainty need to improve the examples at some point.

In any case, note that at the moment the main objectives of the examples are

  1. to instruct the users how to use the Crocoddyl API,
  2. to check the numerical behaviors of our solver.
cmastalli commented 4 years ago

Btw, feel free to contribute to improve this example.

ddliu365 commented 4 years ago

sure, thanks for your explanation which is very clear.

I am working on a whole body manipulation with high dynamic motion.

I will update the example once it works with cop or wrench cone.

ddliu365 commented 4 years ago

I have a question about the wrench cone rotation matrix.

How to define this matrix if the foot is not aligned with the world frame?

https://github.com/loco-3d/crocoddyl/blob/b0eeaa5713166d7e6955454b90aedf0fc940baa1/bindings/python/crocoddyl/utils/biped.py#L251

For example, the foot placement is like the following: footplate

cmastalli commented 3 years ago

The first element of wrench cone constructor defines the rotation of it.