diff --git a/examples/simulators/README.md b/examples/simulators/README.md index 0927ac1..7e3ce61 100644 --- a/examples/simulators/README.md +++ b/examples/simulators/README.md @@ -22,6 +22,9 @@ joint position values that were returned by the Bullet simulator on the correspo values from the ROS topics and change them in the simulator. This works with the `bullet_ros_publisher.py` code presented above. By moving the robot with your mouse in the publisher version, you will see the robot in this subscriber version moves in accordance with. This can be useful if you have access to the real platform as well. +4. `simulators.py`: example which loads few primitive shapes and the ANYmal robot using a simulator among `Bullet`, +`Mujoco`, `Raisim`, and `Dart`. Only the line `sim = (render=True)` needs to be changed. Note that +the integration of these other simulators is ongoing. Later, a `ROS`/`ROS_RBDL` "simulator" (without passing by a real simulator like `Bullet`) will allow you to make your code works on a real platform using ROS without changing any other lines of code. This is one of the big diff --git a/examples/simulators/figures/bullet.png b/examples/simulators/figures/bullet.png new file mode 100644 index 0000000..3614ddb Binary files /dev/null and b/examples/simulators/figures/bullet.png differ diff --git a/examples/simulators/figures/dart.png b/examples/simulators/figures/dart.png new file mode 100644 index 0000000..cade748 Binary files /dev/null and b/examples/simulators/figures/dart.png differ diff --git a/examples/simulators/figures/mujoco.png b/examples/simulators/figures/mujoco.png new file mode 100644 index 0000000..f291e07 Binary files /dev/null and b/examples/simulators/figures/mujoco.png differ diff --git a/examples/simulators/figures/raisim.png b/examples/simulators/figures/raisim.png new file mode 100644 index 0000000..4772d30 Binary files /dev/null and b/examples/simulators/figures/raisim.png differ diff --git a/examples/simulators/simulators.py b/examples/simulators/simulators.py new file mode 100644 index 0000000..ac79856 --- /dev/null +++ b/examples/simulators/simulators.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +"""Simulator tests. + +Example on how to load different things with the simulators. This example is still in an experimental phase. For +now, only Bullet is fully-supported. We are working on the other ones, especially the Mujoco simulator. +- Bullet: OK +- Raisim: OK (todo: for collision bodies, it only accepts OBJ files) +- MuJoCo: OK (todo: control still missing) +- DART: OK, but capsules don't have collision shapes... (todo: fix some URDFs) +- VREP: Not implemented yet + problem when importing PyRep with pybullet. Also, need to figure out how to call the + 'loadURDF' plugin. +- Isaac: not available yet. +""" + +import os +from itertools import count + +from pyrobolearn.simulators.bullet import Bullet +from pyrobolearn.simulators.raisim import Raisim +from pyrobolearn.simulators.dart import Dart +from pyrobolearn.simulators.mujoco import Mujoco +# from pyrobolearn.simulators.vrep import VREP # Problem when importing PyRep with Pybullet +# from pyrobolearn.simulators.isaac import Isaac # Not available yet + + +sim = Bullet(render=True) +# sim = Raisim(render=True) +# sim = Dart(render=True) +# sim = Mujoco(render=True) +# sim = VREP(render=True) +# sim = Isaac(render=True) +print("Gravity: {}".format(sim.get_gravity())) + +# load floor +floor = sim.load_floor(dimension=20) + +# create box +box = sim.create_primitive_object(sim.GEOM_BOX, position=(0, 0, 2), mass=1, rgba_color=(1, 0, 0, 1)) +sphere = sim.create_primitive_object(sim.GEOM_SPHERE, position=(2, 2, 2), mass=1, rgba_color=(0, 1, 0, 1)) +cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1) +capsule = sim.create_primitive_object(sim.GEOM_CAPSULE, position=(0, -2, 2), mass=1, rgba_color=(0, 0, 1, 1), + radius=0.5, height=0.5) + +# load robot +urdf_path = os.path.dirname(os.path.abspath(__file__)) + '/../../pyrobolearn/robots/urdfs/' +# path = urdf_path + 'rrbot/rrbot.urdf' +# path = urdf_path + 'jaco/jaco.urdf' +# path = urdf_path + 'kuka/kuka_iiwa/iiwa14.urdf' +# path = urdf_path + 'hyq2max/hyq2max.urdf' +path = urdf_path + 'anymal/anymal.urdf' +# path = urdf_path + 'centauro/centauro_stick.urdf' + +robot = sim.load_urdf(path, position=(3, -3, 2), use_fixed_base=False) + +# perform step +for t in count(): + sim.step(sleep_time=sim.dt) diff --git a/pyrobolearn/robots/centauro.py b/pyrobolearn/robots/centauro.py index 045ff1d..ca3142b 100644 --- a/pyrobolearn/robots/centauro.py +++ b/pyrobolearn/robots/centauro.py @@ -76,6 +76,13 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulator): self.hands = [self.get_link_ids(link) for link in ['arm1_8', 'arm2_8']] + # load joint configurations + srdf = os.path.dirname(__file__) + '/urdfs/centauro/centauro.srdf' + self.load_joint_configurations(srdf) + # print(self._joint_configuration.keys()) + joint_ids, joint_values = self._joint_configuration['home'] + self.reset_joint_states(q=joint_values, joint_ids=joint_ids) + # Test if __name__ == "__main__": @@ -97,8 +104,8 @@ if __name__ == "__main__": print("Number of Legs: {}".format(robot.num_legs)) print("Number of Arms: {}".format(robot.num_arms)) - robot.add_joint_slider(robot.right_front_leg) - robot.drive(speed=3) + # robot.add_joint_slider(robot.left_arm) + # robot.drive(speed=3) # run simulator for _ in count(): diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 3499bd7..ce1c009 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -14,6 +14,7 @@ import os import time import copy import collections +import xml.etree.ElementTree as ET # import rbdl import numpy as np # import quaternion @@ -59,7 +60,7 @@ class Robot(ControllableBody): """ def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1., visual_ticks=12, - *args, **kwargs): + parts=None, *args, **kwargs): """ Initialize the robot. @@ -71,6 +72,7 @@ class Robot(ControllableBody): fixed_base (bool, None): if True, the base of the robot will be fixed. scale (float): scaling factor. visual_ticks (int): the number of ticks to sleep before updating the visuals. + parts (list[Robot], None): robotic parts to assemble. # TODO: this needs to be implemented """ # check parameters if position is None: @@ -1431,6 +1433,10 @@ class Robot(ControllableBody): Return the joint positions for the home position defined by the user. This method has to be overwritten in the child class. """ + if 'home' in self._joint_configuration: + joint_ids, joint_values = self._joint_configuration['home'] + if len(joint_ids) == self.num_actuated_joints: + return joint_values return np.zeros(self.num_actuated_joints) def set_home_joint_positions(self): @@ -1476,7 +1482,7 @@ class Robot(ControllableBody): if name is None: return list(self._joint_configuration.keys()) if name in self._joint_configuration: - item = self._joint_configuration[name] + item = self._joint_configuration[name] # name.lower() if isinstance(item, str): # the item is an alias return self._joint_configuration[item] return item @@ -1495,6 +1501,32 @@ class Robot(ControllableBody): """ return name in self._joint_configuration + def load_joint_configurations(self, srdf): + """ + Load the joint configurations that are defined in the given SRDF file. + + Args: + srdf (str): path to the SRDF file which contains joint configurations with their corresponding name. + """ + if isinstance(srdf, str) and os.path.isfile(srdf): + tree_xml = ET.parse(srdf) + root = tree_xml.getroot() + + # parse tags + for group_state_tag in root.findall('group_state'): + name = group_state_tag.attrib['name'].lower() + + # parse each + joint_ids, joint_values = [], [] + for joint_tag in group_state_tag.findall('joint'): + values = [float(c) for c in joint_tag.attrib['value'].split()] + values = values[0] if len(values) == 1 else np.array(values) + joint_name = joint_tag.attrib['name'] + joint_ids.append(self.get_joint_ids(joint_name)) + joint_values.append(values) + + self._joint_configuration[name] = [joint_ids, np.asarray(joint_values)] + ################################## # Links (task/operational space) # ################################## @@ -2643,7 +2675,7 @@ class Robot(ControllableBody): Returns: np.array[float[4,4]],4]: homogeneous matrix """ - return get_homogeneous_transform(position, orientation) + return get_homogeneous_matrix(position, orientation) ############## # Kinematics # diff --git a/pyrobolearn/robots/urdfs/centauro/centauro.srdf b/pyrobolearn/robots/urdfs/centauro/centauro.srdf new file mode 100644 index 0000000..28dc06a --- /dev/null +++ b/pyrobolearn/robots/urdfs/centauro/centauro.srdf @@ -0,0 +1,1273 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pyrobolearn/robots/urdfs/coman/coman.urdf b/pyrobolearn/robots/urdfs/coman/coman.urdf index 3bd44d8..d2dbfcc 100644 --- a/pyrobolearn/robots/urdfs/coman/coman.urdf +++ b/pyrobolearn/robots/urdfs/coman/coman.urdf @@ -96,7 +96,7 @@ The coordinate frame called gaze defines the head position and orientation. The and to define the end effectors of the robot The coordinate frame called base_link is rigidly attached to the robot root body. It is recommended to choose the robot waist as its root body. The base_link can be attached to the root in any arbitrary position or orientation; for every hardware platform there will be a different place on the base that provides an obvious point of reference. Note that REP 103 [1] specifies a preferred orientation for frames. --> - - + - + @@ -66,7 +66,7 @@ - + @@ -97,7 +97,7 @@ - + @@ -128,7 +128,7 @@ - + @@ -183,7 +183,7 @@ - + @@ -214,7 +214,7 @@ - + @@ -245,7 +245,7 @@ - + @@ -300,7 +300,7 @@ - + @@ -331,7 +331,7 @@ - + @@ -362,7 +362,7 @@ - + @@ -417,7 +417,7 @@ - + @@ -448,7 +448,7 @@ - + @@ -479,7 +479,7 @@ - + @@ -534,7 +534,7 @@ - + @@ -565,7 +565,7 @@ - + @@ -596,7 +596,7 @@ - + @@ -651,7 +651,7 @@ - + @@ -682,7 +682,7 @@ - + @@ -713,7 +713,7 @@ - + diff --git a/pyrobolearn/robots/urdfs/hyq2max/hyq2max.urdf b/pyrobolearn/robots/urdfs/hyq2max/hyq2max.urdf index 5b53fd7..ace16cd 100644 --- a/pyrobolearn/robots/urdfs/hyq2max/hyq2max.urdf +++ b/pyrobolearn/robots/urdfs/hyq2max/hyq2max.urdf @@ -36,7 +36,7 @@ is the world frame). For more, see http://www.ros.org/wiki/xacro --> - + - + diff --git a/pyrobolearn/robots/urdfs/jaco/jaco.urdf b/pyrobolearn/robots/urdfs/jaco/jaco.urdf index 5eec70b..371568c 100644 --- a/pyrobolearn/robots/urdfs/jaco/jaco.urdf +++ b/pyrobolearn/robots/urdfs/jaco/jaco.urdf @@ -5,7 +5,7 @@ diff --git a/pyrobolearn/robots/urdfs/littledog/littleDog.urdf b/pyrobolearn/robots/urdfs/littledog/littleDog.urdf index 8d9b63e..7914d4e 100644 --- a/pyrobolearn/robots/urdfs/littledog/littleDog.urdf +++ b/pyrobolearn/robots/urdfs/littledog/littleDog.urdf @@ -1,6 +1,7 @@ - + @@ -72,7 +73,7 @@ - + @@ -106,7 +107,7 @@ + + + + -