diff --git a/pyrobolearn/simulators/README.md b/pyrobolearn/simulators/README.md index 4a62ad1..9752a81 100644 --- a/pyrobolearn/simulators/README.md +++ b/pyrobolearn/simulators/README.md @@ -7,7 +7,8 @@ Work is under progress for other simulators. import pyrobolearn as prl sim = prl.simulators.Bullet() -sim1 = prl.simulators.Dart() +sim1 = prl.simulators.BulletROS() +sim2 = prl.simulators.Dart() ``` #### What to check next? @@ -17,8 +18,11 @@ Check the `worlds` folder and the `robots` folder. #### TODOs - [x] implement Bullet interface -- [ ] implement Dart interface -- [ ] implement ROS_RBDL interface -- [ ] implement Gazebo_ROS interface +- [ ] implement BulletROS interface (ongoing) - [ ] implement Mujoco interface +- [ ] implement Isaac interface +- [ ] implement Dart interface +- [ ] implement RBDL_ROS interface +- [ ] implement GazeboROS interface +- [ ] implement V-REP (PyRep) interface - [ ] implement `simulator_randomizer` (similar to `physics_randomizer`) diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 06fab1a..2449086 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -35,7 +35,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["Bullet (Erwin Coumans and Yunfei Bai)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -317,14 +317,14 @@ class Bullet(Simulator): # pass def step(self, sleep_time=0.): - """Perform a step in the simulator. + """Perform a step in the simulator, and sleep the specified amount of time. "stepSimulation will perform all the actions in a single forward dynamics simulation step such as collision detection, constraint solving and integration. The default timestep is 1/240 second, it can be changed using the setTimeStep or setPhysicsEngineParameter API." [1] Args: - sleep_time (float): time to sleep after performing one step in the simulation. + sleep_time (float): amount of time to sleep after performing one step in the simulation. """ self.sim.stepSimulation() time.sleep(sleep_time) diff --git a/pyrobolearn/simulators/dart.py b/pyrobolearn/simulators/dart.py index 7e488b9..801c5a0 100644 --- a/pyrobolearn/simulators/dart.py +++ b/pyrobolearn/simulators/dart.py @@ -45,7 +45,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["DART", "PyDART", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" diff --git a/pyrobolearn/simulators/gazebo.py b/pyrobolearn/simulators/gazebo.py index 32e0399..c716874 100644 --- a/pyrobolearn/simulators/gazebo.py +++ b/pyrobolearn/simulators/gazebo.py @@ -21,7 +21,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["Gazebo", "ROS", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" diff --git a/pyrobolearn/simulators/isaac.py b/pyrobolearn/simulators/isaac.py index 371957a..418be22 100644 --- a/pyrobolearn/simulators/isaac.py +++ b/pyrobolearn/simulators/isaac.py @@ -26,7 +26,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["Nvidia Isaac", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -49,4 +49,5 @@ class Isaac(Simulator): """ def __init__(self, render=True, **kwargs): - super(Isaac, self).__init__() + super(Isaac, self).__init__(render=render) + raise NotImplementedError diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index a79660c..da33a33 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -11,19 +11,21 @@ Dependencies in PRL: * `pyrobolearn.simulators.simulator.Simulator` References: - [1] MuJoCo: http://www.mujoco.org/ - [2] MuJoCo Python: https://github.com/openai/mujoco-py - [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco + - [1] MuJoCo: http://www.mujoco.org/ + - [2] MuJoCo Python: https://github.com/openai/mujoco-py + - [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco """ # TODO # import mujoco_py as mujoco +# from dm_control import mujoco from pyrobolearn.simulators.simulator import Simulator + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["MuJoCo (Emo Todorov et al.)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -41,9 +43,9 @@ class Mujoco(Simulator): Warnings: The MuJoCo simulator requires a license in order to use it. References: - [1] MuJoCo: http://www.mujoco.org/ - [2] MuJoCo Python: https://github.com/openai/mujoco-py - [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco + - [1] MuJoCo: http://www.mujoco.org/ + - [2] MuJoCo Python: https://github.com/openai/mujoco-py + - [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco """ def __init__(self, render=True): diff --git a/pyrobolearn/simulators/opensim.py b/pyrobolearn/simulators/opensim.py index 8112122..e4642e9 100644 --- a/pyrobolearn/simulators/opensim.py +++ b/pyrobolearn/simulators/opensim.py @@ -22,7 +22,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["OpenSim (National Centers for Biomedical Computing at Stanford)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" diff --git a/pyrobolearn/simulators/raisim.py b/pyrobolearn/simulators/raisim.py index ec070c6..90202ef 100644 --- a/pyrobolearn/simulators/raisim.py +++ b/pyrobolearn/simulators/raisim.py @@ -28,7 +28,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["RaiSim (ETHz)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -130,10 +130,10 @@ class Raisim(Simulator): pass def step(self, sleep_time=0): - """Perform a step in the simulator, and sleep the specified time. + """Perform a step in the simulator, and sleep the specified amount of time. Args: - sleep_time (float): time to sleep after performing one step in the simulation. + sleep_time (float): amount of time to sleep after performing one step in the simulation. """ pass diff --git a/pyrobolearn/simulators/rbdl_.py b/pyrobolearn/simulators/rbdl_.py index 5dd3acc..2dc9442 100644 --- a/pyrobolearn/simulators/rbdl_.py +++ b/pyrobolearn/simulators/rbdl_.py @@ -18,7 +18,7 @@ import rbdl __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Martin Felis (martin@fysx.org)"] +__credits__ = ["Martin Felis (martin@fysx.org)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" diff --git a/pyrobolearn/simulators/rbdl_ros.py b/pyrobolearn/simulators/rbdl_ros.py new file mode 100644 index 0000000..d2d0e0a --- /dev/null +++ b/pyrobolearn/simulators/rbdl_ros.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +"""ROS-RBDL simulator + +This 'simulator' is not per se a simulator, it communicates with the real robots in the real world using ROS [1], and +computes any necessary kinematic and dynamics information using the RBDL library [2]. + +Specifically, this 'simulator' starts the `roscore` (if not already running), then loads robot urdf models and creates +the necessary topics/services, and uses the rigid body dynamics library to compute kinematic and dynamic information +about the model. + +Dependencies in PRL: +* `pyrobolearn.simulators.simulator.Simulator` + +References: + [1] ROS: http://www.ros.org/ + [2] RBDL: https://rbdl.bitbucket.io/ +""" + +# TODO + +import rospy +import rbdl + +from pyrobolearn.simulators.simulator import Simulator + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RBDL_ROS(Simulator): + r"""RBDL-ROS Interface. + + References: + [1] ROS: http://www.ros.org/ + [2] RBDL: https://rbdl.bitbucket.io/ + [3] RBDL in Python: https://rbdl.bitbucket.io/dd/dee/_python_example.html + """ + + def __init__(self, **kwargs): + super(RBDL_ROS, self).__init__(render=False) + raise NotImplementedError + + def step(self, sleep_time=0): + """Perform a step in the simulator, and sleep the specified amount of time. + + Args: + sleep_time (float): amount of time to sleep after performing one step in the simulation. + """ + pass + + def load_urdf(self, filename, position, orientation): + # load the model in rbdl + model = rbdl.loadModel(filename) diff --git a/pyrobolearn/simulators/ros.py b/pyrobolearn/simulators/ros.py index 8466ec3..1fa9197 100644 --- a/pyrobolearn/simulators/ros.py +++ b/pyrobolearn/simulators/ros.py @@ -30,7 +30,7 @@ from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] +__credits__ = ["ROS (Willow Garage)", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -38,18 +38,8 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class ROSModel(object): - r"""ROS Model - - """ - - def __init__(self, filename): - self.urdf = filename - # get ros services and ros topics from URDF - - # create - pass - +# TODO: maybe I should inherit from MiddleWare instead of Simulator... Then we can give these MiddleWare to different +# simulators. Other communication middleware layer includes YARP, etc. class ROS(Simulator): r"""ROS Interface diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 7fc2765..a36de20 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -111,6 +111,7 @@ class Simulator(object): GEOM_MESH = 5 GEOM_PLANE = 6 GEOM_CAPSULE = 7 + GEOM_CONE = 8 # NEW GUI = 1 GUI_MAIN_THREAD = 8 @@ -310,10 +311,10 @@ class Simulator(object): pass def step(self, sleep_time=0): - """Perform a step in the simulator, and sleep the specified time. + """Perform a step in the simulator, and sleep the specified amount of time. Args: - sleep_time (float): time to sleep after performing one step in the simulation. + sleep_time (float): amount of time to sleep after performing one step in the simulation. """ pass diff --git a/pyrobolearn/simulators/vrep.py b/pyrobolearn/simulators/vrep.py new file mode 100644 index 0000000..dc7713c --- /dev/null +++ b/pyrobolearn/simulators/vrep.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python +"""Define the V-REP Simulator API (using PyRep). + +This is the main interface that communicates with the V-REP simulator [1] through its Python wrapper PyRep [2]. +By defining this interface, it allows to decouple the PyRoboLearn framework from the simulator. It also converts some +data types to the ones required by PyRep. + +Warnings: + - You have to install V-REP beforehand. + - This only works with Python 3 + +Dependencies in PRL: +* `pyrobolearn.simulators.simulator.Simulator` + +References: + - [1] V-REP: http://www.coppeliarobotics.com/ + - [2] PyRep: https://github.com/stepjam/PyRep +""" + +# TODO: finish to implement this interface (use the pyrep / vrep interface) + +import time + +try: + import pyrep + from pyrep.objects.shape import Shape, PrimitiveShape + from pyrep.backend import vrep # this backend contains all the interesting methods +except ImportError as e: + raise ImportError("PyRep seems to not be installed on this machine; try to follow the installation instructions " + "in: \nhttps://github.com/stepjam/PyRep\n" + "Original error: " + str(e)) +except SyntaxError as e: + raise SyntaxError("PyRep only works with Python 3!! \n" + "Original error: " + str(e)) + + +from pyrobolearn.simulators.simulator import Simulator + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["V-REP (Coppelia Robotics)", "PyRep (James et al.)", "Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class VREP(Simulator): + r"""V-REP Simulator interface (using PyRep) + + This is the main interface that communicates with the V-REP simulator [1] through its Python wrapper PyRep [2]. + By defining this interface, it allows to decouple the PyRoboLearn framework from the simulator. It also converts + some data types to the ones required by PyRep. + + Warnings: + - You have to install V-REP beforehand. + - This only works with Python 3 + + References: + - [1] V-REP: http://www.coppeliarobotics.com/ + - [2] PyRep: https://github.com/stepjam/PyRep + """ + + def __init__(self, render=True): + super(VREP, self).__init__(render=render) + + # create simulator + self.sim = pyrep.PyRep() + + # launch simulator + scene = "" # "scene.ttt" + self.sim.launch(scene_file=scene, headless=self._render) + + # visual and collision shape ids + self.visual_shapes = dict() + self.collision_shapes = dict() + self.bodies = dict() + + # keep track of the number of ids + self.count_id = 0 + + # primitive shape mapping + self.primitive_shape_map = {self.GEOM_BOX: PrimitiveShape.CUBOID, self.GEOM_SPHERE: PrimitiveShape.SPHERE, + self.GEOM_CYLINDER: PrimitiveShape.CYLINDER} # self.GEOM_CONE: PrimitiveShape.CONE} + + raise NotImplementedError + + def close(self): + """Close the simulator.""" + self.sim.stop() # stop the simulation + self.sim.shutdown() # close the application + + def step(self, sleep_time=0): + """Perform a step in the simulator, and sleep the specified amount of time. + + Args: + sleep_time (float): amount of time to sleep after performing one step in the simulation. + """ + self.sim.step() + time.sleep(sleep_time) + + def get_time_step(self): + """Get the time step in the simulator. + + Returns: + float: time step in the simulator + """ + return vrep.simGetFloatParameter(vrep.sim_floatparam_simulation_time_step) + + def set_time_step(self, time_step): + """Set the time step in the simulator. + + Args: + time_step (float): Each time you call 'step' the time step will proceed with 'time_step'. + """ + # vrep.simSetFloatParameter(vrep.sim_floatparam_simulation_time_step, dt) + self.sim.set_simulation_timestep(time_step) + + def create_visual_shape(self, shape_type, radius=0.5, half_extents=(1., 1., 1.), length=1., filename=None, + mesh_scale=(1., 1., 1.), plane_normal=(0., 0., 1.), flags=-1, rgba_color=None, + specular_color=None, visual_frame_position=None, vertices=None, indices=None, uvs=None, + normals=None, visual_frame_orientation=None): + """Create a visual shape in the simulator.""" + + if shape_type not in self.primitive_shape_map: + raise NotImplementedError("The specified shape is currently not supported in this simulator") + + # compute size + size = (0, 0, 0) + if shape_type == self.GEOM_BOX: # if box + size = (half_extents[0]*2, half_extents[1]*2, half_extents[2]*2) + elif shape_type == self.GEOM_SPHERE: + size = (radius, radius, radius) + elif shape_type == self.GEOM_CYLINDER: + size = (radius, radius, length) + + # check color + if rgba_color is not None: + rgba_color = rgba_color[:3] + + # save the visual shape + self.count_id += 1 + self.visual_shapes[self.count_id] = {'type': self.primitive_shape_map[shape_type], 'size': size, + 'color': rgba_color} + return self.count_id + + def create_collision_shape(self, shape_type, radius=0.5, half_extents=(1., 1., 1.), height=1., filename=None, + mesh_scale=(1., 1., 1.), plane_normal=(0., 0., 1.), flags=-1, + collision_frame_position=None, collision_frame_orientation=None): + """Create a collision shape in the simulator.""" + + if shape_type not in self.primitive_shape_map: + raise NotImplementedError("The specified shape is currently not supported in this simulator") + + self.count_id += 1 + self.collision_shapes[self.count_id] = self.primitive_shape_map[shape_type] + return self.count_id + + def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.), + orientation=(0., 0., 0., 1.), *args, **kwargs): + """Create a body.""" + + # TODO: finish this + + if visual_shape_id in self.visual_shapes: + shape = self.visual_shapes[visual_shape_id] + shape = Shape.create(type=shape['type'], size=shape['size'], color=shape['color']) + shape.set_detectable() + shape.set_position(position) + shape.set_quaternion(orientation) + if collision_shape_id in self.collision_shapes: + if mass == 0: + shape.set_respondable(False) + else: + shape.set_mass(mass) + shape.set_collidable(True) + + # add body + self.count_id += 1 + self.bodies[self.count_id] = shape + return self.count_id + + raise ValueError("Visual shape id not known...") + + def remove_body(self, body_id): + """Remove a particular body in the simulator. + + Args: + body_id (int): unique body id. + """ + if body_id in self.bodies: + self.bodies[body_id].remove() + + def num_bodies(self): + """Return the number of bodies present in the simulator. + + Returns: + int: number of bodies + """ + return len(self.bodies) + + +# Tests +if __name__ == '__main__': + + # create simulator + sim = VREP() + + # create sphere like in pybullet + visual = sim.create_visual_shape(sim.GEOM_SPHERE) + collision = sim.create_collision_shape(sim.GEOM_SPHERE) + body = sim.create_body(visual_shape_id=visual, collision_shape_id=collision, mass=1, position=(0., 0., 0.), + orientation=(0., 0., 0., 1.)) + + for t in range(1000): + sim.step()