diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index a0176d06de4c0bafbb025b85086bc7d62fd2d694..9dc623aaf572e1e515cf1c5394c99b1975f81f21 100644 --- a/python/example_robot_data/robots_loader.py +++ b/python/example_robot_data/robots_loader.py @@ -50,14 +50,14 @@ def addFreeFlyerJointLimits(model): def robot_loader(path, urdf_filename, - urdf_subpath=None, 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(urdf_subpath, urdf_filename) + 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) @@ -81,11 +81,8 @@ def loadANYmal(withArm=None): SRDF_FILENAME = "anymal-kinova.srdf" REF_POSTURE = "standing_with_arm_up" - URDF_SUBPATH = "anymal_b_simple_description/robots" - return robot_loader('anymal_b_simple_description', URDF_FILENAME, - URDF_SUBPATH, SRDF_FILENAME, ref_posture=REF_POSTURE, free_flyer=True) @@ -94,17 +91,15 @@ def loadANYmal(withArm=None): def loadTalosArm(): URDF_FILENAME = "talos_left_arm.urdf" SRDF_FILENAME = "talos.srdf" - URDF_SUBPATH = "talos_data/robots" - return robot_loader('talos_data', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME) + return robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME) def loadTalos(legs=False): URDF_FILENAME = "talos_reduced.urdf" SRDF_FILENAME = "talos.srdf" - URDF_SUBPATH = "talos_data/robots" - robot = robot_loader('talos_data', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, free_flyer=True) + robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=True) assert (robot.model.armature[:6] == 0.).all() @@ -157,7 +152,7 @@ def loadTalos(legs=False): # Load SRDF file robot.q0 = robot.q0[:robot.model.nq] - model_path = getModelPath(join(URDF_SUBPATH, URDF_FILENAME)) + 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()) @@ -175,14 +170,8 @@ def loadTalosLegs(): def loadHyQ(): URDF_FILENAME = "hyq_no_sensors.urdf" SRDF_FILENAME = "hyq.srdf" - URDF_SUBPATH = "hyq_description/robots" - return robot_loader('hyq_description', - URDF_FILENAME, - URDF_SUBPATH, - SRDF_FILENAME, - ref_posture="standing", - free_flyer=True) + return robot_loader('hyq_description', URDF_FILENAME, SRDF_FILENAME, ref_posture="standing", free_flyer=True) def loadSolo(solo=True): @@ -191,36 +180,27 @@ def loadSolo(solo=True): else: URDF_FILENAME = "solo12.urdf" SRDF_FILENAME = "solo.srdf" - URDF_SUBPATH = "solo_description/robots" - return robot_loader('solo_description', - URDF_FILENAME, - URDF_SUBPATH, - SRDF_FILENAME, - ref_posture="standing", - free_flyer=True) + return robot_loader('solo_description', URDF_FILENAME, SRDF_FILENAME, ref_posture="standing", free_flyer=True) def loadKinova(): URDF_FILENAME = "kinova.urdf" SRDF_FILENAME = "kinova.srdf" - URDF_SUBPATH = "kinova_description/robots" - return robot_loader('kinova_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, ref_posture="arm_up") + return robot_loader('kinova_description', URDF_FILENAME, SRDF_FILENAME, ref_posture="arm_up") def loadTiago(): URDF_FILENAME = "tiago.urdf" # SRDF_FILENAME = "tiago.srdf" - URDF_SUBPATH = "tiago_description/robots" - return robot_loader('tiago_description', URDF_FILENAME, URDF_SUBPATH) + return robot_loader('tiago_description', URDF_FILENAME) def loadTiagoNoHand(): URDF_FILENAME = "tiago_no_hand.urdf" # SRDF_FILENAME = "tiago.srdf" - URDF_SUBPATH = "tiago_description/robots" - return robot_loader('tiago_description', URDF_FILENAME, URDF_SUBPATH) + return robot_loader('tiago_description', URDF_FILENAME) def loadICub(reduced=True): @@ -229,53 +209,46 @@ def loadICub(reduced=True): else: URDF_FILENAME = "icub.urdf" SRDF_FILENAME = "icub.srdf" - URDF_SUBPATH = "icub_description/robots" - return robot_loader('icub_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, free_flyer=True) + return robot_loader('icub_description', URDF_FILENAME, SRDF_FILENAME, free_flyer=True) def loadPanda(): URDF_FILENAME = "panda.urdf" - URDF_SUBPATH = "panda_description/urdf" - return robot_loader('panda_description', URDF_FILENAME, URDF_SUBPATH) + return robot_loader('panda_description', URDF_FILENAME, urdf_subpath='urdf') def loadUR(robot=5, limited=False, gripper=False): 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" if robot == 5 or robot == 3 and gripper: SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '') else: SRDF_FILENAME = None - return robot_loader('ur_description', URDF_FILENAME, URDF_SUBPATH, SRDF_FILENAME, ref_posture=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" - return robot_loader('hector_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True) + return robot_loader('hector_description', URDF_FILENAME, free_flyer=True) def loadDoublePendulum(): URDF_FILENAME = "double_pendulum.urdf" - URDF_SUBPATH = "double_pendulum_description/urdf" - return robot_loader('double_pendulum_description', URDF_FILENAME, URDF_SUBPATH) + return robot_loader('double_pendulum_description', URDF_FILENAME, urdf_subpath='urdf') def loadRomeo(): URDF_FILENAME = "romeo.urdf" - URDF_SUBPATH = "romeo_description/urdf" - return robot_loader('romeo_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True) + return robot_loader('romeo_description', URDF_FILENAME, urdf_subpath='urdf', free_flyer=True) def loadIris(): URDF_FILENAME = "iris_simple.urdf" - URDF_SUBPATH = "iris_description/robots" - return robot_loader('iris_description', URDF_FILENAME, URDF_SUBPATH, free_flyer=True) + return robot_loader('iris_description', URDF_FILENAME, free_flyer=True)