[Python] robot_loader: infer SRDF_SUBPATH

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)