Skip to content
Snippets Groups Projects
Commit 6a0754df authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

black

parent 30494a40
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
python/example_robot_data/__main__.py
+ 2
− 2
View file @ 6a0754df
......@@ -5,8 +5,8 @@ from .robots_loader import ROBOTS, load
ROBOTS = sorted(ROBOTS.keys())
parser = ArgumentParser(description=load.__doc__)
parser.add_argument('robot', nargs='?', default=ROBOTS[0], choices=ROBOTS)
parser.add_argument('--no-display', action='store_true')
parser.add_argument("robot", nargs="?", default=ROBOTS[0], choices=ROBOTS)
parser.add_argument("--no-display", action="store_true")
args = parser.parse_args()
......
python/example_robot_data/robots_loader.py
+ 172
− 111
View file @ 6a0754df
......@@ -12,29 +12,46 @@ pin.switchToNumpyArray()
def getModelPath(subpath, printmsg=False):
source = dirname(dirname(dirname(__file__))) # top level source directory
paths = [
join(dirname(dirname(dirname(source))), 'robots'), # function called from "make release" in build/ dir
join(dirname(source), 'robots'), # function called from a build/ dir inside top level source
join(source, 'robots') # function called from top level source dir
join(
dirname(dirname(dirname(source))), "robots"
), # function called from "make release" in build/ dir
join(
dirname(source), "robots"
), # function called from a build/ dir inside top level source
join(source, "robots"), # function called from top level source dir
]
try:
from .path import EXAMPLE_ROBOT_DATA_MODEL_DIR, EXAMPLE_ROBOT_DATA_SOURCE_DIR
paths.append(EXAMPLE_ROBOT_DATA_MODEL_DIR) # function called from installed project
paths.append(EXAMPLE_ROBOT_DATA_SOURCE_DIR) # function called from off-tree build dir
paths.append(
EXAMPLE_ROBOT_DATA_MODEL_DIR
) # function called from installed project
paths.append(
EXAMPLE_ROBOT_DATA_SOURCE_DIR
) # function called from off-tree build dir
except ImportError:
pass
paths += [join(p, '../../../share/example-robot-data/robots') for p in sys.path]
paths += [join(p, "../../../share/example-robot-data/robots") for p in sys.path]
for path in paths:
if exists(join(path, subpath.strip('/'))):
if exists(join(path, subpath.strip("/"))):
if printmsg:
print("using %s as modelPath" % path)
return path
raise IOError('%s not found' % subpath)
raise IOError("%s not found" % subpath)
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
def readParamsFromSrdf(
model,
SRDF_PATH,
verbose=False,
has_rotor_parameters=True,
referencePose="half_sitting",
):
if has_rotor_parameters:
pin.loadRotorParameters(model, SRDF_PATH, verbose)
model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat))
model.armature = np.multiply(
model.rotorInertia.flat, np.square(model.rotorGearRatio.flat)
)
pin.loadReferenceConfigurations(model, SRDF_PATH, verbose)
q0 = pin.neutral(model)
if referencePose is not None:
......@@ -44,16 +61,16 @@ def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=Tru
class RobotLoader(object):
path = ''
urdf_filename = ''
srdf_filename = ''
sdf_filename = ''
sdf_root_link_name = ''
path = ""
urdf_filename = ""
srdf_filename = ""
sdf_filename = ""
sdf_root_link_name = ""
sdf_parent_guidance = []
urdf_subpath = 'robots'
srdf_subpath = 'srdf'
sdf_subpath = ''
ref_posture = 'half_sitting'
urdf_subpath = "robots"
srdf_subpath = "srdf"
sdf_subpath = ""
ref_posture = "half_sitting"
has_rotor_parameters = False
free_flyer = False
verbose = False
......@@ -68,8 +85,11 @@ class RobotLoader(object):
if self.model_path is None:
self.model_path = getModelPath(df_path, self.verbose)
self.df_path = join(self.model_path, df_path)
self.robot = builder(self.df_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
self.robot = builder(
self.df_path,
[join(self.model_path, "../..")],
pin.JointModelFreeFlyer() if self.free_flyer else None,
)
else:
df_path = join(self.path, self.sdf_subpath, self.sdf_filename)
try:
......@@ -77,30 +97,47 @@ class RobotLoader(object):
if self.model_path is None:
self.model_path = getModelPath(df_path, self.verbose)
self.df_path = join(self.model_path, df_path)
if tuple(int(i) for i in pin.__version__.split('.')) > (2, 9, 1):
self.robot = builder(self.df_path,
package_dirs=[join(self.model_path, '../..')],
root_joint=pin.JointModelFreeFlyer() if self.free_flyer else None,
root_link_name=self.sdf_root_link_name,
parent_guidance=self.sdf_parent_guidance)
if tuple(int(i) for i in pin.__version__.split(".")) > (2, 9, 1):
self.robot = builder(
self.df_path,
package_dirs=[join(self.model_path, "../..")],
root_joint=pin.JointModelFreeFlyer()
if self.free_flyer
else None,
root_link_name=self.sdf_root_link_name,
parent_guidance=self.sdf_parent_guidance,
)
else:
self.robot = builder(self.df_path,
package_dirs=[join(self.model_path, '../..')],
root_joint=pin.JointModelFreeFlyer() if self.free_flyer else None)
self.robot = builder(
self.df_path,
package_dirs=[join(self.model_path, "../..")],
root_joint=pin.JointModelFreeFlyer()
if self.free_flyer
else None,
)
except AttributeError:
raise ImportError("Building SDF models require pinocchio >= 3.0.0")
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
self.srdf_path = join(
self.model_path, self.path, self.srdf_subpath, self.srdf_filename
)
self.robot.q0 = readParamsFromSrdf(
self.robot.model,
self.srdf_path,
self.verbose,
self.has_rotor_parameters,
self.ref_posture,
)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
pin.removeCollisionPairs(
self.robot.model, self.robot.collision_model, self.srdf_path, False
)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
......@@ -126,7 +163,7 @@ class RobotLoader(object):
class A1Loader(RobotLoader):
path = 'a1_description'
path = "a1_description"
urdf_filename = "a1.urdf"
urdf_subpath = "urdf"
srdf_filename = "a1.srdf"
......@@ -135,7 +172,7 @@ class A1Loader(RobotLoader):
class ANYmalLoader(RobotLoader):
path = 'anymal_b_simple_description'
path = "anymal_b_simple_description"
urdf_filename = "anymal.urdf"
srdf_filename = "anymal.srdf"
ref_posture = "standing"
......@@ -143,7 +180,7 @@ class ANYmalLoader(RobotLoader):
class LaikagoLoader(RobotLoader):
path = 'laikago_description'
path = "laikago_description"
urdf_subpath = "urdf"
urdf_filename = "laikago.urdf"
free_flyer = True
......@@ -162,25 +199,34 @@ class BaxterLoader(RobotLoader):
class CassieLoader(RobotLoader):
path = 'cassie_description'
if tuple(int(i) for i in pin.__version__.split('.')) > (2, 9, 1):
path = "cassie_description"
if tuple(int(i) for i in pin.__version__.split(".")) > (2, 9, 1):
sdf_filename = "cassie.sdf"
else:
sdf_filename = "cassie_v2.sdf"
sdf_subpath = 'robots'
sdf_subpath = "robots"
srdf_filename = "cassie_v2.srdf"
ref_posture = "standing"
free_flyer = True
sdf_root_link_name = "pelvis"
sdf_parent_guidance = [
"left-roll-op", "left-yaw-op", "left-pitch-op", "left-knee-op", "left-tarsus-spring-joint", "left-foot-op",
"right-roll-op", "right-yaw-op", "right-pitch-op", "right-knee-op", "right-tarsus-spring-joint",
"right-foot-op"
"left-roll-op",
"left-yaw-op",
"left-pitch-op",
"left-knee-op",
"left-tarsus-spring-joint",
"left-foot-op",
"right-roll-op",
"right-yaw-op",
"right-pitch-op",
"right-knee-op",
"right-tarsus-spring-joint",
"right-foot-op",
]
class TalosLoader(RobotLoader):
path = 'talos_data'
path = "talos_data"
urdf_filename = "talos_reduced.urdf"
srdf_filename = "talos.srdf"
free_flyer = True
......@@ -188,7 +234,7 @@ class TalosLoader(RobotLoader):
class AsrTwoDofLoader(RobotLoader):
path = 'asr_twodof_description'
path = "asr_twodof_description"
urdf_filename = "TwoDofs.urdf"
urdf_subpath = "urdf"
......@@ -211,20 +257,29 @@ class TalosArmLoader(TalosLoader):
class TalosLegsLoader(TalosLoader):
def __init__(self):
super(TalosLegsLoader, self).__init__()
legMaxId = 14
m1 = self.robot.model
m2 = pin.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
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)
idx_q, idx_v = m2.joints[jid].idx_q, m2.joints[jid].idx_v
m2.upperPositionLimit[idx_q:idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
m2.lowerPositionLimit[idx_q:idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
m2.velocityLimit[idx_v:idx_v + j.nv] = m1.velocityLimit[j.idx_v:j.idx_v + j.nv]
m2.effortLimit[idx_v:idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
m2.upperPositionLimit[idx_q : idx_q + j.nq] = m1.upperPositionLimit[
j.idx_q : j.idx_q + j.nq
]
m2.lowerPositionLimit[idx_q : idx_q + j.nq] = m1.lowerPositionLimit[
j.idx_q : j.idx_q + j.nq
]
m2.velocityLimit[idx_v : idx_v + j.nv] = m1.velocityLimit[
j.idx_v : j.idx_v + j.nv
]
m2.effortLimit[idx_v : idx_v + j.nv] = m1.effortLimit[
j.idx_v : j.idx_v + j.nv
]
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
......@@ -255,10 +310,15 @@ class TalosLegsLoader(TalosLoader):
self.robot.visual_data = pin.GeometryData(g2)
# Load SRDF file
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters,
self.ref_posture)
assert (m2.armature[:6] == 0.).all()
self.robot.q0 = readParamsFromSrdf(
self.robot.model,
self.srdf_path,
self.verbose,
self.has_rotor_parameters,
self.ref_posture,
)
assert (m2.armature[:6] == 0.0).all()
# Add the free-flyer joint limits to the new model
self.addFreeFlyerJointLimits()
......@@ -272,7 +332,7 @@ class HyQLoader(RobotLoader):
class BoltLoader(RobotLoader):
path = 'bolt_description'
path = "bolt_description"
urdf_filename = "bolt.urdf"
srdf_filename = "bolt.srdf"
ref_posture = "standing"
......@@ -280,7 +340,7 @@ class BoltLoader(RobotLoader):
class Solo8Loader(RobotLoader):
path = 'solo_description'
path = "solo_description"
urdf_filename = "solo.urdf"
srdf_filename = "solo.srdf"
ref_posture = "standing"
......@@ -288,7 +348,6 @@ class Solo8Loader(RobotLoader):
class SoloLoader(Solo8Loader):
def __init__(self, *args, **kwargs):
warnings.warn('"solo" is deprecated, please try to load "solo8"')
return super(SoloLoader, self).__init__(*args, **kwargs)
......@@ -299,7 +358,7 @@ class Solo12Loader(Solo8Loader):
class FingerEduLoader(RobotLoader):
path = 'finger_edu_description'
path = "finger_edu_description"
urdf_filename = "finger_edu.urdf"
srdf_filename = "finger_edu.srdf"
ref_posture = "hanging"
......@@ -409,16 +468,16 @@ class RomeoLoader(RobotLoader):
class SimpleHumanoidLoader(RobotLoader):
path = 'simple_humanoid_description'
urdf_subpath = 'urdf'
urdf_filename = 'simple_humanoid.urdf'
srdf_filename = 'simple_humanoid.srdf'
path = "simple_humanoid_description"
urdf_subpath = "urdf"
urdf_filename = "simple_humanoid.urdf"
srdf_filename = "simple_humanoid.srdf"
free_flyer = True
class SimpleHumanoidClassicalLoader(SimpleHumanoidLoader):
urdf_filename = 'simple_humanoid_classical.urdf'
srdf_filename = 'simple_humanoid_classical.srdf'
urdf_filename = "simple_humanoid_classical.urdf"
srdf_filename = "simple_humanoid_classical.srdf"
class IrisLoader(RobotLoader):
......@@ -428,56 +487,58 @@ class IrisLoader(RobotLoader):
ROBOTS = {
'a1': A1Loader,
'anymal': ANYmalLoader,
'anymal_kinova': ANYmalKinovaLoader,
'asr_twodof': AsrTwoDofLoader,
'baxter': BaxterLoader,
'cassie': CassieLoader,
'double_pendulum': DoublePendulumLoader,
'double_pendulum_continuous': DoublePendulumContinuousLoader,
'double_pendulum_simple': DoublePendulumSimpleLoader,
'hector': HectorLoader,
'hyq': HyQLoader,
'icub': ICubLoader,
'icub_reduced': ICubReducedLoader,
'iris': IrisLoader,
'kinova': KinovaLoader,
'laikago': LaikagoLoader,
'panda': PandaLoader,
'romeo': RomeoLoader,
'simple_humanoid': SimpleHumanoidLoader,
'simple_humanoid_classical': SimpleHumanoidClassicalLoader,
'bolt': BoltLoader,
'solo': SoloLoader,
'solo8': Solo8Loader,
'solo12': Solo12Loader,
'finger_edu': FingerEduLoader,
'talos': TalosLoader,
'talos_box': TalosBoxLoader,
'talos_arm': TalosArmLoader,
'talos_legs': TalosLegsLoader,
'talos_full': TalosFullLoader,
'talos_full_box': TalosFullBoxLoader,
'tiago': TiagoLoader,
'tiago_dual': TiagoDualLoader,
'tiago_no_hand': TiagoNoHandLoader,
'ur3': UR5Loader,
'ur3_gripper': UR3GripperLoader,
'ur3_limited': UR3LimitedLoader,
'ur5': UR5Loader,
'ur5_gripper': UR5GripperLoader,
'ur5_limited': UR5LimitedLoader,
'ur10': UR10Loader,
'ur10_limited': UR10LimitedLoader,
"a1": A1Loader,
"anymal": ANYmalLoader,
"anymal_kinova": ANYmalKinovaLoader,
"asr_twodof": AsrTwoDofLoader,
"baxter": BaxterLoader,
"cassie": CassieLoader,
"double_pendulum": DoublePendulumLoader,
"double_pendulum_continuous": DoublePendulumContinuousLoader,
"double_pendulum_simple": DoublePendulumSimpleLoader,
"hector": HectorLoader,
"hyq": HyQLoader,
"icub": ICubLoader,
"icub_reduced": ICubReducedLoader,
"iris": IrisLoader,
"kinova": KinovaLoader,
"laikago": LaikagoLoader,
"panda": PandaLoader,
"romeo": RomeoLoader,
"simple_humanoid": SimpleHumanoidLoader,
"simple_humanoid_classical": SimpleHumanoidClassicalLoader,
"bolt": BoltLoader,
"solo": SoloLoader,
"solo8": Solo8Loader,
"solo12": Solo12Loader,
"finger_edu": FingerEduLoader,
"talos": TalosLoader,
"talos_box": TalosBoxLoader,
"talos_arm": TalosArmLoader,
"talos_legs": TalosLegsLoader,
"talos_full": TalosFullLoader,
"talos_full_box": TalosFullBoxLoader,
"tiago": TiagoLoader,
"tiago_dual": TiagoDualLoader,
"tiago_no_hand": TiagoNoHandLoader,
"ur3": UR5Loader,
"ur3_gripper": UR3GripperLoader,
"ur3_limited": UR3LimitedLoader,
"ur5": UR5Loader,
"ur5_gripper": UR5GripperLoader,
"ur5_limited": UR5LimitedLoader,
"ur10": UR10Loader,
"ur10_limited": UR10LimitedLoader,
}
def loader(name, display=False, rootNodeName=''):
def loader(name, display=False, rootNodeName=""):
"""Load a robot by its name, and optionnaly display it in a viewer."""
if name not in ROBOTS:
robots = ", ".join(sorted(ROBOTS.keys()))
raise ValueError("Robot '%s' not found. Possible values are %s" % (name, robots))
raise ValueError(
"Robot '%s' not found. Possible values are %s" % (name, robots)
)
inst = ROBOTS[name]()
if display:
if rootNodeName:
......@@ -489,12 +550,12 @@ def loader(name, display=False, rootNodeName=''):
return inst
def load(name, display=False, rootNodeName=''):
def load(name, display=False, rootNodeName=""):
"""Load a robot by its name, and optionnaly display it in a viewer."""
return loader(name, display, rootNodeName).robot
def load_full(name, display=False, rootNodeName=''):
def load_full(name, display=False, rootNodeName=""):
"""Load a robot by its name, optionnaly display it in a viewer, and provide its q0 and paths."""
inst = loader(name, display, rootNodeName)
return inst.robot, inst.robot.q0, inst.df_path, inst.srdf_path
unittest/test_load.py
+ 51
− 44
View file @ 6a0754df
......@@ -11,7 +11,6 @@ from example_robot_data import load_full
class RobotTestCase(unittest.TestCase):
def check(self, name, expected_nq, expected_nv, one_kg_bodies=[]):
"""Helper function for the real tests"""
robot, _, urdf, _ = load_full(name, display=False)
......@@ -33,132 +32,140 @@ class RobotTestCase(unittest.TestCase):
pybullet.disconnect(client_id)
def test_a1(self):
self.check('a1', 19, 18)
self.check("a1", 19, 18)
def test_anymal(self):
self.check('anymal', 19, 18)
self.check("anymal", 19, 18)
def test_anymal_kinova(self):
self.check('anymal_kinova', 25, 24)
self.check("anymal_kinova", 25, 24)
def test_baxter(self):
self.check('baxter', 19, 19)
self.check("baxter", 19, 19)
def test_cassie(self):
try:
self.check('cassie', 29, 28)
self.check("cassie", 29, 28)
except ImportError:
import pinocchio
self.assertLess(int(pinocchio.__version__.split('.')[0]), 3)
self.assertLess(int(pinocchio.__version__.split(".")[0]), 3)
def test_double_pendulum(self):
self.check('double_pendulum', 2, 2)
self.check("double_pendulum", 2, 2)
def test_double_pendulum_continuous(self):
self.check('double_pendulum_continuous', 4, 2)
self.check("double_pendulum_continuous", 4, 2)
def test_double_pendulum_simple(self):
self.check('double_pendulum_simple', 2, 2)
self.check("double_pendulum_simple", 2, 2)
def test_asr(self):
self.check('asr_twodof', 2, 2, one_kg_bodies=['ground'])
self.check("asr_twodof", 2, 2, one_kg_bodies=["ground"])
def test_hector(self):
self.check('hector', 7, 6)
self.check("hector", 7, 6)
def test_hyq(self):
self.check('hyq', 19, 18)
self.check("hyq", 19, 18)
def test_icub(self):
self.check('icub', 39, 38)
self.check("icub", 39, 38)
def test_icub_reduced(self):
self.check('icub_reduced', 36, 35)
self.check("icub_reduced", 36, 35)
def test_iris(self):
self.check('iris', 7, 6)
self.check("iris", 7, 6)
def test_kinova(self):
self.check('kinova', 9, 6)
self.check("kinova", 9, 6)
def test_panda(self):
self.check('panda', 9, 9)
self.check("panda", 9, 9)
def test_romeo(self):
self.check('romeo', 62, 61)
self.check("romeo", 62, 61)
def test_simple_humanoid(self):
self.check('simple_humanoid', 36, 35, one_kg_bodies=['LARM_LINK3', 'RARM_LINK3'])
self.check(
"simple_humanoid", 36, 35, one_kg_bodies=["LARM_LINK3", "RARM_LINK3"]
)
def test_simple_humanoid_classical(self):
self.check('simple_humanoid_classical', 36, 35, one_kg_bodies=['LARM_LINK3', 'RARM_LINK3'])
self.check(
"simple_humanoid_classical",
36,
35,
one_kg_bodies=["LARM_LINK3", "RARM_LINK3"],
)
def test_bolt(self):
self.check('bolt', 13, 12)
self.check("bolt", 13, 12)
def test_solo8(self):
self.check('solo8', 15, 14)
self.check("solo8", 15, 14)
def test_solo12(self):
self.check('solo12', 19, 18)
self.check("solo12", 19, 18)
def test_finger_edu(self):
self.check('finger_edu', 3, 3, one_kg_bodies=['finger_base_link'])
self.check("finger_edu", 3, 3, one_kg_bodies=["finger_base_link"])
def test_talos(self):
self.check('talos', 39, 38)
self.check("talos", 39, 38)
def test_laikago(self):
self.check('laikago', 19, 18)
self.check("laikago", 19, 18)
def test_talos_box(self):
self.check('talos_box', 39, 38)
self.check("talos_box", 39, 38)
def test_talos_full(self):
self.check('talos_full', 51, 50)
self.check("talos_full", 51, 50)
def test_talos_full_box(self):
self.check('talos_full_box', 51, 50)
self.check("talos_full_box", 51, 50)
def test_talos_arm(self):
self.check('talos_arm', 7, 7)
self.check("talos_arm", 7, 7)
def test_talos_legs(self):
self.check('talos_legs', 19, 18)
self.check("talos_legs", 19, 18)
def test_tiago(self):
self.check('tiago', 50, 48)
self.check("tiago", 50, 48)
def test_tiago_dual(self):
self.check('tiago_dual', 111, 101)
self.check("tiago_dual", 111, 101)
def test_tiago_no_hand(self):
self.check('tiago_no_hand', 14, 12)
self.check("tiago_no_hand", 14, 12)
def test_ur3(self):
self.check('ur3', 6, 6)
self.check("ur3", 6, 6)
def test_ur3_gripper(self):
self.check('ur3_gripper', 6, 6)
self.check("ur3_gripper", 6, 6)
def test_ur3_limited(self):
self.check('ur3_limited', 6, 6)
self.check("ur3_limited", 6, 6)
def test_ur5(self):
self.check('ur5', 6, 6)
self.check("ur5", 6, 6)
def test_ur5_gripper(self):
self.check('ur5_gripper', 6, 6)
self.check("ur5_gripper", 6, 6)
def test_ur5_limited(self):
self.check('ur5_limited', 6, 6)
self.check("ur5_limited", 6, 6)
def test_ur10(self):
self.check('ur10', 6, 6)
self.check("ur10", 6, 6)
def test_ur10_limited(self):
self.check('ur10_limited', 6, 6)
self.check("ur10_limited", 6, 6)
if __name__ == '__main__':
if __name__ == "__main__":
unittest.main()
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