From 70d0f48bdb4eda7ee315ae963dec3a5b2a8ddd9f Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Fri, 24 Jul 2020 19:06:54 +0200
Subject: [PATCH] [Python] robot_loader: infer SRDF_SUBPATH
---
python/example_robot_data/robots_loader.py | 57 +++++++++-------------
1 file changed, 23 insertions(+), 34 deletions(-)
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index a8c4df0..a0176d0 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -48,10 +48,10 @@ def addFreeFlyerJointLimits(model):
model.lowerPositionLimit = lb
-def robot_loader(urdf_filename,
- urdf_subpath,
+def robot_loader(path,
+ urdf_filename,
+ urdf_subpath=None,
srdf_filename=None,
- srdf_subpath=None,
verbose=False,
has_rotor_parameters=False,
ref_posture='half_sitting',
@@ -63,7 +63,7 @@ def robot_loader(urdf_filename,
pin.JointModelFreeFlyer() if free_flyer else None)
if srdf_filename is not None:
- robot.q0 = readParamsFromSrdf(robot.model, join(model_path, srdf_subpath, srdf_filename), verbose,
+ robot.q0 = readParamsFromSrdf(robot.model, join(model_path, path, 'srdf', srdf_filename), verbose,
has_rotor_parameters, ref_posture)
if free_flyer:
addFreeFlyerJointLimits(robot.model)
@@ -82,12 +82,11 @@ def loadANYmal(withArm=None):
REF_POSTURE = "standing_with_arm_up"
URDF_SUBPATH = "anymal_b_simple_description/robots"
- SRDF_SUBPATH = "anymal_b_simple_description/srdf"
- return robot_loader(URDF_FILENAME,
+ return robot_loader('anymal_b_simple_description',
+ URDF_FILENAME,
URDF_SUBPATH,
SRDF_FILENAME,
- SRDF_SUBPATH,
ref_posture=REF_POSTURE,
free_flyer=True)
@@ -96,18 +95,16 @@ def loadTalosArm():
URDF_FILENAME = "talos_left_arm.urdf"
SRDF_FILENAME = "talos.srdf"
URDF_SUBPATH = "talos_data/robots"
- SRDF_SUBPATH = "talos_data/srdf"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, SRDF_SUBPATH)
+ return robot_loader('talos_data', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME)
def loadTalos(legs=False):
URDF_FILENAME = "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
- SRDF_SUBPATH = "talos_data/srdf"
URDF_SUBPATH = "talos_data/robots"
- robot = robot_loader(URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, SRDF_SUBPATH, free_flyer=True)
+ robot = robot_loader('talos_data', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, free_flyer=True)
assert (robot.model.armature[:6] == 0.).all()
@@ -161,7 +158,7 @@ def loadTalos(legs=False):
# Load SRDF file
robot.q0 = robot.q0[:robot.model.nq]
model_path = getModelPath(join(URDF_SUBPATH, URDF_FILENAME))
- robot.q0 = readParamsFromSrdf(robot.model, join(model_path, SRDF_SUBPATH, SRDF_FILENAME), False)
+ 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
@@ -178,13 +175,12 @@ def loadTalosLegs():
def loadHyQ():
URDF_FILENAME = "hyq_no_sensors.urdf"
SRDF_FILENAME = "hyq.srdf"
- SRDF_SUBPATH = "hyq_description/srdf"
URDF_SUBPATH = "hyq_description/robots"
- return robot_loader(URDF_FILENAME,
+ return robot_loader('hyq_description',
+ URDF_FILENAME,
URDF_SUBPATH,
SRDF_FILENAME,
- SRDF_SUBPATH,
ref_posture="standing",
free_flyer=True)
@@ -195,13 +191,12 @@ def loadSolo(solo=True):
else:
URDF_FILENAME = "solo12.urdf"
SRDF_FILENAME = "solo.srdf"
- SRDF_SUBPATH = "solo_description/srdf"
URDF_SUBPATH = "solo_description/robots"
- return robot_loader(URDF_FILENAME,
+ return robot_loader('solo_description',
+ URDF_FILENAME,
URDF_SUBPATH,
SRDF_FILENAME,
- SRDF_SUBPATH,
ref_posture="standing",
free_flyer=True)
@@ -209,26 +204,23 @@ def loadSolo(solo=True):
def loadKinova():
URDF_FILENAME = "kinova.urdf"
SRDF_FILENAME = "kinova.srdf"
- SRDF_SUBPATH = "kinova_description/srdf"
URDF_SUBPATH = "kinova_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, SRDF_SUBPATH, ref_posture="arm_up")
+ return robot_loader('kinova_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, ref_posture="arm_up")
def loadTiago():
URDF_FILENAME = "tiago.urdf"
# SRDF_FILENAME = "tiago.srdf"
- # SRDF_SUBPATH = "tiago_description/srdf"
URDF_SUBPATH = "tiago_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH)
+ return robot_loader('tiago_description', URDF_FILENAME, URDF_SUBPATH)
def loadTiagoNoHand():
URDF_FILENAME = "tiago_no_hand.urdf"
# SRDF_FILENAME = "tiago.srdf"
- # SRDF_SUBPATH = "tiago_description/srdf"
URDF_SUBPATH = "tiago_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH)
+ return robot_loader('tiago_description', URDF_FILENAME, URDF_SUBPATH)
def loadICub(reduced=True):
@@ -237,17 +229,16 @@ def loadICub(reduced=True):
else:
URDF_FILENAME = "icub.urdf"
SRDF_FILENAME = "icub.srdf"
- SRDF_SUBPATH = "icub_description/srdf"
URDF_SUBPATH = "icub_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, SRDF_SUBPATH, free_flyer=True)
+ return robot_loader('icub_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, free_flyer=True)
def loadPanda():
URDF_FILENAME = "panda.urdf"
URDF_SUBPATH = "panda_description/urdf"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH)
+ return robot_loader('panda_description', URDF_FILENAME, URDF_SUBPATH)
def loadUR(robot=5, limited=False, gripper=False):
@@ -256,37 +247,35 @@ def loadUR(robot=5, limited=False, gripper=False):
URDF_SUBPATH = "ur_description/urdf"
if robot == 5 or robot == 3 and gripper:
SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
- SRDF_SUBPATH = "ur_description/srdf"
else:
SRDF_FILENAME = None
- SRDF_SUBPATH = None
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, SRDF_SUBPATH, ref_posture=None)
+ return robot_loader('ur_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, ref_posture=None)
def loadHector():
URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "hector_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
+ return robot_loader('hector_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
def loadDoublePendulum():
URDF_FILENAME = "double_pendulum.urdf"
URDF_SUBPATH = "double_pendulum_description/urdf"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH)
+ return robot_loader('double_pendulum_description', URDF_FILENAME, URDF_SUBPATH)
def loadRomeo():
URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "romeo_description/urdf"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
+ return robot_loader('romeo_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
def loadIris():
URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "iris_description/robots"
- return robot_loader(URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
+ return robot_loader('iris_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True)
--
GitLab