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
806 stars 166 forks source link

Avoid self-collision for manipulator task #1097

Closed ThorbenPultke closed 1 year ago

ThorbenPultke commented 1 year ago

Hi there, thanks again for this great oc framework!

I encountered a question which i was not able to answer on my own.

In https://github.com/loco-3d/crocoddyl/blob/0500e398861564b6986d99206a1e0ccec0d66a33/examples/humanoid_manipulation_ubound.py#L77-L80 you include an example on how to implement self-collision avoidance for the talos humanoid robot. From my understanding, what you did there is actually setting upper and lower bounds on the states and their derivatives, isn't it? Like in https://github.com/loco-3d/crocoddyl/blob/57ee3a1764ce06269b9c134c66da688ee1a54d2e/bindings/python/crocoddyl/utils/quadruped.py#L477-L480

What I am trying to do is to prevent self-collisions e.g. from the endeffector of my manipulator and its shoulder-elbow-link in euclidean space - besides limiting state bounds with the mentioned cost function. Are self-collisions calculated and taken into account automatically when specifying state bounds? My first intention was to describe body links as capsules and penalize the distance from all frame origins towards all body links.

Best regards! Thorben

ThorbenPultke commented 1 year ago

After some investigation and tests concerning my questions, I came to the conclusion that, indeed, if you specify state bounds (e.g. limit max and min joint positions and angular velocities for a manipulator), crocoddyl seems to automatically includes self-collision avoidance in terms of those soft constraints. This is quite interesting since, at least for me, I didnt saw a hint to that anywhere? Did I miss something? Or am I completely mislead?

cmastalli commented 1 year ago

Hi @ThorbenPultke,

We are happy that you find useful Crocoddyl and we welcome you to contribute to our project.

I am not sure if you have sorted out your doubts; however, let me explain you a few things.

The above-mentioned example adds a quadratic penalisation (soft constraint) to the robot's state limits. It encourages the solver to find solutions that don't violate the joint limits, for instance, but I wouldn't say its purpose is to model self-collision (soft-)constraints. Self-collision involves checking whether two or more geometries are colliding.

Crocoddyl also supports the evaluation of self-collision constraints. This can be done through the ResidualModelPairCollision class (see https://github.com/loco-3d/crocoddyl/blob/master/include/crocoddyl/multibody/residuals/pair-collision.hpp). Note that this class is compiled when you have installed Pinocchio with HPP-FCL support.

Finally, we are working on novel solvers to handle efficiently any type of constraint.

If this reply answers your questions, then I would ask you to kindly close this issue.

Thanks, Carlos

ThorbenPultke commented 1 year ago

Hi Carlos! Thank you so much for your detailed explaination. I indeed was quite confused with the comment "cost for self-collision" https://github.com/loco-3d/crocoddyl/blob/0500e398861564b6986d99206a1e0ccec0d66a33/examples/humanoid_manipulation_ubound.py#L65 while doing my research on how one would include self-collision avoidance.

I now managed to include soft constraints via the cost function with your mentioned class to avoid those self-collisions. Thanks again for your help! :)

Thorben