Commit 70d0f48b authored by Guilhem Saurel's avatar Guilhem Saurel

[Python] robot_loader: infer SRDF_SUBPATH

parent 8f215add
......@@ -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)
Markdown is supported
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