Adding holonomic constraints

[shbang91]

Hello Dr. Caron,

Thank you for maintaining this nice repo.

I have a question regarding how to set a holonomic constraint when solving IK using the API you provided in this repo.
I've looked through the documentation of this repo, but I only found the body and posture task class, so my best guess is I could not solve the IK on the holonomic system given the current APIs.

My problem is: I have a 7-DOF manipulator in which one of the joints is passive(q_1) while satisfying the relation of q_1 = q_2 (one active joint) at the same time, so I would like to move this hard constraint to the cost function (by making it a soft constraint). Is there any task class that I can use to solve this problem?

I would really appreciate you if you could get me any ideas or suggestions.

Best,
Seung Hyeon

[stephane-caron]

That's an interesting use case, we should add an example for it 🙂 I see a few ways this could co:

• You can make your own task, similar to the posture task but only over $q_1$ and $q_2$ rather than over the whole $q$. This is an easy way if you want a quick proof-of-concept.
• We can add a more general LinearHolonomicTask that would enforce $A q = b$. It would cover $q_1 - q_2 = 0$ as a particular case.
• We can add an even more general HolonomicTask that would enforce $A(q) = 0$, but then we'll want a way to specify $A(q)$ that's both differentiable and convenient to use.

[shbang91]

Thanks for your comment!

I think the holonomic task depends on its use case. Based on your first suggestion, I made a simple holonomic task class, in which I construct the Jacobian of the holonomic constraint inside compute_task_dynamics(self, configuration: Configuration) and subsequently formulate the weight matrix H inside compute_qp_objective(self, configuration:Configuration). I tested this with my robot, and it works fine.

If you want me to make a pull request for the holonomic task class, I can work on that to make it more generic. Let me know what you think.

Thank you again for your help!

[stephane-caron]

Great 😃 Feel free to open a PR, I'm all in for adding it to Pink as it can be useful to others.

[shbang91]

Awesome. Once I finish the work, I'll open a PR. Thank you!

[stephane-caron]

Since #40 the error and Jacobian are now computed in two separate functions (this change will hit v0.9.0).

Feel free to finish your work and open a PR with the old API, I can help with adapting to the new one. And of course if you can make the PR for the new API directly all the better 😉

[shbang91]

Sounds good. To me, it makes more sense to separate those functions.
It has been a tough week but now I have some time to work on it. I think I can finish it shortly. Thank you.

[stephane-caron]

Closing this issue now that #43 has been merged.

Opened #45 as a follow-up to #43 (comment).