diff --git a/cmake b/cmake
index d63b949baa72cae06bad7497d3fcb35a9c7e124c..fb4c22c319ec5320f9a85527eb1a4130954846f5 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit d63b949baa72cae06bad7497d3fcb35a9c7e124c
+Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index f0fa302461131b77800d5d5d97d54b89a298d5c8..630fa9c0c62a9b7e4dda20bc05671c2060827cce 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -1,11 +1,13 @@
import sys
+import warnings
from os.path import dirname, exists, join
import numpy as np
-import pinocchio
+
+import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper
-pinocchio.switchToNumpyArray()
+pin.switchToNumpyArray()
def getModelPath(subpath, printmsg=False):
@@ -27,16 +29,12 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % subpath)
-def getVisualPath(modelPath):
- return join(modelPath, '../..')
-
-
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
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))
- pinocchio.loadReferenceConfigurations(model, SRDF_PATH, verbose)
- q0 = pinocchio.neutral(model)
+ pin.loadReferenceConfigurations(model, SRDF_PATH, verbose)
+ q0 = pin.neutral(model)
if referencePose is not None:
q0 = model.referenceConfigurations[referencePose].copy()
return q0
@@ -51,6 +49,29 @@ def addFreeFlyerJointLimits(model):
model.lowerPositionLimit = lb
+def robot_loader(path,
+ urdf_filename,
+ srdf_filename=None,
+ urdf_subpath='robots',
+ verbose=False,
+ has_rotor_parameters=False,
+ ref_posture='half_sitting',
+ free_flyer=False):
+ """Helper function to load a robot."""
+ urdf_path = join(path, urdf_subpath, urdf_filename)
+ model_path = getModelPath(urdf_path)
+ robot = RobotWrapper.BuildFromURDF(join(model_path, urdf_path), [join(model_path, '../..')],
+ pin.JointModelFreeFlyer() if free_flyer else None)
+
+ if srdf_filename is not None:
+ robot.q0 = readParamsFromSrdf(robot.model, join(model_path, path, 'srdf', srdf_filename), verbose,
+ has_rotor_parameters, ref_posture)
+ if free_flyer:
+ addFreeFlyerJointLimits(robot.model)
+
+ return robot
+
+
def loadANYmal(withArm=None):
if withArm is None:
URDF_FILENAME = "anymal.urdf"
@@ -60,256 +81,149 @@ def loadANYmal(withArm=None):
URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = "standing_with_arm_up"
- URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
- SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
- return robot
+ return robot_loader('anymal_b_simple_description',
+ URDF_FILENAME,
+ SRDF_FILENAME,
+ ref_posture=REF_POSTURE,
+ free_flyer=True)
-def loadTalosArm():
- URDF_FILENAME = "talos_left_arm.urdf"
- URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
- SRDF_FILENAME = "talos.srdf"
- SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
- return robot
+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)
-def 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)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
- assert ((robot.model.armature[:6] == 0.).all())
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
return robot
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)
- 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, pinocchio.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 = 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 = robot.q0[:robot.model.nq]
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
-
- assert ((m2.armature[:6] == 0.).all())
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
- return robot
+ warnings.warn("`loadTalosLegs()` is deprecated. Please use `loadTalos(legs=True)`", DeprecationWarning, 2)
+ return loadTalos(legs=True)
+
+
+def loadTalosArm():
+ warnings.warn("`loadTalosArm()` is deprecated. Please use `loadTalos(arm=True)`", DeprecationWarning, 2)
+ return loadTalos(arm=True)
def loadHyQ():
- URDF_FILENAME = "hyq_no_sensors.urdf"
- SRDF_FILENAME = "hyq.srdf"
- SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
- return robot
+ return robot_loader('hyq_description', "hyq_no_sensors.urdf", "hyq.srdf", ref_posture="standing", free_flyer=True)
def loadSolo(solo=True):
- if solo:
- URDF_FILENAME = "solo.urdf"
- else:
- URDF_FILENAME = "solo12.urdf"
- SRDF_FILENAME = "solo.srdf"
- SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
- return robot
+ return robot_loader('solo_description',
+ "solo.urdf" if solo else "solo12.urdf",
+ "solo.srdf",
+ ref_posture="standing",
+ free_flyer=True)
def loadKinova():
- URDF_FILENAME = "kinova.urdf"
- SRDF_FILENAME = "kinova.srdf"
- SRDF_SUBPATH = "/kinova_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "arm_up")
- return robot
+ return robot_loader('kinova_description', "kinova.urdf", "kinova.srdf", ref_posture="arm_up")
-def loadTiago():
- URDF_FILENAME = "tiago.urdf"
- # SRDF_FILENAME = "tiago.srdf"
- # SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
- # Load SRDF file
- # robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
- return robot
+def loadTiago(hand=True):
+ return robot_loader('tiago_description', "tiago.urdf" if hand else "tiago_no_hand.urdf")
def loadTiagoNoHand():
- URDF_FILENAME = "tiago_no_hand.urdf"
- # SRDF_FILENAME = "tiago.srdf"
- # SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
- # Load SRDF file
- # robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
- return robot
+ warnings.warn("`loadTiagoNoHand()` is deprecated. Please use `loadTiago(hand=False)`", DeprecationWarning, 2)
+ return loadTiago(hand=False)
def loadICub(reduced=True):
- if reduced:
- URDF_FILENAME = "icub_reduced.urdf"
- else:
- URDF_FILENAME = "icub.urdf"
- SRDF_FILENAME = "icub.srdf"
- SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
- URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- # Load URDF file
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- # Load SRDF file
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False)
- # Add the free-flyer joint limits
- addFreeFlyerJointLimits(robot.model)
- return robot
+ return robot_loader('icub_description',
+ "icub_reduced.urdf" if reduced else "icub.urdf",
+ "icub.srdf",
+ free_flyer=True)
def loadPanda():
- URDF_FILENAME = "panda.urdf"
- URDF_SUBPATH = "/panda_description/urdf/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
-
- return robot
+ return robot_loader('panda_description', "panda.urdf", urdf_subpath='urdf')
def loadUR(robot=5, limited=False, gripper=False):
- assert (not (gripper and (robot == 10 or limited)))
+ assert not (gripper and (robot == 10 or limited))
URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
- URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
if robot == 5 or robot == 3 and gripper:
SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
- SRDF_SUBPATH = "/ur_description/srdf/" + SRDF_FILENAME
- robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, None)
- return robot
+ else:
+ SRDF_FILENAME = None
+
+ return robot_loader('ur_description', URDF_FILENAME, SRDF_FILENAME, urdf_subpath='urdf', ref_posture=None)
def loadHector():
- URDF_FILENAME = "quadrotor_base.urdf"
- URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- return robot
+ return robot_loader('hector_description', "quadroto_base.urdf", free_flyer=True)
def loadDoublePendulum():
- URDF_FILENAME = "double_pendulum.urdf"
- URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
- return robot
+ return robot_loader('double_pendulum_description', "double_pendulum.urdf", urdf_subpath='urdf')
def loadRomeo():
- URDF_FILENAME = "romeo.urdf"
- URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
+ return robot_loader('romeo_description', "romeo.urdf", urdf_subpath='urdf', free_flyer=True)
def loadIris():
- URDF_FILENAME = "iris_simple.urdf"
- URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
- modelPath = getModelPath(URDF_SUBPATH)
- robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
- pinocchio.JointModelFreeFlyer())
- return robot
+ return robot_loader('iris_description', "iris_simple.urdf", free_flyer=True)
diff --git a/unittest/test_load.py b/unittest/test_load.py
index 5ad3655c0eb811c0a158532b0cbdad27d1e61cd4..0b3c96eb1be70ca77490c1a988f344df2c2fe57c 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -48,19 +48,19 @@ class TalosTest(RobotTestCase):
class TalosArmTest(RobotTestCase):
- RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
+ RobotTestCase.ROBOT = example_robot_data.loadTalos(arm=True)
RobotTestCase.NQ = 7
RobotTestCase.NV = 7
class TalosArmFloatingTest(RobotTestCase):
- RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
+ RobotTestCase.ROBOT = example_robot_data.loadTalos(arm=True)
RobotTestCase.NQ = 14
RobotTestCase.NV = 13
class TalosLegsTest(RobotTestCase):
- RobotTestCase.ROBOT = example_robot_data.loadTalosLegs()
+ RobotTestCase.ROBOT = example_robot_data.loadTalos(legs=True)
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
@@ -90,7 +90,7 @@ class TiagoTest(RobotTestCase):
class TiagoNoHandTest(RobotTestCase):
- RobotTestCase.ROBOT = example_robot_data.loadTiagoNoHand()
+ RobotTestCase.ROBOT = example_robot_data.loadTiago(hand=False)
RobotTestCase.NQ = 14
RobotTestCase.NV = 12