search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.97k stars 398 forks source link

implementation of optimizable link/bone lengths #1334

Closed khanhha closed 4 years ago

khanhha commented 4 years ago

Hi Pinocchio team,

I would like to ask two questions about the usage of Pinocchio in a computer vision problem that regresses human bone length and joint angles from multi-view 2d poses in multi-camera images.

I need to do this because as far as I know, the rest pose, link lengths of a robot in Pinocchio, which is defined prior in the input URDF file is fixed and now I need to make it optimizable, as a part of the configuration vector.

So far, I have implemented a solution that uses PrismaticJoint to simulate the effect of optimizable segment lengths. I did one IK experiment to solve for goals that are out of reach of the rest pose, and it seems to work. As you can see in the below video, the Prismatic Wrist joint is extended to reach a very far target.

https://www.youtube.com/watch?v=grgLSQfN9Pg

However, it's just a work-around solution. Could you please suggest me a more correct way to do it in Pinocchio?

Thank you very much and I look forward to hearing from you soon.

Best Khanh Ha

jcarpent commented 4 years ago

Hi @khanhha,

the first question is how and which code files/classes in Pinocchio I should modify to build a Model that has optimizable segment lengths? As far as I know, the model.jointPlacements which represents the rest pose of a model are fixed, is there any chance I can make it optimizable?

Even if a model seems to have a fixed structure, you can easily change the parameters like the inertias or the joint placements, they will be used in the computations then.

* my second question is how to calculate the Jacobian matrix that relates the joint changes to the link/bone lengths changes, given the Pinocchio code base?

This is exactly the purpose of my PR https://github.com/stack-of-tasks/pinocchio/pull/1332 done for you. You can use it either in Python or C++. I will make a new release today.

Best,

Justin

khanhha commented 4 years ago

That's so awesome. A few days ago I wrote you an email asking about this feature, and now you did it. You're the greatest developer of all the times in my eye :D I am checking the code now. Best Khanh

jcarpent commented 4 years ago

Thanks for this gentle feedback. The code is now released. Have great fun with it.

khanhha commented 4 years ago

Here is a demonstration of the feature. In this video, the joint translation in the rest pose is optimized so the left wrist reaches a target location.