Forward kinematics running into recursion error for a robot with a prismatic joint (MWE provided).

[DanielTakeshi]

Context

I am testing the forward kinematics by building off of the example script here:

https://github.com/facebookresearch/theseus/blob/main/examples/inverse_kinematics.py

I am testing a UR5. I have two urdf files, and one has the UR5 base is attached to a linear slider so that it has a translation (prismatic) joint. So, the UR5 can translate its base along that linear slider, in addition to the usual revolute DOFs for the robot joints. Visually it looks something like this.

I have found that running the forward kinematics function from the get_forward_kinematics_fns will work for the UR5 without the prismatic joint. But it does not work when it has the prismatic joint.

Minimum Working Example (MWE)

Here is the minimum working example code to test:

"""Minimum working example for Theseus and forward kinematics."""
import os
import torch
torch.set_printoptions(sci_mode=False, linewidth=150)
from torchkin.forward_kinematics import Robot, get_forward_kinematics_fns
dtype = torch.float64

# First we load the URDF file describing the robot and create a `Robot` object to
# represent it in Python. The `Robot` class can be used to build a kinematics tree
# of the robot.
option = 3  # NOTE: pick option in {1, 2, 3}.
if option == 1:
    URDF_REL_PATH = (
        "../tests/theseus_tests/embodied/kinematics/data/panda_no_gripper.urdf"
    )
    link_names = ["panda_virtual_ee_link"]
elif option == 2:
    URDF_REL_PATH = (
        "../tests/theseus_tests/embodied/kinematics/data/ur5_no_translation_joint.urdf"
    )
    link_names = ["tool0"]
elif option == 3:
    URDF_REL_PATH = (
        "../tests/theseus_tests/embodied/kinematics/data/ur5_with_translation_joint.urdf"
    )
    link_names = ["tool0"]

urdf_path = os.path.join(os.path.dirname(__file__), URDF_REL_PATH)
robot = Robot.from_urdf_file(urdf_path, dtype)

# We can get differentiable forward kinematics functions for specific links
# by using `get_forward_kinematics_fns`. This function creates three differentiable
# functions for evaluating forward kinematics, body jacobian and spatial jacobian of
# the selected links, in that order. The return types of these functions are as
# follows:
#
# - fk: returns a tuple of link poses in the order of link names
# - jfk_b: returns a tuple where the first is a list of link body jacobians, and
#          the second is a tuple of link poses---both are in the order of link names
# - jfk_s: same as jfk_b except returning the spatial jacobians
fk, jfk_b, jfk_s = get_forward_kinematics_fns(robot, link_names)

# Generate target theta, then do forward kinematics to find the EE pose.
target_theta = torch.rand(10, robot.dof, dtype=dtype)
target_pose: torch.Tensor = fk(target_theta)[0]  # shape (10,3,4)
print(target_pose)

You will also need the URDF files.

ur5_no_translation_joint.urdf:
https://drive.google.com/file/d/1R64q5MBRptyD3C-NDBUSR6g5UZVcUdH2/view?usp=sharing

ur5_with_translation_joint.urdf:
https://drive.google.com/file/d/1jFdGAu42ectZudZ4XiBTEZt5dgNYuwsT/view?usp=sharing

These can be put in the same directory as the URDF of the Panda which Theseus provides:

(theseus) seita@takeshi:~/theseus/tests/theseus_tests/embodied/kinematics/data (main)$ ls -lh 
-rw-rw-r-- 1 seita seita 1.0K Jun 28 14:44 panda_fk_dataset.json
-rw-rw-r-- 1 seita seita 8.0K Jun 28 14:44 panda_no_gripper.urdf
-rw-rw-r-- 1 seita seita 9.1K Jul  3 11:27 ur5_no_translation_joint.urdf
-rw-rw-r-- 1 seita seita  11K Jul  3 11:27 ur5_with_translation_joint.urdf
(theseus) seita@takeshi:~/theseus/tests/theseus_tests/embodied/kinematics/data (main)$ 

Testing the MWE

To test, suppose the script above is named examples/FK_MWE.py. Then test while changing the option in the MWE, in this line specifically:

option = 3  # NOTE: pick option in {1, 2, 3}.

Values of 1 and 2 work in that the code does not error out and produces target poses, while a value of 3 results in this recursion depth error:

(theseus) seita@takeshi:~/theseus (main)$ python examples/FK_MWE.py 
Traceback (most recent call last):
  File "examples/FK_MWE.py", line 45, in <module>
    target_pose: torch.Tensor = fk(target_theta)[0]  # shape (10,3,4)
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/forward_kinematics.py", line 142, in forward_kinematics
    output = ForwardKinematics.apply(angles)
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torch/autograd/function.py", line 506, in apply
    return super().apply(*args, **kwargs)  # type: ignore[misc]
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/forward_kinematics.py", line 77, in forward
    poses = _forward_kinematics_helper(angles)
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/forward_kinematics.py", line 46, in _forward_kinematics_helper
    joint.relative_pose(angles[:, joint.id])
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/joint.py", line 381, in relative_pose
    return self._relative_pose_impl(angle)
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/joint.py", line 369, in _relative_pose_impl
    trans = self.translation(angle)
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/joint.py", line 366, in translation
    return self.translation(angle.view(-1, 1))
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/joint.py", line 366, in translation
    return self.translation(angle.view(-1, 1))
  File "/home/seita/miniconda3/envs/theseus/lib/python3.8/site-packages/torchkin/joint.py", line 366, in translation
    return self.translation(angle.view(-1, 1))
  [Previous line repeated 987 more times]
RecursionError: maximum recursion depth exceeded while calling a Python object
(theseus) seita@takeshi:~/theseus (main)$ 

One can increase the recursion depth, but that hasn't worked for me (I tested up to 3000) and raising it too much can be a bit dangerous.

Are prismatic joints supported for the forward kinematics? Thanks!

[mhmukadam]

Thanks for flagging this and providing the extensive details to reproduce it! @fantaosha can help with this in the next few days soon after he is back from break.

[fantaosha]

Hi @DanielTakeshi, thanks for your interests in Theseus. This is a bug and has been fixed in #568. Let us know if it works.

[DanielTakeshi]

Thank you @mhmukadam and @fantaosha , I just tested the code and yes it does work now.