diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 959e431..03f62b8 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -18,15 +18,19 @@ References: [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ +# general imports +import os +import inspect import time import numpy as np import quaternion +# import pybullet import pybullet import pybullet_data from pybullet_envs.bullet.bullet_client import BulletClient -from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter +# import PRL simulator from pyrobolearn.simulators.simulator import Simulator @@ -77,22 +81,30 @@ class Bullet(Simulator): Erwin Coumans and Yunfei Bai, 2017/2018 """ - def __init__(self, render=True, **kwargs): # , converter=None): + def __init__(self, render=True, **kwargs): super(Bullet, self).__init__() # parse the kwargs # Connect to pybullet - if render: - self.sim = BulletClient(connection_mode=pybullet.GUI) + if render: # GUI + self.connection_mode = pybullet.GUI + self.sim = BulletClient(connection_mode=self.connection_mode) self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_GUI, 0) - else: - self.sim = BulletClient(connection_mode=pybullet.DIRECT) + else: # without GUI + self.connection_mode = pybullet.DIRECT + self.sim = BulletClient(connection_mode=self.connection_mode) + + # set simulator ID self.id = self.sim._client # add additional search path when loading URDFs, SDFs, MJCFs, etc. self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) + # TODO: add gazebo_models path + + self.models = {} + # go through the global variables / attributes defined in pybullet and set them here # this includes for instance: JOINT_REVOLUTE, POSITION_CONTROL, etc. # for attribute in dir(pybullet): @@ -136,6 +148,24 @@ class Bullet(Simulator): # Simulators # ############## + def __init(self, connection_mode): + """Initialize the simulator with the specified connection mode.""" + # close the previous simulator + if self.sim is not None: + self.close() + + # initialize the simulator (create it, set its id, and set the path to the models) + # self.sim.connect(connection_mode) + self.sim = BulletClient(connection_mode=connection_mode) + self.id = self.sim._client + self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) + + # reload the models + models = self.models.copy() + for idx in models: + kwargs = models[idx] + self.load_urdf(**kwargs) + def reset(self): """Reset the simulator. @@ -145,10 +175,11 @@ class Bullet(Simulator): def close(self): """Close the simulator.""" - try: - self.sim.disconnect(physicsClientId=self.id) - except pybullet.error: - pass + del self.sim + # try: + # self.sim.disconnect(physicsClientId=self.id) + # except pybullet.error: + # pass def step(self, sleep_time=0.): """Perform a step in the simulator. @@ -170,9 +201,30 @@ class Bullet(Simulator): flag (bool): If True, it will render the simulator by enabling the GUI. """ if flag: - self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 1) + # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 1) + if self.connection_mode == pybullet.DIRECT: + # save the state of the simulator + filename = 'PYROBOLEARN_RENDERING_STATE.bullet' + self.save(filename=filename) + # change the connection mode + self.connection_mode = pybullet.GUI + self.__init(self.connection_mode) + # load the state of the world in the simulator + self.load(filename) + os.remove(filename) else: - self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 0) + # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 0) + if self.connection_mode == pybullet.GUI: + # save the state of the simulator + filename = 'PYROBOLEARN_RENDERING_STATE.bullet' + self.save(filename=filename) + # change the connection mode + self.connection_mode = pybullet.DIRECT + self.__init(self.connection_mode) + # load the state of the world in the simulator + self.load(filename) + os.remove(filename) + # TODO: reset the camera def set_time_step(self, time_step): """Set the specified time step in the simulator. @@ -532,7 +584,11 @@ class Bullet(Simulator): if scale is not None: kwargs['globalScaling'] = scale - return self.sim.loadURDF(filename, **kwargs) + model_id = self.sim.loadURDF(filename, **kwargs) + frame = inspect.currentframe() + args, _, _, values = inspect.getargvalues(frame) + self.models[model_id] = {arg: values[arg] for arg in args[1:]} + return model_id def load_sdf(self, filename, scaling=1., *args, **kwargs): """Load the given SDF file. @@ -569,6 +625,11 @@ class Bullet(Simulator): """ Load a mesh in the world (only available in the simulator). + Warnings (see https://github.com/bulletphysics/bullet3/issues/1813): + - it only accepts wavefront obj files + - wavefront obj files can have at most 1 texture + - there is a limited pre-allocated memory for visual meshes + Args: filename (str): path to file for the mesh. Currently, only Wavefront .obj. It will create convex hulls for each object (marked as 'o') in the .obj file. @@ -616,6 +677,69 @@ class Bullet(Simulator): return mesh + @staticmethod + def _get_3d_models(extension, fullpath=False): + """Return the list of 3d models (urdf, sdf, mjcf/xml, obj). + + Args: + extension (str): extension of the 3D models (urdf, sdf, mjcf/xml, obj). + fullpath (bool): If True, it will return the full path to the 3D objects. If False, it will just return + the name of the files (without the extension). + """ + extension = '.' + extension + path = pybullet_data.getDataPath() + results = [] + for dir_path, dir_names, filenames in os.walk(path): + for filename in filenames: + if os.path.splitext(filename)[1] == extension: + if fullpath: + results.append(os.path.join(dir_path, filename)) # append the fullpath + else: + results.append(filename[:-len(extension)]) # remove extension + return results + + @staticmethod + def get_available_sdfs(fullpath=False): + """Return the list of available SDFs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the SDFs. If False, it will just return the + name of the SDF files (without the extension). + """ + return Bullet._get_3d_models(extension='sdf', fullpath=fullpath) + + @staticmethod + def get_available_urdfs(fullpath=False): + """Return the list of available URDFs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the URDFs. If False, it will just return the + name of the URDF files (without the extension). + """ + return Bullet._get_3d_models(extension='urdf', fullpath=fullpath) + + @staticmethod + def get_available_mjcfs(fullpath=False): + """Return the list of available MJCFs (=XMLs) from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the MJCFs/XMLs. If False, it will just return + the name of the MJCF/XML files (without the extension). + """ + results1 = Bullet._get_3d_models(extension='mjcf', fullpath=fullpath) + results2 = Bullet._get_3d_models(extension='xml', fullpath=fullpath) + return results1 + results2 + + @staticmethod + def get_available_objs(fullpath=False): + """Return the list of available OBJs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the OBJs. If False, it will just return the + name of the OBJ files (without the extension). + """ + return Bullet._get_3d_models(extension='obj', fullpath=fullpath) + ########## # Bodies # ########## diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 163342a..7dd5589 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -406,6 +406,46 @@ class Simulator(object): """ pass + @staticmethod + def get_available_sdfs(fullpath=False): + """Return the list of available SDFs in the simulator. + + Args: + fullpath (bool): If True, it will return the full path to the SDFs. If False, it will just return the + name of the SDF files (without the extension). + """ + return [] + + @staticmethod + def get_available_urdfs(fullpath=False): + """Return the list of available URDFs in the simulator. + + Args: + fullpath (bool): If True, it will return the full path to the URDFs. If False, it will just return the + name of the URDF files (without the extension). + """ + return [] + + @staticmethod + def get_available_mjcfs(fullpath=False): + """Return the list of available MJCFs in the simulator. + + Args: + fullpath (bool): If True, it will return the full path to the MJCFs. If False, it will just return the + name of the MJCF files (without the extension). + """ + return [] + + @staticmethod + def get_available_objs(fullpath=False): + """Return the list of available OBJs in the simulator. + + Args: + fullpath (bool): If True, it will return the full path to the OBJs. If False, it will just return the + name of the OBJ files (without the extension). + """ + return [] + # bodies def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.), diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index e0fbb87..cb27f36 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -447,14 +447,14 @@ class World(object): for joint_id, position, velocity in zip(robot.joints, positions, velocities): self.sim.reset_joint_state(robot_id, joint_id, position, velocity) - def load_urdf(self, filename, position, orientation, fixed_base=False, scale=1., name=None): + def load_urdf(self, filename, position, orientation=(0, 0, 0, 1), fixed_base=False, scale=1., name=None): """ Load URDF specified by the given path. This is basically a wrapper around the simulator's `load_urdf` method. Args: filename (str): path to the URDF file position (float[3]): position of the object described in the URDF - orientation (float[4]): orientation represented as a quaternion + orientation (float[4]): orientation represented as a quaternion [x,y,z,w] fixed_base (bool): if the base of the object should be fixed or not scale (float): scale factor for the object name (str, None): name of the object. If None, it will extract it from the URDF. @@ -511,6 +511,42 @@ class World(object): raise ValueError('Extension name of the file is not known; this method only accepts URDF/SDF files.') return object_id + def get_available_sdfs(self, fullpath=False): + """Return the list of available SDFs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the SDFs. If False, it will just return the + name of the SDF files (without the extension). + """ + return self.sim.get_available_sdfs(fullpath=fullpath) + + def get_available_urdfs(self, fullpath=False): + """Return the list of available URDFs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the URDFs. If False, it will just return the + name of the URDF files (without the extension). + """ + return self.sim.get_available_urdfs(fullpath=fullpath) + + def get_available_mjcfs(self, fullpath=False): + """Return the list of available MJCFs (=XMLs) from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the MJCFs/XMLs. If False, it will just return + the name of the MJCF/XML files (without the extension). + """ + return self.sim.get_available_mjcfs(fullpath=fullpath) + + def get_available_objs(self, fullpath=False): + """Return the list of available OBJs from the `pybullet_data.getDataPath()` method. + + Args: + fullpath (bool): If True, it will return the full path to the OBJs. If False, it will just return the + name of the OBJ files (without the extension). + """ + return self.sim.get_available_objs(fullpath=fullpath) + def load_object(self, object_type, path=None, position=(0, 0, 0), orientation=(0, 0, 0, 1), scaling=1.): """ Load the specified object. This is a method that allows you to quickly load stuffs however it is less @@ -1102,7 +1138,7 @@ class World(object): def create_city(self): pass - def load_table(self, position, orientation=(0, 0, 0, 1), scaling=1.): + def load_table(self, position, orientation=None, scaling=1.): """ Load a table in the world. @@ -1613,6 +1649,21 @@ class World(object): angular_damping=angular_damping, contact_stiffness=contact_stiffness, contact_damping=contact_damping) + def apply_texture(self, texture, body_id, link_id=-1): + """ + Apply the texture to the given object. + + Args: + texture (str): path to the texture. + body_id (int): unique body id. + link_id (int): link id. If -1, it will be the base. + + Returns: + + """ + texture = self.sim.load_texture(texture) + self.sim.change_visual_shape(object_id=body_id, link_id=link_id, texture_id=texture) + class BasicWorld(World): r"""Basic World class. @@ -1667,19 +1718,19 @@ if __name__ == '__main__': sim = BulletSim() # create world - # world = BasicWorld(sim) - world = World(sim) + world = BasicWorld(sim) + # world = World(sim) # world.load_bot_lab() # load meshes - world.load_mesh('utils/terrains/terrain_map.obj', - position=[0, 0, -2], - orientation=[.707, 0, 0, .707], - mass=0., - scale=(.1, .1, .1), - # color=[1, 0, 0, 1], - flags=1) - # world.load_visual_mesh('meshes/cube_color.dae', position=[0, 0, 1]) + # world.load_mesh('utils/terrains/terrain_map.obj', + # position=[0, 0, -2], + # orientation=[.707, 0, 0, .707], + # mass=0., + # scale=(.1, .1, .1), + # # color=[1, 0, 0, 1], + # flags=1) + # world.load_mesh('cube.obj', position=[0, 0, 2], scale=(.1, .1, .1), flags=0) # world.load_mesh('bedroom.obj', [0, 0, 0], mass=0., color=[0.4, 0.4, 0.4, 1], flags=1) #, scale=(0.01, 0.01, 0.01)) # world.load_mesh('mtsthelens.obj', [0, 0, -8], mass=0., color=[0.2, 0.5, 0.2, 1], flags=1, scale=(0.01,0.01,0.01)) # world.load_mesh('meshes/terrain.obj', [0,0,0], mass=0., color=[1,1,1,1], flags=1) @@ -1698,7 +1749,7 @@ if __name__ == '__main__': # world.load_ellipsoid([0,0,2], mass=0, scale=[2.,1.,1.], color=(0,0,1,1)) world.load_visual_cone([0, 0, 0.1*0.5], orientation=(0, 1, 0, 0), scale=(0.1, 0.1, 0.1), color=(0.5, 0, 0, 0.5)) - world.load_right_triangular_prism([-1, -1, 2]) + # world.load_right_triangular_prism([-1, -1, 2]) # floor = world.load_mesh(filename='box', [1, 0, 2], mass=0, color=None) # floor = world.load_floor() diff --git a/scripts/install_gazebo_models.sh b/scripts/install_gazebo_models.sh new file mode 100755 index 0000000..7bd8d36 --- /dev/null +++ b/scripts/install_gazebo_models.sh @@ -0,0 +1,4 @@ +#!/bin/sh +# install gazebo_models + +hg clone https://bitbucket.org/osrf/gazebo_models/