Adding joint from URDF to model

[arjung128]

Hi, I was able to load in the Stretch URDF after updating to the latest pinocchio version -- thanks so much for your help!

I am trying to obtain the position of "joint_gripper_fingertip_right" for inverse kinematics, which is a joint in the URDF file but not a joint in the model (i.e. it is not displayed under the list of joints when I look at model.hasConfigurationLimit). Is there a way to load this joint in from the URDF into the model such that it can be used during inverse kinematics?

Thanks in advance!

[fabinsch]

Hi @arjung128 ,
the joint joint_gripper_fingertip_right is a fixed joint in your URDF . In pinocchio, it is registered as a frame and you can access its transformation by doing.

In [1]: model, collision_model, visual_model = pin.buildModelsFromUrdf("./stretch.urdf")
In [2]: frame_id = model.getFrameId('joint_gripper_fingertip_right')
In [3]: model.frames[frame_id]
Out[11]: 
Frame name: joint_gripper_fingertip_right paired to (parent joint/ parent frame)(13/80)
with relative placement wrt parent joint:
  R =
    0.857474 -4.94049e-15    -0.514527
    0.514527   7.4829e-14     0.857474
 3.43059e-14           -1  6.66134e-14
  p = -0.190597    -0.015         0
containing inertia:
  m = 0.00382161
  c = 2.83786e-08 6.75132e-09  0.00812579
  I = 
0.001     0     0
    0 0.001     0
    0     0 0.001

Please check the examples and detailed tutorials, also there is a cheatsheet to give you an overview. You will have to use functions like pin.updateFramePlacements.

[stephane-caron]

Thanks @arjung128 for sharing the compiled Stretch URDF 😃 On a related note, here is one example on using Pinocchio to do task-based inverse kinematics with this description: mobile_stretch.py

stretch_example.mp4

[arjung128]

@fabinsch @stephane-caron This is incredibly helpful -- thanks so much!

When I try using line model.frames[frame_id], I keep getting the same output. Here's is my code, where I also add a block to visualize the fingertip location, which seems to be quite off:

import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
import numpy as np

model, geom_model, visual_model = pin.buildModelsFromUrdf('stretch/stretch-re2-2025/exported_urdf/stretch.urdf', 'stretch/stretch-re2-2025/exported_urdf/')
data, geom_data, visual_data = pin.createDatas(model, geom_model, visual_model)

viewer = meshcat.Visualizer()
viz = MeshcatVisualizer(model, geom_model, visual_model)
viz.initViewer(viewer)
viz.loadViewerModel(rootNodeName="stretch")

qpos = pin.randomConfiguration(model)
viz.display(np.array(qpos))

pin.forwardKinematics(model,data,qpos)
pin.framesForwardKinematics(model, data, qpos)
pin.updateFramePlacements(model, data)

frame_id = model.getFrameId('joint_gripper_fingertip_right')
print(model.frames[frame_id])

Which always prints out:

Frame name: joint_gripper_fingertip_right paired to (parent joint/ previous frame)(13/80)
with relative placement wrt parent joint:
  R =
    0.857474 -4.94049e-15    -0.514527
    0.514527   7.4829e-14     0.857474
 3.43059e-14           -1  6.66134e-14
  p = -0.190597    -0.015         0
containing inertia:
  m = 0.00382161
  c = 2.83786e-08 6.75132e-09  0.00812579
  I = 
0.001     0     0
    0 0.001     0
    0     0 0.001

This output doesn't seem to change even as qpos changes randomly. What am I doing wrong? Is there a way I can get this position of the right finger gripper in the world coordinates? Thanks!

[fabinsch]

You have to use data.oMf[frame_id]. The model never changes, just data changes. These are concepts that are explained in the tutorials and examples.

[arjung128]

@fabinsch Got it, thanks so much for your help! I really appreciate it.