Does Pinocchio support non-chain robots ?

[Acwok]

Hello,

My robot is a free-flyer cube with two ur5 arms. I'd like to use one arm tip as a fixed base and the other as the end-effector.
I've tried MoveIt 2 but their ik solvers don't support urdf with non-chain structure: chain robot example: base -> ... -> tip_link non-chain robot example: tip_link_1 -> ... -> base <- ... <- tip_link_2

I've looked at some tutorials, but I only found examples with a single UR5 with a fixed base. However, as far as I understand, Pinocchio should also work with legged robots (which are very similar to our robot).
Therefore I am planning to create a MoveIt 2 kinematics plugin using Pinocchio.

I feel that integrating Pinocchio in a MoveIt plugin can be done easily, but I'm wondering if Pinocchio the right solution for my problem. Does Pinocchio support non-chain robots ?
If yes, do you have examples using inverse kinematics on robots with floating base ?

[jcarpent]

Thanks @Acwok for your interest and your suggestion of creating a MoveIt 2 module based on Pinocchio.

Does Pinocchio support non-chain robots ?

The short answer is yes. Pinocchio has been designed for generic tree-like robots and has been recently extended for robots with closed kinematic chains.

If yes, do you have examples using inverse kinematics on robots with floating base ?

Please see this tiny example in Python https://github.com/stack-of-tasks/pinocchio/blob/dc8333cb38e63a37e87f9b26c3ad7330b85b56d1/examples/inverse-kinematics.py

[stephane-caron]

Adding to that, here is an IK example of a "flying" dual-arm with UR3's:

https://github.com/stack-of-tasks/pinocchio/assets/1189580/665a5673-c2d0-45f3-afa4-b6d0428aaf9c

You can find the code here: flying_dual_arm_ur3.py. It is the same algorithm as in the example linked by @jcarpent above (with a weighted optimization to handle multiple tasks).

I found it a bit tedious to combine copies of the same model with the current API, but maybe I just haven't found a good way to do that :wink: Spinning it as a discussion in https://github.com/stack-of-tasks/pinocchio/discussions/1958.

[Acwok]

Thank you very much! It seems to be exactly what I was looking for 😃
I'm going to test it.

[GuoPingPan]

how to deal will a dual-arm robot? Do I need to separate the left and right arms in the urdf of the humanoid robot？Turn it to right_arm.urdf and left_arm.urdf(one arm with fixed base)? Dear @stephane-caron @jcarpent, I am looking forward to your reply.

I only change the ID of the official IK code, also diffierent target. I found that the right arm integrate will affect left arm.

while True:
    pinocchio.forwardKinematics(model,data,q)
    dMi = oMdes.actInv(data.oMi[LEFT_JOINT_ID])
    err = pinocchio.log(dMi).vector
    if norm(err) < eps:
        success = True
        break
    if i >= IT_MAX:
        success = False
        break
    J = pinocchio.computeJointJacobian(model,data,q,LEFT_JOINT_ID)
    v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
    q = pinocchio.integrate(model,q,v*DT)
    if not i % 10:
        print('%d: error = %s' % (i, err.T))
    i += 1

while True:
    pinocchio.forwardKinematics(model,data,q)
    dMi = oMdes.actInv(data.oMi[RIGHT_JOINT_ID])
    err = pinocchio.log(dMi).vector
    if norm(err) < eps:
        success = True
        break
    if i >= IT_MAX:
        success = False
        break
    J = pinocchio.computeJointJacobian(model,data,q,RIGHT_JOINT_ID)
    v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
    q = pinocchio.integrate(model,q,v*DT)
    if not i % 10:
        print('%d: error = %s' % (i, err.T))
    i += 1

[stephane-caron]

You can find answers to your questions by looking at how the dual-arm robot is built up in the flying_dual_arm_ur3.py example linked above, specifically this part.

[GuoPingPan]

Thank you very much. But I found it is hard to install pink.