Support for more complex kinematic trees where not all active joints are used for inverse kinematics solving

[StoneT2000]

We have a common use case for our robot learning framework where we often define an extra "virtual" link connected via a fixed joint to some part of the robot, forming a separate path in the kinematic tree from root link to the virtual link compared to root link to e.g. finger links on a say franka panda arm, fetch arm, or widowx arm.

While using PseudoInverseIK I find that it cannot converge when trying to compute IK solutions based on goal poses of links in URDFs with non-linear kinematic trees with multiple leaves.

I think? I find the issue, and it is that PseudoInverseIK does not consider some form of joint position masking where one would mask out all active joints that do not affect the desired link. Below is 1 change to fix that.

https://github.com/UM-ARM-Lab/pytorch_kinematics/blob/master/src/pytorch_kinematics/ik.py#L323-L324

# before: J, m = self.chain.jacobian(q, ret_eef_pose=True)
q = q[self.active_ancestor_indexes] # maybe call this earlier in the file perhaps
J, m = self.chain.jacobian(q, ret_eef_pose=True)

and then the IKSolution object dof argument should be len(self.active_ancestor_indexes) instead of self.dof.

where self.active_ancestor_indexes is some precomputed torch tensor of indexes pointing to each joint index that is an ancestor of the link to do IK on.

I think this might also solve the issues #36 has, I'm guessing they have a convergence problem because their panda URDF has a gripper and they are doing IK on some virtual link that forms a non linear kinematic tree where there are active joints that are not the ancestor of that virtual link.

Example URDF with such a virtual link: https://github.com/haosulab/ManiSkill/blob/main/mani_skill/assets/robots/panda/panda_v2.urdf

[LemonPi]

Technically IK only supports SerialChains rather than arbitrary Chains, but you can create a SerialChain from any existing Chain by defining the end effector node (assuming they share the same root)

serial_chain = pk.SerialChain(chain, "end_frame_name")

In your case if the virtual link is not kinematically important, you can take pass in the serial chain with "panda_hand" or some other link as the end effector. I believe restricting consideration to that serial chain should give you all the active ancestors in it.

I haven't tried IK on non-serial chains so that explains why there's this issue in the first place. Does your solution provide additional capability that using a SerialChain subset does not? If not, I could probably add in better protection against passing in non-SerialChains

[StoneT2000]

Let me get back to you on it. I can try testing it again, perhaps I have some bug.

It would make sense if i could make a correct serial chain. I think I initially observed that the computed dof for a robot was 8 when only 6 ancestor joints were actually kinematically relevant when using the specific end link I wanted to do IK with.

[StoneT2000]

urdf = "/home/stao/.maniskill/data/robots/widowx/wx250s.urdf"
search_path = pybullet_data.getDataPath()
full_urdf = os.path.join(search_path, urdf)
chain = pk.build_serial_chain_from_urdf(open(full_urdf, mode="rb").read(), "ee_gripper_link")
chain = pk.SerialChain(chain, "ee_gripper_link", "base_link")
# ...
ik = pk.PseudoInverseIK(chain, max_iterations=30, num_retries=10,
                            joint_limits=lim.T,
                            early_stopping_any_converged=True,
                            early_stopping_no_improvement="all",
                            # line_search=pk.BacktrackingLineSearch(max_lr=0.2),
                            debug=False,
                            lr=0.2)

This is the code I am running now to replace the test inverse kinematics code.

However ik.dof and len(chain.get_joints()) is 8 when it should be 6 given the URDF I am using. The kinematically not important links are not the virtual links, its the 2 finger links/joints, which are still included in the chain somehow.

These are the links

['base_link', 'shoulder_link', 'upper_arm_link', 'upper_forearm_link', 'lower_forearm_link', 'wrist_link', 'gripper_link', 'ee_arm_link', 'gripper_prop_link', 'gripper_bar_link', 'fingers_link', 'left_finger_link', 'right_finger_link', 'ee_gripper_link']

But the tree looks like this:

['base_link', 'shoulder_link', 'upper_arm_link', 'upper_forearm_link', 'lower_forearm_link', 'wrist_link', 'gripper_link', 'ee_arm_link', 'gripper_prop_link', 'gripper_bar_link'] is all serial (and using these for end effector link all work).

Then

/
└── gripper_bar_link
    └── fingers_link
        ├── left_finger_link
        ├── right_finger_link
        └── ee_gripper_link

where left_finger_link, right_finger_link, and ee_gripper_link are all separate leafs in the tree, and fingers_link -> right/left_finger_link has an active joint in between.

[LemonPi]

I see, can you post wx250s.urdf so I can debug it locally? This should be a bug in the construction of the serial chain.

[StoneT2000]

widowx.zip

Attached here, thanks!

[LemonPi]

Hope you don't mind the wx250s urdf being one of the published test robots. Take a look at the new test_serial_chain_creation.

I haven't actually tested any IK on the wx250s - can you make some and let me know if there are problems? If possible, could you make a pull request for those IK tests afterwards?

[StoneT2000]

Yeah for sure, will make some tomorrow. Thank you for the quick response!