solve whole Body inverse kinematics with iKin / Cartesian Solver

[KevinAStein]

Hello,
is it possible to use the iKin/Cartesian Solver for the inverse kinematics given the left and right foot position and orientation and "CoM-Frame" attached to the pelvis of the robot. We are looking for a whole Body configuration that matches these three end-effectors at the same time. (iCubHeidelberg01)
@hu-yue

thanks

[traversaro]

Hi @KevinAStein ,
at the moment the iKin Cartiasian Solver (whose logic is implemented in the iKinSlv http://wiki.icub.org/brain/group__iKinSlv.html ) is designed for solving the inverse kinematics of kinematic chains.

In particular when you ask for a given pose of of the left foot, you are (implicitly) asking for a given pose of the left foot with respect to the root link of the robot. In other words, you are considering the chain connecting the root_link to the l_foot link. To solve this problem, the cartesian solver returns a joint configuration of only the joints belonging to the left leg chain.

A possible workaround is to consider the chain going from the left foot to the right foot. The DH model for a chain like that can be easily extracted from the URDF model of your robot using the urdf2dh command that you should have installed if you have compiled the codyco-superbuild. The configuration files for a scenario
similar to this one are available in https://github.com/robotology/codyco-modules/tree/master/src/modules/graspAndStepDemo/app/robots/iCubParis01 .

What you mean by "CoM-Frame" attached to the pelvis of the robot? A frame whose origin is coincident with the COM is definitely not attached to the pelvis of the robot, but instead the location of the COM depends on the complete configuration of the robot, including (in the case of the iCubHeidelberg01) the configuration of the 3 degrees of freedom of the torso.

To perform wholeBody inverse kinematics considering also a desired COM you will need to modify the iKinSlv class to use a wholeBody kinematic model of the complete tree of iCub, for example using the function provided in the iDynTree library. This is something that involves a good amount of work, but it is something that we discussed to be useful for us with @DanielePucci , so I will be happy to provide guidance if you want to invest time on this.

[hu-yue]

@traversaro thank you for the answer!
What we mean with "CoM Frame" is that we assume a frame fixed wrt the root that is close to the actual CoM position of the robot while standing in static position, instead of the real CoM frame that comes from the joint configuration of the robot as you correctly state.
This was meant to make things easier to do for @KevinAStein.

We do not exclude the possibility to do what you suggested with the modification of iKin, but we also consider to start the work with our library RBDL.
Thanks!

[DanielePucci]

It is going to be really important to have inverse kinematic algorithms that comply with closed kinematic chains. With @gabrielenava, we have started working on this by implementing really simple integration-based inverse kinematic algorithms that started working this week. Once they work properly, we will make a step further and implement solver-based algorithms, and we will go in deep on the iKin Cartiasian Solver.

If you guys @hu-yue @KevinAStein start working on that, it would be great. In this case, keep us in the loop.

[hu-yue]

@DanielePucci ok thanks! We'll keep in touch.

[pattacini]

@DanielePucci 👍
It might be helpful to exploit @alecive's experience on the topic of closed loop inverse kinematics before he leaves, that is going to happen very soon, sadly.

[DanielePucci]

Thanks @pattacini! I'll keep @alecive in the loop, even if for a small amount of time :(

[DanielePucci]

Btw, @gabrielenava, where did you commit these results? @traversaro

[alecive]

Just a note: I am not dying, guys. Just leaving! 😄

[pattacini]

tonight let's 🎉 🎈 🍻

[DanielePucci]

I have a volleyball match, joining you later 💃