diff --git a/pyrobolearn/simulators/__init__.py b/pyrobolearn/simulators/__init__.py index b56cab2..025ea2a 100644 --- a/pyrobolearn/simulators/__init__.py +++ b/pyrobolearn/simulators/__init__.py @@ -14,6 +14,9 @@ from .bullet_ros import BulletROS # dart simulator # from .dart import Dart +# MuJoCo simulator +# from .mujoco import Mujoco + # # PyBullet simulator # import pybullet # import pybullet_data diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index ab0d5f0..0507983 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -27,19 +27,43 @@ References: - [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco """ +# import standard libraries import os import time +import numpy as np import pickle -import xml.etree.ElementTree as ET +from collections import OrderedDict +# import XML parsers to parse / create XML for MuJoCo +import xml.etree.ElementTree as ET # XML parser +from xml.dom import minidom # to print in a pretty way the XML file + +# import mesh converter (from .obj to .stl) +try: + import pymesh # rapid prototyping platform focused on geometry processing + # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html + + import pyassimp # library to import and export various 3d-model-formats + # doc: http://www.assimp.org/index.php + # github: https://github.com/assimp/assimp +except ImportError as e: + raise ImportError(str(e) + "\nTry to install pymesh pyassimp: `pip install pymesh pyassimp`") + +# import image converter +from PIL import Image + +# import MuJoCo try: import mujoco_py as mujoco + # from dm_control import mujoco except ImportError as e: raise ImportError(str(e) + "\nTry to install `MuJoCo` and `mujoco_py`!") -# from dm_control import mujoco +# import pyrobolearn related functionalities from pyrobolearn.simulators.simulator import Simulator +from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser, converter + # check Python version import sys @@ -57,6 +81,108 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" +class Visual(object): + """Visual information.""" + + def __init__(self, visual_id, dtype="sphere", size=(0., 0., 0.), mesh=None, color=(0.5, 0.5, 0.5, 1), + position=(0., 0., 0.), orientation=(0., 0., 0., 1.)): + """ + Initialize the visual info class. + + Args: + visual_id (int): unique visual id. + dtype (str): primitive type {"plane", "sphere", "box", "capsule", "ellipsoid", "cylinder", "mesh"}. + size (float, tuple of float, np.array): size. + mesh (str): path to mesh. + color (tuple of 4 float): RGBA color. Each channel is between 0 and 1. + position (tuple of 3 float, np.array[3]): position. + orientation (tuple of 4 float, np.array[4]): quaternion (x,y,z,w) + """ + self.id = visual_id + self.dtype = dtype + self.size = size + self.mesh = mesh + self.color = color + self.position = position + self.orientation = orientation + + +class Collision(object): + """Collision information.""" + + def __init__(self, collision_id, dtype="sphere", size=(0., 0., 0.), mesh=None, position=(0., 0., 0.), + orientation=(0., 0., 0., 1.)): + """ + Initialize the collision info class. + + Args: + collision_id (int): unique collision id. + dtype (str): primitive type {"plane", "sphere", "box", "capsule", "ellipsoid", "cylinder", "mesh"}. + size (float, tuple of float, np.array): size. + mesh (str, None): path to the mesh. + position (tuple of 3 float, np.array[3]): position. + orientation (tuple of 4 float, np.array[4]): quaternion (x,y,z,w) + """ + self.id = collision_id + self.dtype = dtype + self.size = size + self.mesh = mesh + self.position = position + self.orientation = orientation + + +class Texture(object): + """Texture information""" + + def __init__(self, texture_id, texture, material): + """ + Initialize the texture. + + Args: + texture_id (int): texture id. + """ + self.id = texture_id + self.texture = texture + self.material = material + + +class Body(object): + """Body.""" + + def __init__(self, body_id, body): + """ + Initialize the Body. + + Args: + body_id (int): unique body id. + body (xml.etree.ElementTree.Element): body tag in the xml file. + """ + self.id = body_id + if not isinstance(body, ET.Element): + raise TypeError("Expecting the given 'body' to be an instance of `ET.Element`, but got instead: " + "{}".format(type(body))) + self.body = body + + self.q_start = 0 + self.q_end = 0 + self.fixed_base = False + + # list of inner bodies (=links) + self.bodies = [] + self.joints = [] + self.joint = None + + @property + def name(self): + """Return the body name.""" + return self.body.attrib.get("name") + + +class Joint(object): + """Joint.""" + pass + + class Mujoco(Simulator): r"""Mujoco Simulator interface. @@ -98,20 +224,48 @@ class Mujoco(Simulator): # define variables self.load_at_the_end = load_at_the_end + self.model = None + self.sim = None self.viewer = None - # create empty world - xml_path = os.path.dirname(os.path.abspath(__file__)) + '/mujoco_empty_world.xml' - model = mujoco.load_model_from_path(xml_path) - self.model = model - self.sim = mujoco.MjSim(model) + # create dynamically an empty world (XML) - # parse the world - self.world = self._parse() + # path to the xml file + self.xml_path = os.path.dirname(os.path.abspath(__file__)) + '/prl_mujoco.xml' + + # create root + self._root = ET.Element("mujoco") + + # create worldbody + ET.SubElement(self._root, 'worldbody') + self._worldbody = self._root.find('worldbody') + + # add a light + ET.SubElement(self._worldbody, "light", attrib={"diffuse": ".5 .5 .5", "pos": "0 0 3", "dir": "0 0 -1"}) + + # create model, simulator and viewer + # self.model = mujoco.load_model_from_path(self.xml_path) + # self.sim = mujoco.MjSim(self.model) + # self.viewer = mujoco.MjViewer(self.sim) # define saving states self.__simulator_saving_states = {} + # keep track of visual and collision shapes + self.visual_shapes = {} # {visual_id: Visual} + self.collision_shapes = {} # {collision_id: Collision} + self.bodies = OrderedDict() # {body_id: Body} + self.textures = {} # {texture_id: Texture} + self.constraints = OrderedDict() # {constraint_id: Constraint} + + # create counters + self._visual_cnt = 0 + self._collision_cnt = 0 + self._body_cnt = 1 # 0 is for the world + self._texture_cnt = 0 + self._constraint_cnt = 0 + self.q_cnt = 0 + ############## # Properties # ############## @@ -126,10 +280,10 @@ class Mujoco(Simulator): """Return the simulator time step.""" return self.dt - @property - def dt(self): - """Return the simulator time step.""" - return self.sim.model.opt.timestep + # @property + # def dt(self): + # """Return the simulator time step.""" + # return self.sim.model.opt.timestep ############# # Operators # @@ -151,15 +305,37 @@ class Mujoco(Simulator): difference.""" return True + @staticmethod + def supports_sensors(sensor_type=None): # TODO: map the names + """Return True if the simulator provides supports for the specified sensor.""" + sensor_type = sensor_type.lower() + return sensor_type in {'touch', 'accelerometer', 'velocimeter', 'gyro', 'force', 'torque', 'magnetometer', + 'rangefinder', 'jointpos', 'jointvel', 'tendonpos', 'tendonvel', 'actuatorpos', + 'actuatorvel', 'actuatorfrc', 'ballquat', 'ballangvel', 'jointlimitpos', + 'jointlimitvel', 'jointlimitfrc', 'tendonlimitpos', 'tendonlimitvel', 'tendonlimitfrc', + 'framepos', 'framequat', 'framexaxis', 'frameyaxis', 'framelinvel', 'frameangvel', + 'framelinacc', 'frameangacc', 'subtreecom', 'subtreelinvel', 'subtreeangmom', 'user'} + ########### # Methods # ########### - ############## - # Simulators # - ############## + ########### + # Private # + ########### - def _parse(self, xml_path): + def _create_sim(self, path, render=False): + self.model = mujoco.load_model_from_path(path) + self.sim = mujoco.MjSim(self.model) + if render: + self.viewer = mujoco.MjViewer(self.sim) + + ####### + # XML # + ####### + + @staticmethod + def _parse(xml_path): """ Parse the provided XML file. @@ -169,9 +345,66 @@ class Mujoco(Simulator): Returns: xml.etree.ElementTree.Element: root element in the XML file. """ - root = ET.parse(xml_path).getroot() + tree = ET.parse(xml_path) + root = tree.getroot() # 'mujoco' + + # check bodies # TODO + return root + @staticmethod + def _write_xml(root, filename): + """ + Write an XML. + + Args: + root (xml.etree.ElementTree.Element): root element of the tree in the XML file. + filename (str): path to the file to write the XML in. + """ + xmlstr = minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") + with open(filename, "w") as f: + f.write(xmlstr) # .encode('utf-8')) + + @staticmethod + def _remove_xml(filename): + """ + Remove the specified XML file. + + Args: + filename (str): path to the XML file. + """ + if os.path.exists(filename) and os.path.isfile(filename): + os.remove(filename) + + @staticmethod + def does_file_exist(filename): + """Check if the given filename exists or not. + + Args: + filename (str): path to the file. + """ + return os.path.exists(filename) and os.path.isfile(filename) + + @staticmethod + def _convert_wxyz_to_xyzw(q): + """Convert a quaternion in the (w,x,y,z) format to (x,y,z,w).""" + return np.roll(q, shift=-1) + + @staticmethod + def _convert_xyzw_to_wxyz(q): + """Convert a quaternion in the (x,y,z,w) format to (w,x,y,z).""" + return np.roll(q, shift=1) + + def _load(self): + """Load the model from the XML path, and create the simulator.""" + self._write_xml(self._root, self.xml_path) + self._create_sim(self.xml_path, render=False) + self._remove_xml(self.xml_path) + + ############## + # Simulators # + ############## + def reset(self): """Reset the simulator. @@ -179,20 +412,53 @@ class Mujoco(Simulator): """ self.sim.reset() + def close(self): + """Close the simulator.""" + # remove the XML file + self._remove_xml(self.xml_path) + + # delete the simulator + del self.sim + + def seed(self, seed=None): + """Set the given seed in the simulator.""" + # It seems this is not possible in MuJoCo + pass + 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.forward() # computes forward kinematics - self.sim.step() # advance the simulation + # if the simulator/model has not been created + if self.sim is None: + # if the xml file doesn't exist, create one + # if not self.does_file_exist(self.xml_path): + # self._write_xml(self._root, self.xml_path) + # self._create_sim(self.xml_path, render=False) + self._load() + + # computes forward kinematics in the simulator + self.sim.forward() + + # advance the simulation + self.sim.step() if self.is_rendering(): if self.viewer is None: self.viewer = mujoco.MjViewer(self.sim) self.viewer.render() time.sleep(sleep_time) + def reset_scene_camera(self, camera=None): + """ + Reinitialize/Reset the scene view camera to the previous one. + + Args: + camera (object): scene view camera. This is let to the user to decide what to do. + """ + pass + def render(self, enable=True): """Render the simulation. @@ -319,6 +585,11 @@ class Mujoco(Simulator): Returns: list(int): list of object unique id for each object loaded """ + # parse sdf + tree = ET.parse(filename) + root = tree.getroot() + + # add bodies pass def load_mjcf(self, filename, scaling=1., *args, **kwargs): @@ -337,10 +608,786 @@ class Mujoco(Simulator): self.model = mujoco.load_model_from_path(filename) self.sim = mujoco.MjSim(self.model) + def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None, + with_collision=True, flags=None, *args, **kwargs): + """Load a mesh into the simulator. + + 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. + position (float[3]): position of the mesh in the Cartesian world space (in meters) + orientation (float[4], np.quaternion): orientation of the mesh using quaternion. + If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. + scale (float[3]): scale the mesh in the (x,y,z) directions + color (int[4], None): color of the mesh for red, green, blue, and alpha, each in range [0,1]. + with_collision (bool): If True, it will also create the collision mesh, and not only a visual mesh. + flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static + triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain. + + Returns: + int: unique id of the mesh in the world + """ + # create collision shape if specified + + # create visual shape + + # create body + pass + + ########## + # Bodies # + ########## + + 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): # DONE + """Create a body in the simulator. + + Args: + visual_shape_id (int): unique id from createVisualShape or -1. You can reuse the visual shape (instancing) + collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape + for multiple multibodies (instancing) + mass (float): mass of the base, in kg (if using SI units) + position (np.array[3]): Cartesian world position of the base + orientation (np.array[4]): Orientation of base as quaternion [x,y,z,w] + + Returns: + int: non-negative unique id or -1 for failure. + """ + # check that at least the visual or collision shape is provided + if visual_shape_id == -1 and collision_shape_id == -1: + raise ValueError("Expecting the visual shape or collision shape id to be specified.") + + # get the corresponding visual / collision object + visual = self.visual_shapes.get(visual_shape_id, None) + collision = self.collision_shapes.get(collision_shape_id, None) + + # convert position and orientation as strings + pos = str(np.asarray(position))[1:-1] + quat = str(self._convert_xyzw_to_wxyz(np.asarray(orientation)))[1:-1] + + # create tag in xml + body = ET.SubElement(self._worldbody, "body", attrib={"name": "body_" + str(self._body_cnt), + "pos": pos, "quat": quat}) + + # create tag in xml + joint = ET.SubElement(body, "joint", attrib={"type": "free"}) + + # create tag in xml + if visual is None: # if no visual, use collision shape + geom = ET.SubElement(body, "geom", attrib={"type": collision.dtype, + "size": str(np.asarray(collision.size).reshape(-1))[1:-1], + "rgba": "0 0 0 0", "mass": str(mass)}) # transparent + + # if primitive shape type is a mesh + if collision.dtype == "mesh": + # check tag in xml + asset = self._root.find("asset") + + # if no tag, create one + if asset is None: + asset = ET.SubElement(self._root, "asset") + + # create mesh tag + mesh_name = "mesh_" + str(collision.id) + mesh = ET.SubElement(asset, "mesh", attrib={"name": mesh_name, + "file": collision.mesh, + "scale": str(np.asarray(collision.size)[1:-1])}) + + # set the mesh asset name + geom.attrib["mesh"] = mesh_name + else: + # if visual is given, use this one instead + geom = ET.SubElement(body, "geom", attrib={"type": visual.dtype, + "size": str(np.asarray(visual.size).reshape(-1))[1:-1], + "rgba": str(np.asarray(visual.color))[1:-1], + "mass": str(mass)}) + + # if primitive shape type is a mesh + if visual.dtype == "mesh": + # check tag in xml + asset = self._root.find("asset") + + # if no tag, create one + if asset is None: + asset = ET.SubElement(self._root, "asset") + + # create mesh tag + mesh_name = "mesh_" + str(visual.id) + mesh = ET.SubElement(asset, "mesh", attrib={"name": mesh_name, + "file": visual.mesh, + "scale": str(np.asarray(visual.size)[1:-1])}) + + # set the mesh asset name + geom.attrib["mesh"] = mesh_name + + # if no collision shape + if collision is None: + geom.attrib["contype"] = "0" + geom.attrib["conaffinity"] = "0" + + # create tag in xml + # TODO: pos must be provided and corresponds to the inertial frame, try to use MuJoCo to compute it + # inertial = ET.SubElement(body, "inertial", attrib={"pos": pos, "mass": str(mass)}) + + # add body in self.bodies + body = Body(self._body_cnt, body=body) + if mass == 0: + body.fixed_base = True + else: + body.q_start = self.q_cnt + body.q_end = self.q_cnt + 7 + self.q_cnt += 7 + self.bodies[self._body_cnt] = body + + # increment body counter + self._body_cnt += 1 + + self._load() + + # return body id + return self._body_cnt - 1 + + def remove_body(self, body_id): # DONE + """Remove a particular body in the simulator. + + Args: + body_id (int): unique body id. + """ + # remove body from the bodies + body = self.bodies.pop(body_id) + + # remove it from the worldbody + self._worldbody.remove(body) + + # if the model / sim were loaded, reload them + if self.sim is not None and self.model is not None: + self._load() + if self.is_rendering() and self.viewer is not None: + self.viewer = mujoco.MjViewer(self.sim) + + def num_bodies(self): # DONE + """Return the number of bodies present in the simulator. + + Returns: + int: number of bodies + """ + return len(self.bodies) + + def get_body_info(self, body_id): # DONE + """Get the specified body information. + + Specifically, it returns the base name extracted from the URDF, SDF, MJCF, or other file. + + Args: + body_id (int): unique body id. + + Returns: + str: base name + """ + return self.bodies[body_id].name + + def get_body_id(self, index): # DONE + """ + Get the body id associated to the index which is between 0 and `num_bodies()`. + + Args: + index (int): index between [0, `num_bodies()`] + + Returns: + int: unique body id. + """ + return list(self.bodies.items())[index][0] + + ############### + # constraints # + ############### + + def create_constraint(self, parent_body_id, parent_link_id, child_body_id, child_link_id, joint_type, + joint_axis, parent_frame_position, child_frame_position, + parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.), + *args, **kwargs): + """ + Create a constraint. + + Args: + parent_body_id (int): parent body unique id + parent_link_id (int): parent link index (or -1 for the base) + child_body_id (int): child body unique id, or -1 for no body (specify a non-dynamic child frame in world + coordinates) + child_link_id (int): child link index, or -1 for the base + joint_type (int): joint type: JOINT_PRISMATIC (=1), JOINT_FIXED (=4), JOINT_POINT2POINT (=5), + JOINT_GEAR (=6) + joint_axis (np.array[3]): joint axis, in child link frame + parent_frame_position (np.array[3]): position of the joint frame relative to parent CoM frame. + child_frame_position (np.array[3]): position of the joint frame relative to a given child CoM frame (or + world origin if no child specified) + parent_frame_orientation (np.array[4]): the orientation of the joint frame relative to parent CoM + coordinate frame + child_frame_orientation (np.array[4]): the orientation of the joint frame relative to the child CoM + coordinate frame (or world origin frame if no child specified) + + Returns: + int: constraint unique id. + """ + # check tag in xml (which are used for constraints). + asset = self._root.find("equality") + + # if no tag, create one + if asset is None: + asset = ET.SubElement(self._root, "equality") + + # Constraints: + # - connect: constraint that connects two bodies at a point (ball joints) + # - weld: + # - joint: + # - tendon: + # - distance: + + # create constraint tag + constraint = ET.SubElement(asset, "connect", attrib={"name": "constraint_" + str(self._constraint_cnt), + "body1": None, "body2": None}) + + # remember constraint + self.constraints[self._constraint_cnt] = constraint + + # increment constraint counter + self._constraint_cnt += 1 + + # return constraint id + return self._constraint_cnt - 1 + + def remove_constraint(self, constraint_id): + """ + Remove the specified constraint. + + Args: + constraint_id (int): constraint unique id. + """ + pass + + def change_constraint(self, constraint_id, *args, **kwargs): + """ + Change the parameters of an existing constraint. + + Args: + constraint_id (int): constraint unique id. + """ + pass + + def num_constraints(self): + """ + Get the number of constraints created. + + Returns: + int: number of constraints created. + """ + return len(self.constraints) + + def get_constraint_id(self, index): + """ + Get the constraint unique id associated with the index which is between 0 and `num_constraints()`. + + Args: + index (int): index between [0, `num_constraints()`] + + Returns: + int: constraint unique id. + """ + return list(self.constraints.items())[index][0] + + def get_constraint_info(self, constraint_id): + """ + Get information about the given constaint id. + + Args: + constraint_id (int): constraint unique id. + + Returns: + dict, list: info + """ + pass + + def get_constraint_state(self, constraint_id): + """ + Get the state of the given constraint. + + Args: + constraint_id (int): constraint unique id. + + Returns: + dict, list: state + """ + pass + + ########### + # objects # + ########### + + def get_mass(self, body_id): + """ + Return the total mass of the robot (=sum of all mass links). + + Args: + body_id (int): unique object id, as returned from `load_urdf`. + + Returns: + float: total mass of the robot [kg] + """ + body = self.bodies[body_id] + mass = self.get_base_mass(body_id) + for b in body.bodies: + mass += sim.get_base_mass(b.id) + return mass + + def get_base_mass(self, body_id): + """Return the base mass of the robot. + + Args: + body_id (int): unique object id. + """ + return self.sim.model.body_mass[body_id] + + def get_base_name(self, body_id): + """ + Return the base name. + + Args: + body_id (int): unique object id. + + Returns: + str: base name + """ + return self.sim.model.body_id2name(box2) + + def get_center_of_mass_position(self, body_id, link_ids=None): # TODO + """ + Return the center of mass position. + + Args: + body_id (int): unique body id. + link_ids (list of int): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.array[3]: center of mass position in the Cartesian world coordinates + """ + return self.sim.data.subtree_com[body_id] + + def get_center_of_mass_velocity(self, body_id, link_ids=None): # TODO + """ + Return the center of mass linear velocity. + + Args: + body_id (int): unique body id. + link_ids (list of int): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.array[3]: center of mass linear velocity. + """ + return self.sim.data.subtree_linvel[body_id] + + def get_base_pose(self, body_id): + """ + Get the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[3]: base position + np.array[4]: base orientation (quaternion [x,y,z,w]) + """ + # WARNING: body_xpos is one step late compared to qpos + # return self.sim.data.body_xpos[body_id], self._convert_wxyz_to_xyzw(self.sim.data.body_xquat[body_id]) + + # print(self.sim.data.body_xpos, q[body.q_start:body.q_start+3]) + # print(self.sim.data.body_xquat, q[body.q_start+3:body.q_end]) + + body = self.bodies[body_id] + q = self.sim.data.qpos + return q[body.q_start:body.q_start+3], self._convert_wxyz_to_xyzw(q[body.q_start+3:body.q_start+7]) + + def get_base_position(self, body_id): + """ + Return the base position of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[3]: base position. + """ + # return self.sim.data.body_xpos[body_id] + body = self.bodies[body_id] + q = self.sim.data.qpos + return q[body.q_start:body.q_start + 3] + + def get_base_orientation(self, body_id): + """ + Get the base orientation of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[4]: base orientation in the form of a quaternion (x,y,z,w) + """ + # return self._convert_wxyz_to_xyzw(self.sim.data.body_xquat[body_id]) + body = self.bodies[body_id] + q = self.sim.data.qpos + return self._convert_wxyz_to_xyzw(q[body.q_start+3:body.q_start+7]) + + def reset_base_pose(self, body_id, position, orientation): + """ + Reset the base position and orientation of the specified object id. + + "It is best only to do this at the start, and not during a running simulation, since the command will override + the effect of all physics simulation. The linear and angular velocity is set to zero. You can use + `reset_base_velocity` to reset to a non-zero linear and/or angular velocity." [1] + + Args: + body_id (int): unique object id. + position (np.array[3]): new base position. + orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ + body = self.bodies[body_id] + q = self.sim.data.qpos + q[body.q_start:body.q_start + 3] = position + q[body.q_start + 3:body.q_start + 7] = self._convert_xyzw_to_wxyz(orientation) + + def reset_base_position(self, body_id, position): + """ + Reset the base position of the specified body/object id while preserving its orientation. + + Args: + body_id (int): unique object id. + position (np.array[3]): new base position. + """ + # self.sim.data.body_xpos[body_id] = position + # self.sim.forward() + body = self.bodies[body_id] + q = self.sim.data.qpos + q[body.q_start:body.q_start + 3] = position + + def reset_base_orientation(self, body_id, orientation): + """ + Reset the base orientation of the specified body/object id while preserving its position. + + Args: + body_id (int): unique object id. + orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ + body = self.bodies[body_id] + q = self.sim.data.qpos + q[body.q_start+3:body.q_start+7] = self._convert_xyzw_to_wxyz(orientation) + + def get_base_velocity(self, body_id): + """ + Return the base linear and angular velocities. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[3]: linear velocity of the base in Cartesian world space coordinates + np.array[3]: angular velocity of the base in Cartesian world space coordinates + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + return dq[body.q_start:body.q_start+3], dq[body.q_start+3:body.q_start+6] + + def get_base_linear_velocity(self, body_id): + """ + Return the linear velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[3]: linear velocity of the base in Cartesian world space coordinates + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + return dq[body.q_start:body.q_start + 3] + + def get_base_angular_velocity(self, body_id): + """ + Return the angular velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[3]: angular velocity of the base in Cartesian world space coordinates + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + return dq[body.q_start+3:body.q_start+6] + + def reset_base_velocity(self, body_id, linear_velocity=None, angular_velocity=None): + """ + Reset the base velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.array[3]): new linear velocity of the base. + angular_velocity (np.array[3]): new angular velocity of the base. + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + dq[body.q_start:body.q_start + 3] = linear_velocity + dq[body.q_start + 3:body.q_start + 6] = angular_velocity + + def reset_base_linear_velocity(self, body_id, linear_velocity): + """ + Reset the base linear velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.array[3]): new linear velocity of the base + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + dq[body.q_start:body.q_start + 3] = linear_velocity + + def reset_base_angular_velocity(self, body_id, angular_velocity): + """ + Reset the base angular velocity. + + Args: + body_id (int): unique object id. + angular_velocity (np.array[3]): new angular velocity of the base + """ + body = self.bodies[body_id] + dq = self.sim.data.qvel + dq[body.q_start + 3:body.q_start + 6] = angular_velocity + + def get_base_acceleration(self, body_id): + """ + Get the base acceleration. This is only valid if the simulator `supports_acceleration`. + + Args: + body_id (int): unique object id. + + Returns: + np.array[3]: linear acceleration [m/s^2] + np.array[3]: angular acceleration [rad/s^2] + """ + body = self.bodies[body_id] + ddq = self.sim.data.qacc + return ddq[body.q_start:body.q_start+3], ddq[body.q_start+3:body.q_start+6] + + def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), frame=1): + """ + Apply the specified external force on the specified position on the body / link. + + Args: + body_id (int): unique body id. + link_id (int): unique link id. If -1, it will be the base. + force (np.array[3]): external force to be applied. + position (np.array[3]): position on the link where the force is applied. See `flags` for coordinate + systems. If None, it is the center of mass of the body (or the link if specified). + frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they + are described in the world frame. + """ + pass + + def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1): + """ + Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external + torques are cleared to 0. + + Args: + body_id (int): unique body id. + link_id (int): link id to apply the torque, if -1 it will apply the torque on the base + torque (float[3]): Cartesian torques to be applied on the body + frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for + Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. + """ + pass + + ############################# + # robots (joints and links) # + ############################# + + def num_joints(self, body_id): + """ + Return the total number of joints of the specified body. This is the same as calling `num_links`. + + Args: + body_id (int): unique body id. + + Returns: + int: number of joints with the associated body id. + """ + body = self.bodies[body_id] + return len(body.links) + + def num_actuated_joints(self, body_id): + """ + Return the total number of actuated joints associated with the given body id. + + Args: + body_id (int): unique body id. + + Returns: + int: number of actuated joints of the specified body. + """ + body = self.bodies[body_id] + return len(body.joints) + ################# # visualization # ################# + 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. + + Args: + shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4), + GEOM_PLANE (=6), GEOM_MESH (=5), GEOM_ELLIPSOID (=9) + radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER + half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX, and GEOM_ELLIPSOID + length (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height). + filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each + object (marked as 'o') in the .obj file. + mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH). + plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE). + flags (int): unused / to be decided + rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1]. + specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range + [0..1] + visual_frame_position (np.array[3]): translational offset of the visual shape with respect to the link frame + vertices (list of np.array[3]): Instead of creating a mesh from obj file, you can provide vertices, indices, + uvs and normals + indices (list of int): triangle indices, should be a multiple of 3. + uvs (list of np.array[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the + texture image. The number of uvs should be equal to number of vertices + normals (list of np.array[3]): vertex normals, number should be equal to number of vertices. + visual_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the visual shape with + respect to the link frame + + Returns: + int: The return value is a non-negative int unique id for the visual shape or -1 if the call failed. + """ + if shape_type == self.GEOM_SPHERE: + size = radius + shape_type = "sphere" + elif shape_type == self.GEOM_BOX: + size = half_extents + shape_type = "box" + elif shape_type == self.GEOM_CAPSULE: + size = (radius, length / 2.) + shape_type = "capsule" + elif shape_type == self.GEOM_CYLINDER: + size = (radius, length / 2.) + shape_type = "cylinder" + elif shape_type == self.GEOM_PLANE: + size = (0., 0., 1.) # (X half-size, Y half-size, spacing between square grid lines) + # compute the orientation based on the normal + shape_type = "plane" + elif shape_type == self.GEOM_ELLIPSOID: + size = half_extents # (X radius, Y radius, Z radius) + shape_type = "ellipsoid" + elif shape_type == self.GEOM_MESH: + size = mesh_scale + shape_type = "mesh" + else: + raise ValueError("Unknown visual shape type.") + + # create visual + self.visual_shapes[self._visual_cnt] = Visual(visual_id=self._visual_cnt, dtype=shape_type, size=size, + mesh=filename, color=rgba_color, position=visual_frame_position, + orientation=visual_frame_orientation) + + # increment visual shape counter + self._visual_cnt += 1 + + # return visual shape id + return self._visual_cnt - 1 + + def get_visual_shape_data(self, object_id, flags=-1): + """ + Get the visual shape data associated with the given object id. It will output a list of visual shape data. + + Args: + object_id (int): object unique id. + flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`. + + Returns: + list: + int: object unique id. + int: link index or -1 for the base + int: visual geometry type (TBD) + np.array[3]: dimensions (size, local scale) of the geometry + str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but + could be absolute + np.array[3]: position of local visual frame, relative to link/joint frame + np.array[4]: orientation of local visual frame relative to link/joint frame + list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha + int: texture unique id of the shape or -1 if None. This field only exists if using + VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. + """ + shapes = [] + return shapes + + def change_visual_shape(self, object_id, link_id, shape_id=None, texture_id=None, rgba_color=None, + specular_color=None): + """ + Allows to change the texture of a shape, the RGBA color and other properties. + + Args: + object_id (int): unique object id. + link_id (int): link id. + shape_id (int): shape id. + texture_id (int): texture id. + rgba_color (float[4]): RGBA color. Each is in the range [0..1]. Alpha has to be 0 (invisible) or 1 + (visible) at the moment. + specular_color (int[3]): specular color components, RED, GREEN and BLUE, can be from 0 to large number + (>100). + """ + pass + + def load_texture(self, filename): # DONE + """ + Load a texture from file and return a non-negative texture unique id if the loading succeeds. + This unique id can be used with change_visual_shape. + + Args: + filename (str): path to the file. + + Returns: + int: texture unique id. If non-negative, the texture was loaded successfully. + """ + # check tag in xml + asset = self._root.find("asset") + + # if no tag, create one + if asset is None: + asset = ET.SubElement(self._root, "asset") + + # create texture tag + texture = ET.SubElement(asset, "texture", attrib={"name": "texture_" + str(self._texture_cnt), + "type": "2d", + "file": filename}) + material = ET.SubElement(asset, "material", attrib={"name": "material_" + str(self._texture_cnt), + "texture": "texture_" + str(self._texture_cnt)}) + + # remember texture_id --> (texture, material) + self.textures[self._texture_cnt] = Texture(texture_id=self._texture_cnt, texture=texture, material=material) + + # increment texture counter + self._texture_cnt += 1 + + # return texture id + return self._texture_cnt - 1 + # TODO: change such that we don't return the width and height (the user already knows them) # TODO: check for segmentation image def get_camera_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, @@ -445,6 +1492,91 @@ class Mujoco(Simulator): # Collisions # ############## + 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 collision shape in the simulator. + + Args: + shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4), + GEOM_PLANE (=6), GEOM_MESH (=5), GEOM_ELLIPSOID (=9) + radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER + half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX. + height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height). + filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each + object (marked as 'o') in the .obj file. + mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH). + plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE). + flags (int): unused / to be decided + collision_frame_position (np.array[3]): translational offset of the collision shape with respect to the + link frame + collision_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the collision shape + with respect to the link frame + + Returns: + int: The return value is a non-negative int unique id for the collision shape or -1 if the call failed. + """ + if shape_type == self.GEOM_SPHERE: + size = radius + shape_type = "sphere" + elif shape_type == self.GEOM_BOX: + size = half_extents + shape_type = "box" + elif shape_type == self.GEOM_CAPSULE: + size = (radius, height / 2.) + shape_type = "capsule" + elif shape_type == self.GEOM_CYLINDER: + size = (radius, height / 2.) + shape_type = "cylinder" + elif shape_type == self.GEOM_PLANE: + size = (0., 0., 1.) # (X half-size, Y half-size, spacing between square grid lines) + # compute the orientation based on the normal + shape_type = "plane" + elif shape_type == self.GEOM_ELLIPSOID: + size = half_extents # (X radius, Y radius, Z radius) + shape_type = "ellipsoid" + elif shape_type == self.GEOM_MESH: + size = mesh_scale + shape_type = "mesh" + else: + raise ValueError("Unknown collision shape type.") + + # create visual + self.collision_shapes[self._collision_cnt] = Collision(collision_id=self._collision_cnt, dtype=shape_type, + size=size, mesh=filename, + position=collision_frame_position, + orientation=collision_frame_orientation) + + # increment visual shape counter + self._collision_cnt += 1 + + # return collision shape id + return self._collision_cnt - 1 + + def get_collision_shape_data(self, object_id, link_id=-1): + """ + Get the collision shape data associated with the specified object id and link id. + + Args: + object_id (int): object unique id. + link_id (int): link index or -1 for the base. + + Returns: + int: object unique id. + int: link id. + int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6) + np.array[3]: depends on geometry type: + for GEOM_BOX: extents, + for GEOM_SPHERE: dimensions[0] = radius, + for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius. + For GEOM_MESH: dimensions is the scaling factor. + str: Only for GEOM_MESH: file name (and path) of the collision mesh asset. + np.array[3]: Local position of the collision frame with respect to the center of mass/inertial frame + np.array[4]: Local orientation of the collision frame with respect to the inertial frame + """ + pass + def ray_test(self, from_position, to_position): """ Performs a single raycast to find the intersection information of the first object hit. @@ -471,5 +1603,38 @@ if __name__ == '__main__': sim = Mujoco(render=True) + # create box in simulator + dimensions = 1. * np.ones(3) + collision_shape = sim.create_collision_shape(sim.GEOM_BOX, half_extents=dimensions / 2.) + visual_shape = sim.create_visual_shape(sim.GEOM_BOX, half_extents=dimensions / 2., rgba_color=(0.8, 0.2, 0.2, 1.)) + visual_shape2 = sim.create_visual_shape(sim.GEOM_BOX, half_extents=dimensions / 2., rgba_color=(0.2, 0.2, 0.8, 1.)) + + box = sim.create_body(mass=1., collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=[0., 0., 1.]) + + box2 = sim.create_body(mass=2, visual_shape_id=visual_shape2, collision_shape_id=collision_shape, + position=[0., 1., 2.]) + + print(dir(sim.sim.model)) + print(dir(sim.sim.data)) + for t in count(): + # sim.get_base_pose(box) + # print(sim.get_base_velocity(box)) + # print("base pose: {}".format(sim.get_base_pose(box))) + # if t == 200: + # print("Reset") + # sim.reset_base_position(box, [0., 0., 2.]) + + # print(sim.sim.data.subtree_com) + # print("Mass: ", sim.sim.model.body_mass) + # print("Subtree mass: ", sim.sim.model.body_subtreemass) + # print(sim.get_base_mass(box)) + + # print(sim.sim.data.subtree_com) + + # if t == 200: + # sim.viewer.finish() + # input() + sim.step(sim.dt) diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index d41d4f2..9630a9d 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -111,7 +111,8 @@ class Simulator(object): GEOM_MESH = 5 GEOM_PLANE = 6 GEOM_CAPSULE = 7 - GEOM_CONE = 8 # NEW + GEOM_CONE = 8 # NEW + GEOM_ELLIPSOID = 9 # NEW GUI = 1 GUI_MAIN_THREAD = 8 @@ -319,6 +320,16 @@ class Simulator(object): """Return True if we can use URDFs.""" return False + @staticmethod + def supports_light(): + """Return True if we can define and access to the lights in the simulator.""" + return False + + @staticmethod + def can_load_heightmap(): + """Return True if the simulator can load a heightmap.""" + return False + ########### # Methods # ########### diff --git a/pyrobolearn/utils/__init__.py b/pyrobolearn/utils/__init__.py index c2cc49d..0352514 100644 --- a/pyrobolearn/utils/__init__.py +++ b/pyrobolearn/utils/__init__.py @@ -28,6 +28,9 @@ from . import feedback # import real-time plotting from . import plotting +# import parsers +# from . import parsers + # Built-in functions diff --git a/pyrobolearn/utils/parsers/robots/README.rst b/pyrobolearn/utils/parsers/robots/README.rst new file mode 100644 index 0000000..6dd89d3 --- /dev/null +++ b/pyrobolearn/utils/parsers/robots/README.rst @@ -0,0 +1,5 @@ +Robot parsers +============= + +This folder provides parsers to different robot files (URDF, MJCF, Proto, etc), and allows to convert from one format +to another. diff --git a/pyrobolearn/utils/parsers/robots/__init__.py b/pyrobolearn/utils/parsers/robots/__init__.py index e69de29..eec5263 100644 --- a/pyrobolearn/utils/parsers/robots/__init__.py +++ b/pyrobolearn/utils/parsers/robots/__init__.py @@ -0,0 +1,9 @@ + +# import robot parser +from .robot_parser import RobotParser + +# import urdf parser +from .urdf_parser import URDFParser + +# import mujoco parser +from .mujoco_parser import MuJoCoParser diff --git a/pyrobolearn/utils/parsers/robots/converter.py b/pyrobolearn/utils/parsers/robots/converter.py index e69de29..956779c 100644 --- a/pyrobolearn/utils/parsers/robots/converter.py +++ b/pyrobolearn/utils/parsers/robots/converter.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +"""Define the Converter class which allows to convert from one type of file (urdf, sdf, mjcf, and others) to another +format. +""" + +from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class Converter(object): + r"""Converter""" + + def __init__(self): + pass + + def convert(self, from_filename, to_filename): + pass diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py new file mode 100644 index 0000000..8c78adf --- /dev/null +++ b/pyrobolearn/utils/parsers/robots/data_structures.py @@ -0,0 +1,534 @@ +#!/usr/bin/env python +"""Provide the data structures that are shared among the various parsers and converter. +""" + +import numpy as np +from collections import OrderedDict + +from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class World(object): + r"""World data structure.""" + + def __init__(self, trees=None): + self.trees = trees + + +class Tree(object): + r"""Tree data structure.""" + + def __init__(self, name=None, root=None): + self.name = name + self.root = root + self.bodies = OrderedDict() + self.joints = OrderedDict() + self.materials = {} + + +class Body(object): + r"""Body / Link data structure.""" + + def __init__(self, body_id, name=None): + self.id = body_id + self.name = name + + # inner bodies / links + self.bodies = [] + self.joints = OrderedDict() + + self.inertial = None + self.visual = None + self.collision = None + + +class Joint(object): + r"""Joint data structure. + + Joint types: fixed, revolute/hinge, continuous + """ + + def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None, + position=None, orientation=None, friction=None, damping=None, effort=None, velocity=None): + self.id = joint_id + self.name = name + self.dtype = dtype + self.limits = limits + self.parent = parent + self.child = child + self.axis = axis + self.position = position + self.orientation = orientation + self.friction = friction + self.damping = damping + self.effort = effort + self.velocity = velocity + + @property + def limits(self): + return self._limits + + @limits.setter + def limits(self, limits): + if limits is not None: + if isinstance(limits, str): + limits = [lim for lim in limits.split()] + limits = tuple(limits) + limits = [float(lim) for lim in limits] + if len(limits) != 2: + raise ValueError("Expecting 2 floats for the joint limits") + self._limits = limits + + @property + def axis(self): + return self._axis + + @axis.setter + def axis(self, axis): + if axis is not None: + if isinstance(axis, str): + axis = [float(ax) for ax in axis.split()] + axis = tuple(axis) + if len(axis) != 3: + raise ValueError("Expecting the joint axis to be defined using 3 floats (x,y,z)") + self._axis = axis + + @property + def position(self): + return self._position + + @position.setter + def position(self, position): + if position is not None: + if isinstance(position, str): + position = [float(p) for p in position.split()] + position = np.asarray(position) + self._position = position + + @property + def orientation(self): + return self._orientation + + @orientation.setter + def orientation(self, orientation): + if orientation is not None: + if isinstance(orientation, str): + orientation = [float(o) for o in orientation.split()] + if len(orientation) == 4: # quaternion + orientation = get_rpy_from_quaternion(orientation) + if len(orientation) == 3: # rpy + pass + orientation = np.asarray(orientation) + self._orientation = orientation + + @property + def friction(self): + return self._friction + + @friction.setter + def friction(self, friction): + if friction is not None: + friction = float(friction) + self._friction = friction + + @property + def damping(self): + return self._damping + + @damping.setter + def damping(self, damping): + if damping is not None: + damping = float(damping) + self._damping = damping + + @property + def effort(self): + return self._effort + + @effort.setter + def effort(self, effort): + if effort is not None: + effort = float(effort) + self._effort = effort + + @property + def velocity(self): + return self._velocity + + @velocity.setter + def velocity(self, velocity): + if velocity is not None: + velocity = float(velocity) + self._velocity = velocity + + +class Inertia(object): + r"""Inertia data structure""" + + def __init__(self, ixx=1., iyy=1., izz=1., ixy=0., ixz=0., iyz=0.): + self.ixx = ixx + self.iyy = iyy + self.izz = izz + self.ixy = ixy + self.ixz = ixz + self.iyz = iyz + + @property + def diagonal_inertia(self): + return np.array([self.ixx, self.iyy, self.izz]) + + @property + def full_inertia(self): + return np.array([[self.ixx, self.ixy, self.ixz], + [self.ixy, self.iyy, self.iyz], + [self.ixz, self.iyz, self.izz]]) + + @property + def ixx(self): + return self._ixx + + @ixx.setter + def ixx(self, ixx): + if ixx is not None: + ixx = float(ixx) + self._ixx = ixx + + @property + def iyy(self): + return self._iyy + + @iyy.setter + def iyy(self, iyy): + if iyy is not None: + iyy = float(iyy) + self._iyy = iyy + + @property + def izz(self): + return self._izz + + @izz.setter + def izz(self, izz): + if izz is not None: + izz = float(izz) + self._izz = izz + + @property + def ixy(self): + return self._ixy + + @ixy.setter + def ixy(self, ixy): + if ixy is not None: + ixy = float(ixy) + self._ixy = ixy + + @property + def ixz(self): + return self._ixz + + @ixz.setter + def ixz(self, ixz): + if ixz is not None: + ixz = float(ixz) + self._ixz = ixz + + @property + def iyz(self): + return self._iyz + + @iyz.setter + def iyz(self, iyz): + if iyz is not None: + iyz = float(iyz) + self._iyz = iyz + + +class Inertial(object): + r"""Inertial parameters.""" + + def __init__(self, mass=None, inertia=None, position=None, orientation=None): + self.mass = mass + self.inertia = inertia + self.position = position + self.orientation = orientation + + @property + def mass(self): + return self._mass + + @mass.setter + def mass(self, mass): + if mass is not None: + mass = float(mass) + self._mass = mass + + @property + def inertia(self): + return self._inertia + + @inertia.setter + def inertia(self, inertia): + if inertia is not None: + if isinstance(inertia, str): + inertia = inertia.split() + if isinstance(inertia, (list, tuple, np.ndarray)): + if isinstance(inertia, np.ndarray) and inertia.ndim == 2: + if inertia.shape != (3, 3): + raise ValueError("Expecting a 3x3 inertia matrix") + inertia = Inertia(ixx=inertia[0, 0], ixy=inertia[0, 1], ixz=inertia[0, 2], + iyy=inertia[1, 1], iyz=inertia[1, 2], izz=inertia[2, 2]) + else: + if len(inertia) == 3: + inertia = Inertia(ixx=inertia[0], iyy=inertia[1], izz=inertia[2]) + elif len(inertia) == 6: + inertia = Inertia(ixx=inertia[0], iyy=inertia[1], izz=inertia[2], ixy=inertia[3], + ixz=inertia[4], iyz=inertia[5]) + elif len(inertia) == 9: + inertia = Inertia(ixx=inertia[0], ixy=inertia[1], ixz=inertia[2], iyy=inertia[4], + iyz=inertia[5], izz=inertia[8]) + elif isinstance(inertia, dict): + inertia = Inertia(**inertia) + self._inertia = inertia + + @property + def aligned_inertia(self): + raise NotImplementedError + + @property + def position(self): + return self._position + + @position.setter + def position(self, position): + if position is not None: + if isinstance(position, str): + position = [float(p) for p in position.split()] + position = np.asarray(position) + self._position = position + + @property + def orientation(self): + return self._orientation + + @orientation.setter + def orientation(self, orientation): + if orientation is not None: + if isinstance(orientation, str): + orientation = [float(o) for o in orientation.split()] + if len(orientation) == 4: # quaternion + orientation = get_rpy_from_quaternion(orientation) + if len(orientation) == 3: # rpy + pass + orientation = np.asarray(orientation) + self._orientation = orientation + + @property + def rpy(self): + return self._orientation + + @property + def quaternion(self): + return get_quaternion_from_rpy(self._orientation) + + +class Visual(object): + r"""visual parameters for body.""" + + def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None, + material=None): + self.name = name + self.dtype = dtype + self.size = size # depending on the type it can be different size + self.color = color + self.filename = filename + self.position = position + self.orientation = orientation + self.material = material + + @property + def size(self): + return self._size + + @size.setter + def size(self, size): + if size is not None: + if isinstance(size, str): + size = [float(s) for s in size.split()] + elif isinstance(size, (tuple, list, np.ndarray)): + size = [float(s) for s in size] + elif not isinstance(size, (float, int)): + raise TypeError("Expecting the size to be a float, int, list, tuple or np.ndarray") + self._size = size + + @property + def color(self): + return self._color + + @color.setter + def color(self, color): + if color is not None: + if isinstance(color, str): # e.g. '0.5 0.1 1. 1.' + color = (float(c) for c in color.split()) + if not isinstance(color, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + color = tuple(color) + self._color = color + + @property + def format(self): + """Return the filename format extension for the mesh.""" + if self.filename is not None: + return self.filename.split('.')[-1] + + @property + def position(self): + return self._position + + @position.setter + def position(self, position): + if position is not None: + if isinstance(position, str): + position = [float(p) for p in position.split()] + position = np.asarray(position) + self._position = position + + @property + def orientation(self): + return self._orientation + + @orientation.setter + def orientation(self, orientation): + if orientation is not None: + if isinstance(orientation, str): + orientation = [float(o) for o in orientation.split()] + if len(orientation) == 4: # quaternion + orientation = get_rpy_from_quaternion(orientation) + if len(orientation) == 3: # rpy + pass + orientation = np.asarray(orientation) + self._orientation = orientation + + @property + def rpy(self): + return self._orientation + + @property + def quaternion(self): + return get_quaternion_from_rpy(self._orientation) + + +class Collision(object): + r"""Collision parameters for body.""" + + def __init__(self, name=None, dtype=None, size=None, filename=None, position=None, orientation=None): + self.name = name + self.dtype = dtype + self.size = size + self.filename = filename + self.position = position + self.orientation = orientation + + @property + def size(self): + return self._size + + @size.setter + def size(self, size): + if size is not None: + if isinstance(size, str): + size = [float(s) for s in size.split()] + elif isinstance(size, (tuple, list, np.ndarray)): + size = [float(s) for s in size] + elif not isinstance(size, (float, int)): + raise TypeError("Expecting the size to be a float, int, list, tuple or np.ndarray") + self._size = size + + @property + def format(self): + if self.filename is not None: + return self.filename.split('.')[-1] + + @property + def position(self): + return self._position + + @position.setter + def position(self, position): + if position is not None: + if isinstance(position, str): + position = [float(p) for p in position.split()] + position = np.asarray(position) + self._position = position + + @property + def orientation(self): + return self._orientation + + @orientation.setter + def orientation(self, orientation): + if orientation is not None: + if isinstance(orientation, str): + orientation = [float(o) for o in orientation.split()] + if len(orientation) == 4: # quaternion + orientation = get_rpy_from_quaternion(orientation) + if len(orientation) == 3: # rpy + pass + orientation = np.asarray(orientation) + self._orientation = orientation + + @property + def rpy(self): + return self._orientation + + @property + def quaternion(self): + return get_quaternion_from_rpy(self._orientation) + + +class Material(object): + r"""Material info.""" + + def __init__(self, name=None, color=None): + self.name = name + self.color = color + + @property + def color(self): + return self._color + + @color.setter + def color(self, color): + if color is not None: + if isinstance(color, str): # e.g. '0.5 0.1 1. 1.' + color = (float(c) for c in color.split()) + if not isinstance(color, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + color = tuple(color) + self._color = color + + @property + def rgb(self): + if self.color is None: + return 0.5, 0.5, 0.5 + return tuple(self.color[:3]) + + @property + def rgba(self): + if self.color is None: + return 0.5, 0.5, 0.5, 1. + if len(self.color) == 3: + return tuple(self.color) + (1.,) + return tuple(self.color) diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py index e69de29..ef05f5f 100644 --- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py +++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python +"""Define the MuJoCo parser. +""" + +# import XML parser +import xml.etree.ElementTree as ET +from xml.dom import minidom # to print in a pretty way the XML file + +from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser +from pyrobolearn.utils.parsers.robots.data_structures import Tree + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MuJoCoParser(RobotParser): + r"""MuJoCo Parser""" + + def __init__(self, filename=None): + """ + Initialize the MuJoCo parser. + + Args: + filename (str, None): path to the MuJoCo XML file. + """ + super().__init__(filename) + + def parse(self, filename): + """ + Load and parse the given MuJoCo XML file. + + Args: + filename (str): path to the MuJoCo XML file. + """ + # load and parse the XML file + tree = ET.parse(filename) + + # get the root + root = tree.getroot() + + # check that the root is + if root.tag != 'mujoco': + raise RuntimeError("Expecting the first XML tag to be 'mujoco' but found instead: {}".format(root.tag)) + + # build the tree + + def get_tree(self): + """ + Return the Tree containing all the elements. + + Returns: + Tree: tree data structure. + """ + pass + + def generate(self, tree=None): + """ + Generate the XML tree from the `Tree` data structure. + + Args: + tree (Tree): Tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + pass + diff --git a/pyrobolearn/utils/parsers/robots/proto_parser.py b/pyrobolearn/utils/parsers/robots/proto_parser.py index e69de29..1ca5d71 100644 --- a/pyrobolearn/utils/parsers/robots/proto_parser.py +++ b/pyrobolearn/utils/parsers/robots/proto_parser.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +"""Define the Proto parser. + +Proto files are notably used in Webots. +""" + +from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser +from pyrobolearn.utils.parsers.robots.data_structures import Tree + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ProtoParser(RobotParser): + r"""Proto Parser""" + + def __init__(self, filename=None): + """ + Initialize the Proto parser. + + Args: + filename (str, None): path to the MuJoCo XML file. + """ + super().__init__(filename) + + def parse(self, filename): + """ + Load and parse the given URDF file. + + Args: + filename (str): path to the MuJoCo XML file. + """ + pass + + def get_tree(self): + """ + Return the Tree containing all the elements. + + Returns: + Tree: tree data structure. + """ + pass + + def generate(self, tree=None): + """ + Generate the XML tree from the `Tree` data structure. + + Args: + tree (Tree): Tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + pass diff --git a/pyrobolearn/utils/parsers/robots/robot_parser.py b/pyrobolearn/utils/parsers/robots/robot_parser.py index e69de29..bd97f70 100644 --- a/pyrobolearn/utils/parsers/robots/robot_parser.py +++ b/pyrobolearn/utils/parsers/robots/robot_parser.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python +"""Define the abstract Robot parser. +""" + +# import XML parser +import xml.etree.ElementTree as ET +from xml.dom import minidom # to print in a pretty way the XML file + +from pyrobolearn.utils.parsers.robots.data_structures import Tree + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RobotParser(object): + r"""Robot Parser""" + + def __init__(self, filename=None): + """ + Initialize the robot parser. + + Args: + filename (str, None): path to the MuJoCo XML file. + """ + self.root = None + self.tree = None + self.filename = filename + if filename is not None: + self.parse(filename) + + def parse(self, filename): + """ + Load and parse a given MuJoCo XML filename. + + Args: + filename (str): path to the MuJoCo XML file. + """ + pass + + def get_tree(self): + """ + Return the Tree containing all the elements. + + Returns: + Tree: tree data structure. + """ + return self.tree + + def generate(self, tree=None): + """ + Generate the XML tree from the `Tree` data structure. + + Args: + tree (Tree): Tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + pass + + def get_string(self, root=None): + """ + Return the XML string from the root element. + + Args: + root (ET.Element): root element in the XML file. + + Returns: + str: string representing the XML file. + """ + if root is None: + root = self.root + if not isinstance(root, ET.Element): + raise ValueError("Expecting the root to be an instance of `ET.Element`, but got instead: " + "{}".format(type(root))) + return minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") + + def write(self, filename, root=None): + """ + Write the XML tree in the specified XML file. + + Args: + filename (str): path to the file to write the XML in. + root (ET.Element): root element in the XML file. + """ + xml_str = self.get_string(root) + with open(filename, "w") as f: + f.write(xml_str) # .encode('utf-8')) diff --git a/pyrobolearn/utils/parsers/robots/sdf_parser.py b/pyrobolearn/utils/parsers/robots/sdf_parser.py index e69de29..51d706a 100644 --- a/pyrobolearn/utils/parsers/robots/sdf_parser.py +++ b/pyrobolearn/utils/parsers/robots/sdf_parser.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +"""Define the SDF parser. + +SDF files are notably used in Gazebo, and Bullet. +""" + +# import XML parser +import xml.etree.ElementTree as ET + +from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser +from pyrobolearn.utils.parsers.robots.data_structures import Tree + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class SDFParser(RobotParser): + r"""SDF Parser""" + + def __init__(self, filename=None): + """ + Initialize the SDF parser. + + Args: + filename (str, None): path to the MuJoCo XML file. + """ + super().__init__(filename) + + def parse(self, filename): + """ + Load and parse the given URDF file. + + Args: + filename (str): path to the MuJoCo XML file. + """ + # load and parse the XML file + tree = ET.parse(filename) + + # get the root + root = tree.getroot() + + # check that the root is + if root.tag != 'sdf': + raise RuntimeError("Expecting the first XML tag to be 'sdf' but found instead: {}".format(root.tag)) + + # build the tree + + def get_tree(self): + """ + Return the Tree containing all the elements. + + Returns: + Tree: tree data structure. + """ + pass + + def get_world(self): + """ + Return the world (which is basically a list of Tree). + """ + pass + + def generate(self, tree=None): + """ + Generate the XML tree from the `Tree` data structure. + + Args: + tree (Tree): Tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + pass + diff --git a/pyrobolearn/utils/parsers/robots/urdf_parser.py b/pyrobolearn/utils/parsers/robots/urdf_parser.py index e69de29..ea9b8af 100644 --- a/pyrobolearn/utils/parsers/robots/urdf_parser.py +++ b/pyrobolearn/utils/parsers/robots/urdf_parser.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python +"""Define the URDF parser. + +URDF files are notably used in ROS, Gazebo, Bullet, Dart, and MuJoCo. +""" + +# import XML parser +import xml.etree.ElementTree as ET + +from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser +from pyrobolearn.utils.parsers.robots.data_structures import * + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class URDFParser(RobotParser): + r"""URDF Parser""" + + def __init__(self, filename=None): + """ + Initialize the URDF parser. + + Args: + filename (str, None): path to the MuJoCo XML file. + """ + super().__init__(filename) + + def parse(self, filename): + """ + Load and parse the given URDF file. + + Args: + filename (str): path to the MuJoCo XML file. + """ + # load and parse the XML file + tree_xml = ET.parse(filename) + + # get the root + root = tree_xml.getroot() + + # check that the root is + if root.tag != 'robot': + raise RuntimeError("Expecting the first XML tag to be 'robot' but found instead: {}".format(root.tag)) + + # build the tree + tree = Tree(name=root.attrib.get('name')) + + # check materials + for i, material in enumerate(root.findall('material')): + attrib = material.attrib + mat = Material(name=attrib.get('name', 'material_' + str(i)), color=attrib.get('color')) + tree.materials[mat.name] = mat + + # check bodies / links + for i, body in enumerate(root.findall('link')): + attrib = body.attrib + b = Body(body_id=i, name=attrib.get('name', 'body_' + str(i))) + + # check tag + inertial = body.find('inertial') + if inertial is not None: + i = Inertial() + + # origin + origin = inertial.find('origin') + if origin is not None: + i.position = origin.attrib.get('xyz') + i.orientation = origin.attrib.get('rpy') + + # mass + mass = inertial.find('mass') + if mass is not None: + i.mass = mass.attrib.get('value') + + # inertia + inertia = inertial.find('inertia') + if inertia is not None: + i.inertia = {name: inertia.attrib.get(name) for name in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']} + + # set inertial to body + b.inertial = i + + # check tag + visual = body.find('visual') + if visual is not None: + v = Visual() + + # name + v.name = visual.attrib.get('name') + + # origin + origin = visual.find('origin') + if origin is not None: + v.position = origin.attrib.get('xyz') + v.orientation = origin.attrib.get('rpy') + + # geometry + geometry = visual.find('geometry') + if geometry is not None: + for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']: + geom = geometry.find(geometry_type) + if geom is not None: + dtype = geometry_type + v.dtype = dtype + if dtype == 'box': + v.size = geom.attrib['size'] + elif dtype == 'sphere': + v.size = geom.attrib['radius'] + elif dtype == 'cylinder': + v.size = (geom.attrib['radius'], geom.attrib['length']) + elif dtype == 'mesh': + v.filename = geom.attrib['filename'] + v.size = geom.attrib.get('scale') + + # material + material = visual.find('material') + if material is not None: + name = material.attrib.get('name') + color = material.find('color') + if color is not None: + v.color = color.attrib['rgba'] + else: + mat = tree.materials.get(name) + if mat is not None: + v.material = mat + + # set visual to body + b.visual = v + + # check tag + collision = body.find('collision') + if collision is not None: + c = Collision() + + # name + c.name = collision.attrib.get('name') + + # origin + origin = collision.find('origin') + if origin is not None: + c.position = origin.attrib.get('xyz') + c.orientation = origin.attrib.get('rpy') + + # geometry + geometry = collision.find('geometry') + if geometry is not None: + for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']: + geom = geometry.find(geometry_type) + if geom is not None: + dtype = geometry_type + c.dtype = dtype + if dtype == 'box': + c.size = geom.attrib['size'] + elif dtype == 'sphere': + c.size = geom.attrib['radius'] + elif dtype == 'cylinder': + c.size = (geom.attrib['radius'], geom.attrib['length']) + elif dtype == 'mesh': + c.filename = geom.attrib['filename'] + c.size = geom.attrib.get('scale') + + # set collision to body + b.collision = c + + # add body to tree + tree.bodies[b.name] = b + + # check joints + for i, joint in enumerate(root.findall('joint')): + attrib = joint.attrib + j = Joint(joint_id=i, name=attrib.get('name', 'joint_' + str(i)), dtype=attrib['type']) + + # add parent and child body/link + parent = joint.find('parent') + if parent is None: + raise RuntimeError("Expecting the joint '" + j.name + "' to have a parent link/body") + j.parent = parent.attrib['link'] + + child = joint.find('child') + if child is None: + raise RuntimeError("Expecting the joint '" + j.name + "' to have a child link/body") + j.child = child.attrib['link'] + + # origin + origin = joint.find('origin') + if origin is not None: + j.position = origin.attrib.get('xyz') + j.orientation = origin.attrib.get('rpy') + + # axis + axis = joint.find('axis') + if axis is not None: + j.axis = axis.attrib.get('xyz') + + # dynamics + dynamics = joint.find('dynamics') + if dynamics is not None: + j.damping = dynamics.attrib.get('damping') + j.friction = dynamics.attrib.get('friction') + + # limits + limits = joint.find('limits') + if limits is not None: + j.effort = limits.attrib.get('effort') + j.velocity = limits.attrib.get('velocity') + lower_limit = limits.attrib.get('lower') + upper_limit = limits.attrib.get('upper') + if lower_limit is not None and upper_limit is not None: # TODO: check if we can have one limit + j.limits = [lower_limit, upper_limit] + + # add joint in trees + tree.joints[j.name] = j + + # add joint in parent body + tree.bodies[j.parent] = j + + # set the tree + self.tree = tree + + def generate(self, tree=None): + """ + Generate the XML tree from the `Tree` data structure. + + Args: + tree (Tree): Tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + if tree is None: + tree = self.tree + + pass