Skip to content
Snippets Groups Projects
Commit ba762fbe authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] import pinocchio as pin 

which is the standard way of using pinocchio, according to its examples
and unittests.
parent 153adbef
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
python/example_robot_data/robots_loader.py
+ 18
− 26
View file @ ba762fbe
...@@ -2,10 +2,10 @@ import sys ...@@ -2,10 +2,10 @@ import sys
from os.path import dirname, exists, join from os.path import dirname, exists, join
import numpy as np import numpy as np
import pinocchio import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper from pinocchio.robot_wrapper import RobotWrapper
pinocchio.switchToNumpyArray() pin.switchToNumpyArray()
def getModelPath(subpath, printmsg=False): def getModelPath(subpath, printmsg=False):
...@@ -33,10 +33,10 @@ def getVisualPath(modelPath): ...@@ -33,10 +33,10 @@ def getVisualPath(modelPath):
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'): def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
if has_rotor_parameters: if has_rotor_parameters:
pinocchio.loadRotorParameters(model, SRDF_PATH, verbose) pin.loadRotorParameters(model, SRDF_PATH, verbose)
model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat)) model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(model, SRDF_PATH, verbose) pin.loadReferenceConfigurations(model, SRDF_PATH, verbose)
q0 = pinocchio.neutral(model) q0 = pin.neutral(model)
if referencePose is not None: if referencePose is not None:
q0 = model.referenceConfigurations[referencePose].copy() q0 = model.referenceConfigurations[referencePose].copy()
return q0 return q0
...@@ -64,8 +64,7 @@ def loadANYmal(withArm=None): ...@@ -64,8 +64,7 @@ def loadANYmal(withArm=None):
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE) robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
# Add the free-flyer joint limits # Add the free-flyer joint limits
...@@ -94,8 +93,7 @@ def loadTalos(): ...@@ -94,8 +93,7 @@ def loadTalos():
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False) robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all()) assert ((robot.model.armature[:6] == 0.).all())
...@@ -114,10 +112,10 @@ def loadTalosLegs(): ...@@ -114,10 +112,10 @@ def loadTalosLegs():
legMaxId = 14 legMaxId = 14
m1 = robot.model m1 = robot.model
m2 = pinocchio.Model() m2 = pin.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
if j.id < legMaxId: if j.id < legMaxId:
jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name) jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name)
upperPos = m2.upperPositionLimit upperPos = m2.upperPositionLimit
lowerPos = m2.lowerPositionLimit lowerPos = m2.lowerPositionLimit
effort = m2.effortLimit effort = m2.effortLimit
...@@ -128,7 +126,7 @@ def loadTalosLegs(): ...@@ -128,7 +126,7 @@ def loadTalosLegs():
m2.lowerPositionLimit = lowerPos m2.lowerPositionLimit = lowerPos
m2.effortLimit = effort m2.effortLimit = effort
assert (jid == j.id) assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity()) m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
upperPos = m2.upperPositionLimit upperPos = m2.upperPositionLimit
upperPos[:7] = 1 upperPos[:7] = 1
...@@ -145,7 +143,7 @@ def loadTalosLegs(): ...@@ -145,7 +143,7 @@ def loadTalosLegs():
if f.parent < legMaxId: if f.parent < legMaxId:
m2.addFrame(f) m2.addFrame(f)
g2 = pinocchio.GeometryModel() g2 = pin.GeometryModel()
for g in robot.visual_model.geometryObjects: for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14: if g.parentJoint < 14:
g2.addGeometryObject(g) g2.addGeometryObject(g)
...@@ -154,7 +152,7 @@ def loadTalosLegs(): ...@@ -154,7 +152,7 @@ def loadTalosLegs():
robot.data = m2.createData() robot.data = m2.createData()
robot.visual_model = g2 robot.visual_model = g2
# robot.q0=q2 # robot.q0=q2
robot.visual_data = pinocchio.GeometryData(g2) robot.visual_data = pin.GeometryData(g2)
# Load SRDF file # Load SRDF file
robot.q0 = robot.q0[:robot.model.nq] robot.q0 = robot.q0[:robot.model.nq]
...@@ -173,8 +171,7 @@ def loadHyQ(): ...@@ -173,8 +171,7 @@ def loadHyQ():
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing") robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits # Add the free-flyer joint limits
...@@ -192,8 +189,7 @@ def loadSolo(solo=True): ...@@ -192,8 +189,7 @@ def loadSolo(solo=True):
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing") robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits # Add the free-flyer joint limits
...@@ -250,8 +246,7 @@ def loadICub(reduced=True): ...@@ -250,8 +246,7 @@ def loadICub(reduced=True):
URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False) robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False)
# Add the free-flyer joint limits # Add the free-flyer joint limits
...@@ -285,8 +280,7 @@ def loadHector(): ...@@ -285,8 +280,7 @@ def loadHector():
URDF_FILENAME = "quadrotor_base.urdf" URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
return robot return robot
...@@ -302,14 +296,12 @@ def loadRomeo(): ...@@ -302,14 +296,12 @@ def loadRomeo():
URDF_FILENAME = "romeo.urdf" URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
def loadIris(): def loadIris():
URDF_FILENAME = "iris_simple.urdf" URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)], pin.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
return robot return robot
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment