URDF spherical joints

[iBims1JFK]

Hello, I got the following problem: I try to load a URDF file with a spherical joint. It seems that the URDF parser does not support spherical joints. What is the correct way then to import a model with spherical joints? I am using the pinocchio.buildModelFromUrdf(urdf_path) function.

Thank you for your help Greetings Jonathan

[jcarpent]

As the URDF does not support the Joint Spherical model, you should not use this kind of format. You can build a model from scratch either using Python or C++, or append models together by adding manually some spherical joints.

[jcarpent]

Which system do you have in mind?

[iBims1JFK]

Thank you for your very fast response. I want to control a spherical pendulum on a 4 DOF Barrett WAM. Therefore I got the URDF for the WAM and modified it, so that a pendulum is attached to the end. Pybullet is very confident in rendering that URDF. It is very unfortunate that the URDF reader is not capable of reading that file. It is possible to read that URDF file anyway and just attach that pendulum?

[fabinsch]

Hello, as mentioned by @jcarpent you should not modify the URDF for the WAM to include a spherical joint. Instead, you should load a first model from the original URDF, build another second model manually in pinocchio (in python using model = pin.Model(), jmodel = pin.JointModelSpherical, ..ect.., model.addJoint(parent_id, jmodel, joint_placement, joint_name) to have a pendulum with spherical joint and then join these two models into one by using the appendModel method.

[iBims1JFK]

Thank you for your reply. Is there any documentation about the python bindings for building your own models? At this point, I have not found anything.

[jcarpent]

See https://github.com/stack-of-tasks/pinocchio/blob/master/examples/simulation-pendulum.py

[fabinsch]

here you have an example loading an URDF model, building one from scratch and appending them.

from os.path import dirname, join, abspath
import math
import time 

import numpy as np

import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer as Visualizer
import hppfcl as fcl

# load model from example-robot urdf
pinocchio_model_dir= ("~/workspace/pinocchio/models")  # define the path accordingly 
model_path = join(pinocchio_model_dir, "example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "ur5_robot.urdf"
urdf_model_path = join(join(model_path, "ur_description/urdf/"), urdf_filename)

model1, collision_model1, visual_model1 = pin.buildModelsFromUrdf(
    urdf_model_path, package_dirs=mesh_dir)

# build model from scratch
model2 = pin.Model()
geom_model = pin.GeometryModel()

parent_id = 0
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 1e-2

joint_name = "joint_spherical" 
joint_id = model2.addJoint(parent_id,pin.JointModelSpherical(),joint_placement,joint_name)

body_inertia = pin.Inertia.FromSphere(body_mass,body_radius)
body_placement = joint_placement.copy()
body_placement.translation[2] = 0.1
model2.appendBodyToJoint(joint_id,body_inertia,body_placement)

geom1_name = "ball" 
shape1 = fcl.Sphere(body_radius)
geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1, body_placement)
geom1_obj.meshColor = np.ones((4))
geom_model.addGeometryObject(geom1_obj)

geom2_name = "bar" 
shape2 = fcl.Cylinder(body_radius/4.,body_placement.translation[2])
shape2_placement = body_placement.copy()
shape2_placement.translation[2] /= 2.

geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2, shape2_placement)
geom2_obj.meshColor = np.array([0.,0.,0.,1.])
geom_model.addGeometryObject(geom2_obj)

visual_model2 = geom_model

# join the two models, append pendulum to end effector
frame_id_end_effector = model1.getFrameId("tool0")
model, visual_model = pin.appendModel(
    model1, model2, visual_model1, visual_model2, frame_id_end_effector, pin.SE3.Identity())

print(f"Check the joints of the appended model:\n {model} \n ->Notice the spherical joint at the end.")
viz = Visualizer(model, visual_model, visual_model)

try:
    viz.initViewer(open=True)
except ImportError as err:
    print("Error while initializing the viewer. It seems you should install Python meshcat")
    print(err)
    sys.exit(0)

# Load the robot in the viewer.
viz.viewer.open()
viz.loadViewerModel()

# Display a random robot configuration.
model.lowerPositionLimit.fill(-math.pi/2)
model.upperPositionLimit.fill(+math.pi/2)
q = pin.randomConfiguration(model)
viz.display(q)
time.sleep(60)

[jcarpent]

Solved and enhanced via #1698.

[iBims1JFK]

Just another question, thank you for answering my last questions. The joint configuration got enhanced by 4 more dimensions after adding the spherical joint. I suppose that these 4 dimensions are quaternions. Could you tell me in which order they are? And is there any documentation about the python binding which I have not found yet.

Thank you for your time!

[jcarpent]

We use the same as Eigen for the convention (x,y,z,w). In Python, you can get the Quaternion object by simply:

import pinocchio as pin
quat = pin.Quaternion.Identity()

[fabinsch]

Hi @iBims1JFK , for questions like this it is very helpful to quickly check the docstring. In this case here:

ipython
import pinocchio as pin
pin.Quaternion?

yields all the necessary information:

Docstring:
Quaternion representing rotation.

Supported operations ('q is a Quaternion, 'v' is a Vector3): 'q*q' (rotation composition), 'q*=q', 'q*v' (rotating 'v' by 'q'), 'q==q', 'q!=q', 'q[0..3]'.
Init docstring:
__init__( (object)arg1, (object)R) -> object :
    Initialize from rotation matrix.
        R : a rotation matrix 3x3.

__init__( (object)arg1, (AngleAxis)aa) -> object :
    Initialize from an angle axis.
        aa: angle axis object.

__init__( (object)arg1, (Quaternion)quat) -> object :
    Copy constructor.
        quat: a quaternion.

__init__( (object)arg1, (object)u, (object)v) -> object :
    Initialize from two vectors u and v

__init__( (object)arg1, (object)vec4) -> object :
    Initialize from a vector 4D.
        vec4 : a 4D vector representing quaternion coefficients in the order xyzw.

__init__( (object)arg1) -> object :
    Default constructor

__init__( (object)arg1, (float)w, (float)x, (float)y, (float)z) -> object :
    Initialize from coefficients.

    ... note:: The order of coefficients is *w*, *x*, *y*, *z*. The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!
File:           ~/opt/anaconda3/envs/master_pinocchio/lib/python3.9/site-packages/pinocchio/pinocchio_pywrap.cpython-39-darwin.so
Type:           class
Subclasses:

[fabinsch]

We are working on a proper documentation of the python bindings, for now you can already check out the cheat sheet #1705.