incorrect multi-target inverse kinematics

[khanhha]

Hi, I would like to ask a problem regarding multi-tasks inverse kinematics.

In this problem, I generate multiple target points for limbs and shoulder joints of the robot and then optimize joint angles and root transformation to match these target points. However, the iteration doesn't converge at all.

I know that this might not be a problem with Pinocchio, but I hope that you could give me some directions for debugging it.

• Here a video showing the iteration process.

• Here is how I generate the target points. Adding some noise and translation to the neutral point. The robot I use in this experiment is Romeo_small.URDF

• Here is the IK algorithm. I went over many times but couldn't find a bug in it.

I have been struggling with this problem for a few days and find no way to continue yet. I would really appreciate if you could just give me some hint for debugging it.

I look forward to hearing from you soon.

Best Khanh Ha

I attached the code below, just in case you need it.

hello_humanoid.zip

[jcarpent]

Did you look at the examples?

[khanhha]

@jcarpent : thanks for your quick response. yes, there is an example of inverse kinematics with one target location from Pinocchio. But my IK implementation is for multi-target locations.

[khanhha]

@jcarpent: I guess this code is pretty simple to an expert like you!

Could please help me take a look to see if there is any logical/mathematical error?

In my code, there is one difference from the Pinocchio IK example, the target includes both orientation and location; however, in my code, I have no target orientation, so I end up using SE3 with identity rotation to get the error.

[jcarpent]

Unfortunately I don’t have time to dive into your code. Otherwise, my brain will become crazy.

Small hint: if you only want to reach some position, just consider the three rows of the Jacobian related to the translation, the three first rows. Another hint: do not try all the constraints at the same time. Proceed progressively.

[khanhha]

@jcarpent Thank you very much.

If it works well, I would like to contribute this example to Pinocchio source code.

Unfortunately I don’t have time to dive into your code. Otherwise, my brain will become crazy.

Small hint: if you only want to reach some position, just consider the three rows of the Jacobian related to the translation, the three first rows.

Actually, I have tried your suggestion of using just the first 3 rows of Jacobian matrix. I'll try to give it another try.

Another hint: do not try all the constraints at the same time. Proceed progressively.

Yes, I'll update if it helps.

Best,

[khanhha]

Hi @jcarpent

I've found out another detail that with pin.JointModelFreeFlyer added, the IK is very unstable and fluctuates a lot, it doesn't converge at all.

However, with this JointModelFreeFlyer removed, the IK is more stable and seems to converge.

But without JointModelFreeFlyer, I can't optimize the root point.

Do you have any idea why JointModelFreeFlyer causes this convergence problem.

Thank you very much

[khanhha]

Hi, the problem is solved by change Reference from pin.WORLD to pin.LOCAL_WORLD_ALIGNED. I am still not sure why but the multi-task IK works perfectly now. The issue can be closed I guess. Thank you very much for your support.

[khanhha]

here is a demo of the result

[jcarpent]

Nice. Thanks for your feedback.