pin.JointModelPlanar() 4th joint deforms robot

[arjung128]

Hello,

It seemed to me that the 4th joint when you include pin.JointModelPlanar() represented the rotation of the base. However, the larger the magnitude of this value, the more deformed the robot seems to get:

import meshcat
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer

model_base, geom_model_base, visual_model_base = pin.buildModelsFromUrdf('stretch/stretch-re2-2025/exported_urdf/stretch.urdf', 'stretch/stretch-re2-2025/exported_urdf/', pin.JointModelPlanar())
data_base, geom_data_base, visual_data_base = pin.createDatas(model_base, geom_model_base, visual_model_base)

viewer = meshcat.Visualizer()
viz = MeshcatVisualizer(model_base, geom_model_base, visual_model_base)
viz.initViewer(viewer)
viz.loadViewerModel(rootNodeName="stretch")

init_qpos = pin.neutral(model_base)
viz.display(init_qpos)

pin.forwardKinematics(model_base, data_base, init_qpos)
pin.framesForwardKinematics(model_base, data_base, init_qpos)
print(data_base.oMf[model_base.getFrameId('joint_gripper_fingertip_right')])

This looks normal. But when I change the 4th joint, the robot is deformed (visually):

init_qpos = pin.neutral(model_base)
init_qpos[3] = 1.
viz.display(init_qpos)

pin.forwardKinematics(model_base, data_base, init_qpos)
pin.framesForwardKinematics(model_base, data_base, init_qpos)
print(data_base.oMf[model_base.getFrameId('joint_gripper_fingertip_right')])

The print statements also reveal that the arm becomes longer as a result of the deformation. The effects are even more obvious when the magnitude is increased further, e.g. init_qpos[3] = 5.

Is this the intended behavior? If so, is there a way to obtain a model of the robot where the rotation of the base is represented as one parameter (unlike pin.JointModelFreeFlyer() where the rotation of the base is represented by four parameters) which does not deform the robot?

Any help will be much appreciated, thanks in advance!

[jcarpent]

The JointModelPlanar uses a vector with four components to depict the configuration of the joint. The two first components correspond to the $x$ and $y$ position variables, while the two last correspond to the $\cos(\theta)$ and $\sin(\theta)$ related to the orientation of the joint. We use this convention as the planar joint is supposed to do an infinite turn on itself (we do not count the number of turns), which is better represented as $(\cos(\theta),\sin(\theta))$, similarly to quaternion for 3d orientations.

Shortly speaking, if you consider an orientation $\theta$, you should use:

init_qpos = pin.neutral(model_base)
init_qpos[2] = math.cos(theta)
init_qpos[3] = math.sin(theta)

[arjung128]

This is exactly what I was looking for. Thanks so much for all your help! :)