diff --git a/python/example_robot_data/__main__.py b/python/example_robot_data/__main__.py index 1f066326a51d47c6da8d68c48e5d7ca1b20e0e10..2dc21f77edb1f1fc0d66dc34442955a2dfd2b8b8 100644 --- a/python/example_robot_data/__main__.py +++ b/python/example_robot_data/__main__.py @@ -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() diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index 818069038026ac49d4bac0ab89cd79dcc1a88579..4b7b15d3d862fa478678604412170c641a415352 100644 --- a/python/example_robot_data/robots_loader.py +++ b/python/example_robot_data/robots_loader.py @@ -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 diff --git a/unittest/test_load.py b/unittest/test_load.py index d8833fe5681629b0d65f2a4b66cf3a13c06a6404..b7687a3820ca3b13a12d3a07f754e0011e82437e 100755 --- a/unittest/test_load.py +++ b/unittest/test_load.py @@ -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()