def __init__(self, name=None, display=False):
self.visuals = [('universe', pin.SE3.Identity(),
pin.SE3.Identity().translation)]
self.name = self.__class__.__name__ if name is None else name
self.model = pin.Model()
self.display = display
self.add_joints()
def loadSoloLeg(solo8=True):
if solo8:
URDF_FILENAME = "solo.urdf"
legMaxId = 4
else:
URDF_FILENAME = "solo12.urdf"
legMaxId = 5
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = example_robot_data.getModelPath(URDF_SUBPATH)
robot = example_robot_data.loadSolo(solo8)
m1 = robot.model
m2 = pinocchio.Model()
for index, [j, M, name, parent, Y] in enumerate(
zip(m1.joints, m1.jointPlacements, m1.names, m1.parents,
m1.inertias)):
if j.id < legMaxId:
jointType = j.shortname()
if jointType == 'JointModelFreeFlyer':
# for the freeflyer we just take reduced mass and zero inertia
jointType = 'JointModelPZ'
Y.mass = Y.mass / 4
Y.inertia = np.diag(1e-6 * np.ones(3))
M = pinocchio.SE3.Identity()
if index == 2:
# start with the prismatic joint on the sliding guide
M.translation = np.zeros(3)
# 2D model, flatten y axis
vector2d = M.translation
vector2d[1] = 0.0
M.translation = vector2d
jid = m2.addJoint(parent, getattr(pinocchio, jointType)(), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < legMaxId:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
#q0 = example_robot_data.readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, 'standing')
robot.q0 = np.zeros(m2.nq + 1)
assert ((m2.rotorInertia[:m2.joints[1].nq] == 0.).all())
return robot
def createMonoped(nbJoint, linkLength=1.0, floatingMass=1.0, linkMass=1.0):
rmodel = pinocchio.Model()
prefix = ''
baseInertia = pinocchio.Inertia(floatingMass,
np.matrix([0.0, 0.0, 0.0]).T,
np.diagflat([1e-6, 1e-6, 1e-6]))
linkInertia = pinocchio.Inertia(linkMass,
np.matrix([0.0, 0.0, linkLength/2]).T,
linkMass/5*np.diagflat([1e-2, linkLength**2, 1e-2]))
# PRISMATIC JOINT
jointId = 0
jointPlacement = pinocchio.SE3.Identity()
jointName,bodyName = ["prismatic_joint", "mass"]
jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName)
rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity())
# REVOLUTE JOINTS
for i in range(1, nbJoint + 1):
jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)]
jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName)
rmodel.appendBodyToJoint(jointId,linkInertia, pinocchio.SE3.Identity())
rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT))
jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T)
rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0)
return rmodel
def test_add_joint(self):
model = pin.Model()
idx = 0
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1))
MAX_EFF = 100.
MAX_VEL = 10.
MIN_POS = -1.
MAX_POS = 1.
me = np.array([MAX_EFF])
mv = np.array([MAX_VEL])
lb = np.array([MIN_POS])
ub = np.array([MAX_POS])
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1), me, mv, lb, ub)
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.njoints, 3)
self.assertEqual(model.nq, 2)
self.assertEqual(model.nv, 2)
self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
def __init__(self):
super(TalosLegsLoader, self).__init__()
legMaxId = 14
m1 = self.robot.model
m2 = pin.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements,
m1.names, m1.parents, m1.inertias):
if j.id < legMaxId:
jid = m2.addJoint(parent,
getattr(pin, j.shortname())(), M, name)
upperPos = m2.upperPositionLimit
lowerPos = m2.lowerPositionLimit
effort = m2.effortLimit
upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v +
j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
m2.upperPositionLimit = upperPos
m2.lowerPositionLimit = lowerPos
m2.effortLimit = effort
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
upperPos = m2.upperPositionLimit
upperPos[:7] = 1
m2.upperPositionLimit = upperPos
lowerPos = m2.lowerPositionLimit
lowerPos[:7] = -1
m2.lowerPositionLimit = lowerPos
effort = m2.effortLimit
effort[:6] = np.inf
m2.effortLimit = effort
# q2 = self.robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pin.GeometryModel()
for g in self.robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
self.robot.model = m2
self.robot.data = m2.createData()
self.robot.visual_model = g2
# self.robot.q0=q2
self.robot.visual_data = pin.GeometryData(g2)
# Load SRDF file
self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path,
self.verbose, self.has_rotor_parameters,
self.ref_posture)
assert (m2.armature[:6] == 0.).all()
# Add the free-flyer joint limits to the new model
self.addFreeFlyerJointLimits()
def loadTalosLegs():
robot = loadTalos()
URDF_FILENAME = "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
legMaxId = 14
m1 = robot.model
m2 = pinocchio.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
m1.parents, m1.inertias):
if j.id < legMaxId:
jid = m2.addJoint(parent,
getattr(pinocchio, j.shortname())(), M, name)
up = m2.upperPositionLimit
down = m2.lowerPositionLimit
up[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
down[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
m2.upperPositionLimit = up
m2.lowerPositionLimit = down
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
u = m2.upperPositionLimit
u[:7] = 1
m2.upperPositionLimit = u
limit = m2.lowerPositionLimit
limit[:7] = -1
m2.lowerPositionLimit = limit
# q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
# robot.q0=q2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
robot.q0 = np.array(np.resize(robot.q0, robot.model.nq)).T
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((m2.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def __init__(self, name="whole-body MS model"):
# Pinocchio Model
self.model = se3.Model()
# Skelette Meshes
self.visuals = []
self.visuals.append([0, 'ground', 'none'])
self.forces = []
self.joint_transformations = []
self.name = name
def testTXT(self):
model = pin.buildSampleModelHumanoidRandom()
filename = main_path + "/model.txt"
model.saveToText(filename)
model2 = pin.Model()
model2.loadFromText(filename)
self.assertTrue(model == model2)
def testBIN(self):
model = pin.buildSampleModelHumanoidRandom()
filename = main_path + "/model.bin"
model.saveToBinary(filename)
model2 = pin.Model()
model2.loadFromBinary(filename)
self.assertTrue(model == model2)
def createPendulum(nbJoint, length=1.0, mass=1.0, viewer=None):
'''
Creates the Pinocchio kinematic <rmodel> and visuals <gmodel> models for
a N-pendulum.
@param nbJoint: number of joints <N> of the N-pendulum.
@param length: length of each arm of the pendulum.
@param mass: mass of each arm of the pendulum.
@param viewer: gepetto-viewer CORBA client. If not None, then creates the geometries
in the viewer.
'''
rmodel = pio.Model()
gmodel = pio.GeometryModel()
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
radius = 0.1 * length
prefix = ''
jointId = 0
jointPlacement = pio.SE3.Identity()
inertia = pio.Inertia(mass,
np.matrix([0.0, 0.0, length / 2]).T,
mass / 5 * np.diagflat([1e-2, length**2, 1e-2]))
viewerPrefix = getViewerPrefix(viewer, ["world", "pinocchio", "visuals"])
for i in range(nbJoint):
istr = str(i)
name = prefix + "joint" + istr
jointName, bodyName = [name + "_joint", name + "_body"]
jointId = rmodel.addJoint(jointId, pio.JointModelRY(), jointPlacement,
jointName)
rmodel.appendBodyToJoint(jointId, inertia, pio.SE3.Identity())
if viewer is not None:
viewer.addSphere(viewerPrefix + jointName, 1.5 * radius, colorred)
viewer.addCapsule(viewerPrefix + bodyName, radius, .8 * length,
color)
gmodel.addGeometryObject(
Capsule(jointName, jointId, pio.SE3.Identity(), 1.5 * radius, 0.0))
gmodel.addGeometryObject(
Capsule(bodyName, jointId,
pio.SE3(eye(3),
np.matrix([0., 0., length / 2]).T), radius,
0.8 * length))
jointPlacement = pio.SE3(eye(3), np.matrix([0.0, 0.0, length]).T)
rmodel.addFrame(
pio.Frame('tip', jointId, 0, jointPlacement, pio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.zeros(nbJoint) + 2 * np.pi
rmodel.lowerPositionLimit = np.zeros(nbJoint) - 2 * np.pi
rmodel.velocityLimit = np.zeros(nbJoint) + 5.0
return rmodel, gmodel
def __init__(self):
self.viewer = Display()
self.visuals = []
self.model = pin.Model()
self.createHand()
self.data = self.model.createData()
self.q0 = zero(self.model.nq)
# self.q0[3] = 1.0
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def testXML(self):
model = pin.buildSampleModelHumanoidRandom()
filename = main_path + "/model.xml"
tag_name = "Model"
model.saveToXML(filename, tag_name)
model2 = pin.Model()
model2.loadFromXML(filename, tag_name)
self.assertTrue(model == model2)
def __init__(self):
self.viewer = GepettoViewerServer()
self.visuals = []
self.model = se3.Model()
self.createHand()
self.data = self.model.createData()
self.q0 = zero(self.model.nq)
#self.q0[3] = 1.0
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def __init__(self, model=None, geom_model=None):
if model is None:
model = pin.Model()
if geom_model is None:
geom_model = pin.GeometryModel()
self._model = model
self._geom_model = geom_model
self._data = None
self._geom_data = None
self._clip_bounds = (None, None)
def test_xml(self):
with open(self.model_path) as model:
file_content = model.read()
model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
model = pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer())
self.assertEqual(model,model_ref)
model_self = pin.Model()
pin.buildModelFromXML(file_content,pin.JointModelFreeFlyer(),model_self)
self.assertEqual(model_self,model_ref)
def __init__(self, nbJoint=1):
'''Create a Pinocchio model of a N-pendulum, with N the argument <nbJoint>.'''
# self.viewer = Display()
self.visuals = []
self.model = pin.Model()
self.createPendulum(nbJoint)
self.data = self.model.createData()
self.q0 = zero(self.model.nq)
self.DT = 5e-2 # Step length
self.NDT = 2 # Number of Euler steps per integration (internal)
self.Kf = .10 # Friction coefficient
self.vmax = 8.0 # Max velocity (clipped if larger)
self.umax = 2.0 # Max torque (clipped if larger)
self.withSinCos = False # If true, state is [cos(q),sin(q),qdot], else [q,qdot]
def createSoloTB():
rmodel = pinocchio.Model()
prefix = ''
nbJoint = 2
floatingMass = 1.48538/4 # one quater of total base mass
linkLength = 0.16
firstLinkMass = 0.148538
secondLinkMass = 0.0376361
firstLinkCOMLength = 0.078707
secondLinkCOMLength = 0.102249
baseInertia = pinocchio.Inertia(floatingMass,
np.matrix([0.0, 0.0, 0.0]).T,
np.diagflat([1e-6, 1e-6, 1e-6]))
firstLinkInertia = pinocchio.Inertia(firstLinkMass,
np.matrix([0.0, 0.0, firstLinkCOMLength]).T,
firstLinkMass/3*np.diagflat([1e-6, firstLinkCOMLength**2, 1e-6]))
secondLinkInertia = pinocchio.Inertia(secondLinkMass,
np.matrix([0.0, 0.0, secondLinkCOMLength]).T,
secondLinkMass/3*np.diagflat([1e-6, secondLinkCOMLength**2, 1e-6]))
# PRISMATIC JOINT
jointId = 0
jointPlacement = pinocchio.SE3.Identity()
jointName,bodyName = ["prismatic_joint", "mass"]
jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName)
rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity())
# REVOLUTE JOINTS
for i in range(1, nbJoint + 1):
jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)]
jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName)
if i != nbJoint:
rmodel.appendBodyToJoint(jointId,firstLinkInertia, pinocchio.SE3.Identity())
else:
rmodel.appendBodyToJoint(jointId,secondLinkInertia, pinocchio.SE3.Identity())
rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT))
jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T)
rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME))
rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0)
rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0)
return rmodel
def __init__(self):
self.viewer = GepettoViewerServer()
#self.visuals = []
self.model = pio.Model()
self.gmodel = pio.GeometryModel()
self.createHand()
self.addCollisionPairs()
self.data = self.model.createData()
self.gdata = pio.GeometryData(self.gmodel)
self.gdata.collisionRequest.enable_contact=True
self.q0 = zero(self.model.nq)
self.q0[0] = np.pi
self.q0[-2] = -np.pi/3
self.q0[2:-4] = -np.pi/6
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def loadTalosLegs(modelPath='/opt/openrobots/share/example-robot-data'):
from pinocchio import JointModelFreeFlyer, JointModelRX, JointModelRY, JointModelRZ
robot = loadTalos(modelPath=modelPath)
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
legMaxId = 14
m1 = robot.model
m2 = pinocchio.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
m1.parents, m1.inertias):
if j.id < legMaxId:
jid = m2.addJoint(parent, locals()[j.shortname()](), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
m2.upperPositionLimit = np.matrix([1.] * 19).T
m2.lowerPositionLimit = np.matrix([-1.] * 19).T
#q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
# robot.q0=q2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
pinocchio.getNeutralConfiguration(m2, modelPath + SRDF_SUBPATH, False)
pinocchio.loadRotorParameters(m2, modelPath + SRDF_SUBPATH, False)
m2.armature = \
np.multiply(m2.rotorInertia.flat, np.square(m2.rotorGearRatio.flat))
assert ((m2.armature[:6] == 0.).all())
robot.q0 = m2.neutralConfiguration.copy()
return robot
def test_empty_model_sizes(self):
model = pin.Model()
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.nq, 0)
self.assertEqual(model.nv, 0)
def test_self_load(self):
model = pin.Model()
pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(),
model)
model_ref = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
import sys
import numpy as np
import pinocchio as pin
try:
import hppfcl
except ImportError:
print("This example requires hppfcl")
sys.exit(0)
from pinocchio.visualize import GepettoVisualizer
pin.switchToNumpyArray()
model = pin.Model()
geom_model = pin.GeometryModel()
geometries = [
hppfcl.Capsule(0.1, 0.8),
hppfcl.Sphere(0.5),
hppfcl.Box(1, 1, 1),
hppfcl.Cylinder(0.1, 1.0),
hppfcl.Cone(0.5, 1.0),
]
for i, geom in enumerate(geometries):
placement = pin.SE3(np.eye(3), np.array([i, 0, 0]))
geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
geom_model.addGeometryObject(geom_obj)
viz = GepettoVisualizer(
def loadTalos(legs=False, arm=False):
URDF_FILENAME = "talos_left_arm.urdf" if arm else "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=not arm)
assert (robot.model.armature[:6] == 0.).all()
if legs:
legMaxId = 14
m1 = robot.model
m2 = pin.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
if j.id < legMaxId:
jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name)
upperPos = m2.upperPositionLimit
lowerPos = m2.lowerPositionLimit
effort = m2.effortLimit
upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q +
j.nq]
lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q +
j.nq]
effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
m2.upperPositionLimit = upperPos
m2.lowerPositionLimit = lowerPos
m2.effortLimit = effort
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
upperPos = m2.upperPositionLimit
upperPos[:7] = 1
m2.upperPositionLimit = upperPos
lowerPos = m2.lowerPositionLimit
lowerPos[:7] = -1
m2.lowerPositionLimit = lowerPos
effort = m2.effortLimit
effort[:6] = np.inf
m2.effortLimit = effort
# q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pin.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
# robot.q0=q2
robot.visual_data = pin.GeometryData(g2)
# Load SRDF file
robot.q0 = robot.q0[:robot.model.nq]
model_path = getModelPath(join('talos_data/robots', URDF_FILENAME))
robot.q0 = readParamsFromSrdf(robot.model, join(model_path, 'talos_data/srdf', SRDF_FILENAME), False)
assert (m2.armature[:6] == 0.).all()
# Add the free-flyer joint limits to the new model
addFreeFlyerJointLimits(robot.model)
return robot