diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py
index 0507983..8a171b2 100644
--- a/pyrobolearn/simulators/mujoco.py
+++ b/pyrobolearn/simulators/mujoco.py
@@ -62,7 +62,8 @@ except ImportError as e:
# import pyrobolearn related functionalities
from pyrobolearn.simulators.simulator import Simulator
-from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser, converter
+# from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser
+from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
# check Python version
@@ -237,8 +238,7 @@ class Mujoco(Simulator):
self._root = ET.Element("mujoco")
# create worldbody
- ET.SubElement(self._root, 'worldbody')
- self._worldbody = self._root.find('worldbody')
+ self._worldbody = ET.SubElement(self._root, 'worldbody')
# add a light
ET.SubElement(self._worldbody, "light", attrib={"diffuse": ".5 .5 .5", "pos": "0 0 3", "dir": "0 0 -1"})
@@ -572,8 +572,15 @@ class Mujoco(Simulator):
Returns:
int (non-negative): unique id associated to the load model.
"""
- # create xml file based on URDF file
- pass
+ # parse URDF file
+ urdf_parser = URDFParser(filename=filename)
+ mujoco_generator = MuJoCoParser()
+
+ # generate XML element
+ element = mujoco_generator.generate(urdf_parser.tree)
+
+ # append element to worldbody
+ self._worldbody.append(element)
def load_sdf(self, filename, scaling=1., *args, **kwargs):
"""Load a SDF file in the simulator.
@@ -585,16 +592,24 @@ class Mujoco(Simulator):
Returns:
list(int): list of object unique id for each object loaded
"""
- # parse sdf
- tree = ET.parse(filename)
- root = tree.getroot()
+ # parse sdf file
+ sdf_parser = SDFParser(filename=filename)
+ mujoco_generator = MuJoCoParser()
- # add bodies
- pass
+ # generate XML elements
+ elements = [mujoco_generator.generate(tree) for tree in sdf_parser.world.trees]
+
+ # append each element to worldbody
+ for element in elements:
+ self._worldbody.append(element)
def load_mjcf(self, filename, scaling=1., *args, **kwargs):
"""Load a Mujoco file in the simulator.
+ Warnings: this only loads the bodies, joints, and assets. It does not load other elements such as the physical
+ engine properties (number of iterations, solver, etc), physical properties (gravity, friction, viscosity, etc),
+ and others.
+
Args:
filename (str): a relative or absolute path to the MJCF file on the file system of the physics server.
scaling (float): scale factor for the object
@@ -602,11 +617,19 @@ class Mujoco(Simulator):
Returns:
list(int): list of object unique id for each object loaded
"""
- # update the world
+ # load MJCF # TODO: check if empty world
+ # self.model = mujoco.load_model_from_path(filename)
+ # self.sim = mujoco.MjSim(self.model)
- # load MJCF
- self.model = mujoco.load_model_from_path(filename)
- self.sim = mujoco.MjSim(self.model)
+ # parse MJCF file
+ parser = MuJoCoParser(filename=filename)
+
+ # generate XML elements
+ elements = [parser.generate(tree) for tree in parser.world.trees]
+
+ # append each element to worldbody
+ for element in elements:
+ self._worldbody.append(element)
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):
@@ -628,6 +651,10 @@ class Mujoco(Simulator):
Returns:
int: unique id of the mesh in the world
"""
+ # convert file '.obj' to '.stl' as MuJoCo only supports STL formats.
+
+ # try to look for textures and colors in the '.mtl' file
+
# create collision shape if specified
# create visual shape
@@ -635,6 +662,9 @@ class Mujoco(Simulator):
# create body
pass
+ def load_soft_body(self, shape=None, filename=None): # TODO
+ pass
+
##########
# Bodies #
##########
diff --git a/pyrobolearn/simulators/mujoco_empty_world.xml b/pyrobolearn/simulators/mujoco_empty_world.xml
deleted file mode 100644
index 43e655b..0000000
--- a/pyrobolearn/simulators/mujoco_empty_world.xml
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/pyrobolearn/utils/parsers/robots/__init__.py b/pyrobolearn/utils/parsers/robots/__init__.py
index eec5263..6444af6 100644
--- a/pyrobolearn/utils/parsers/robots/__init__.py
+++ b/pyrobolearn/utils/parsers/robots/__init__.py
@@ -1,9 +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
+
+# import sdf parser
+from .sdf_parser import SDFParser
diff --git a/pyrobolearn/utils/parsers/robots/converter.py b/pyrobolearn/utils/parsers/robots/converter.py
index 956779c..baf42a7 100644
--- a/pyrobolearn/utils/parsers/robots/converter.py
+++ b/pyrobolearn/utils/parsers/robots/converter.py
@@ -3,7 +3,7 @@
format.
"""
-from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser
+from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2019, PyRoboLearn"
@@ -16,10 +16,68 @@ __status__ = "Development"
class Converter(object):
- r"""Converter"""
+ r"""Converter
+
+ This converts one world / robot file to another one.
+ """
def __init__(self):
pass
def convert(self, from_filename, to_filename):
- pass
+ """
+ Convert one robot/world file to another one. If it is a world file to a robot file, it will create a robot file
+ for each model that were in the world.
+
+ Args:
+ from_filename (str): file to parse (specified with the extension).
+ to_filename (str, list of str): file to generate (specified with the extension). You can also only
+ specified the extension if you wish. If that is the case, the name will be taken from the file that
+ is being parsed.
+ """
+ # check the types
+ if not isinstance(from_filename, str):
+ raise TypeError("Expecting the 'from_filename' to be a str, but got instead: "
+ "{}".format(type(from_filename)))
+ if not isinstance(to_filename, str):
+ raise TypeError("Expecting the 'to_filename' to be a str, but got instead: "
+ "{}".format(type(to_filename)))
+
+ # extension for the 'from_filename'
+ from_extension = from_filename.split('.')[-1]
+ if from_extension == 'urdf': # URDF
+ parser = URDFParser(filename=from_filename)
+ elif from_extension == 'sdf' or from_extension == 'world': # SDF
+ parser = SDFParser(filename=from_filename)
+ elif from_extension == 'mjcf' or from_extension == 'xml': # MuJoCo
+ parser = MuJoCoParser(filename=from_filename)
+ elif from_extension == 'proto':
+ # parser = ProtoParser(filename=from_filename)
+ raise NotImplementedError("The proto parser has not been implemented yet")
+ else:
+ raise ValueError("Got the extension '{}' from 'from_filename', however this format is not "
+ "known".format(type(from_extension)))
+
+ # extension for the 'to_filename'
+ to_extension = to_filename.split('.')
+ if len(to_extension) == 1:
+ to_extension = to_filename
+ else:
+ to_extension = to_extension[-1]
+
+ # generator
+ if to_extension == 'urdf': # URDF
+ generator = URDFParser()
+ elif to_extension == 'sdf' or to_extension == 'world': # SDF
+ generator = SDFParser()
+ elif to_extension == 'mjcf' or to_extension == 'xml': # MuJoCo
+ generator = MuJoCoParser()
+ elif to_extension == 'proto':
+ # generator = ProtoParser()
+ raise NotImplementedError("The proto parser has not been implemented yet")
+ else:
+ raise ValueError("Got the extension '{}' from 'to_filename', however this format is not "
+ "known".format(type(from_extension)))
+
+ # generate the files
+ # TODO
diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py
index 8c78adf..d044c65 100644
--- a/pyrobolearn/utils/parsers/robots/data_structures.py
+++ b/pyrobolearn/utils/parsers/robots/data_structures.py
@@ -1,11 +1,11 @@
#!/usr/bin/env python
-"""Provide the data structures that are shared among the various parsers and converter.
+"""Provide the data structures that are shared among the various parsers and converters.
"""
import numpy as np
from collections import OrderedDict
-from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy, get_matrix_from_rpy
__author__ = "Brian Delhaisse"
@@ -18,11 +18,132 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
-class World(object):
- r"""World data structure."""
+class Simulator(object):
+ r"""Simulator data structure."""
- def __init__(self, trees=None):
- self.trees = trees
+ def __init__(self, world=None, physics_engine=None):
+ self.world = world
+ self.engine = physics_engine
+ self.physics = physics_properties
+
+ @property
+ def world(self):
+ return self._world
+
+ @world.setter
+ def world(self, world):
+ if world is not None and not isinstance(world, World):
+ raise TypeError("Expecting the world to be an instance of `World`, but got instead: "
+ "{}".format(type(world)))
+ self._world = world
+
+ @property
+ def engine(self):
+ return self._engine
+
+ @engine.setter
+ def engine(self, engine):
+ if engine is not None and not isinstance(engine, PhysicsEngine):
+ raise TypeError("Expecting the engine to be an instance of `PhysicsEngine`, but got instead: "
+ "{}".format(type(engine)))
+ self._engine = engine
+
+
+class PhysicsEngine(object):
+ r"""Physics Engine properties.
+
+ This include number of iterations, solver used, tolerance, timesteps, etc.
+ """
+
+ def __init__(self, timestep=None):
+ self.timestep = timestep
+ self.num_iterations = None
+ self.solver = None
+ self.tolerance = None
+
+
+class World(object):
+ r"""World data structure.
+
+ World frame (robotics convention with the right-hand rule):
+ - the x axis points forward
+ - the y axis points to the left
+ - the z axis points upward
+ """
+
+ def __init__(self, name=None):
+ self.name = name
+ self.trees = OrderedDict()
+ self.physics = None
+
+ @property
+ def physics(self):
+ return self._physics
+
+ @physics.setter
+ def physics(self, physics):
+ if physics is not None and not isinstance(physics, Physics):
+ raise TypeError("Expecting the physics to be an instance of `Physics`, but got instead: "
+ "{}".format(type(physics)))
+ self._physics = physics
+
+
+class Light(object):
+ r"""Light data structure.
+
+ Type of light: point, directional, and spot
+ """
+
+ def __init__(self, name=None, dtype=None, cast_shadows=None, diffuse=None, specular=None, attenuation=None,
+ direction=None, spot=None, position=None, orientation=None):
+ """
+ Initialize the Light data structure.
+
+ Args:
+ name (str): unique name for the light.
+ dtype (str): type of light, select between {'point', 'directional', 'spot'}
+ cast_shadows (bool): if True, it will cast shadows.
+ diffuse (tuple of 4 float, np.array[4]): diffuse light (RGBA) color.
+ specular (tuple of 4 float, np.array[4]): specular light (RGBA) color.
+ attenuation: light attenuation
+ direction (np.array[3]): direction of the light if dtype='directional' or dtype='spot'.
+ spot: spot light parameters
+ position (tuple/list of 3 float, np.array[3]): position of the light in the world.
+ orientation (tuple/list of 3 float, np.array[3]): orientation of the light in the world.
+ """
+ self.name = name
+ self.dtype =dtype
+ self.shadows = cast_shadows
+ self.diffuse = diffuse
+ self.specular = specular
+ self.attenuation = attenuation
+ self.direction = direction
+ self.spot = spot
+ self.position = position
+ self.orientation = orientation
+
+
+class Physics(object):
+ r"""Physical properties of the world.
+
+ This includes gravity, friction, viscosity, etc.
+ """
+
+ def __init__(self, gravity=(0., 0., -9.81)):
+ self.gravity = gravity
+
+
+class Frame(object):
+ r"""Reference Frame"""
+
+ def __init__(self, position=None, orientation=None, dtype=None, right_handed=True, forward_axis=(1., 0., 0.),
+ up_axis=(0., 0., 1.)):
+ self.position = position
+ self.orientation = orientation
+ self.dtype = dtype # world frame, body frame, joint frame, inertial frame, etc.
+ self.right_handed = right_handed
+ self.forward_axis = forward_axis
+ self.up_axis = up_axis
class Tree(object):
@@ -34,6 +155,71 @@ class Tree(object):
self.bodies = OrderedDict()
self.joints = OrderedDict()
self.materials = {}
+ self.position = None
+ self.orientation = None
+
+ @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)
+
+ @property
+ def rot(self):
+ return get_matrix_from_rpy(self.rpy)
+
+ @property
+ def pose(self):
+ return self.position, self.orientation
+
+ @pose.setter
+ def pose(self, pose):
+ if pose is not None:
+ if isinstance(pose, str):
+ pose = pose.split()
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ elif isinstance(pose, (tuple, list, np.ndarray)):
+ if len(pose) == 2:
+ self.position = pose[0]
+ self.orientation = pose[1]
+ elif len(pose) == 6:
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ else:
+ raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+ else:
+ raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
class Body(object):
@@ -55,7 +241,20 @@ class Body(object):
class Joint(object):
r"""Joint data structure.
- Joint types: fixed, revolute/hinge, continuous
+ Joint types: fixed, floating/free, prismatic, revolute/hinge, continuous, gearbox, revolute2, ball, screw,
+ universal, and planar.
+
+ - fixed: no motions is allowed; both links are rigidly attached to each other.
+ - floating/free: allows motion for all 6 degrees of motion.
+ - prismatic: allows motion along 1 translational DoF.
+ - revolute/hinge: allows rotational motion around one axis (1 DoF).
+ - continuous: a revolute/hinge joint that doesn't have lower or upper limits.
+ - gearbox: geared revolute joint.
+ - revolute2: two revolute joints connected in series
+ - ball: a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
+ - screw: a single DoF joint wich coupled sliding and rotational motion
+ - universal: like a ball joint, but constrains one DoF
+ - planar: allows motion in a plane perpendicular to the axis.
"""
def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None,
@@ -131,6 +330,41 @@ class Joint(object):
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)
+
+ @property
+ def rot(self):
+ return get_matrix_from_rpy(self.rpy)
+
+ @property
+ def pose(self):
+ return self.position, self.orientation
+
+ @pose.setter
+ def pose(self, pose):
+ if pose is not None:
+ if isinstance(pose, str):
+ pose = pose.split()
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ elif isinstance(pose, (tuple, list, np.ndarray)):
+ if len(pose) == 2:
+ self.position = pose[0]
+ self.orientation = pose[1]
+ elif len(pose) == 6:
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ else:
+ raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+ else:
+ raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
@property
def friction(self):
return self._friction
@@ -183,23 +417,49 @@ class Inertia(object):
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 diagonal_inertia(self):
+ """Aligned inertia.
+
+ Returns:
+ np.array[3]: principal moments of the inertia.
+
+ References:
+ - [1] https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_matrix_in_different_reference_frames
+ """
+ inertia = self.full_inertia
+ evals, evecs = np.linalg.eigh(inertia)
+ return evals
+
+ @property
+ def principal_inertia(self):
+ """Return the principal moments of the inertia (np.array[3]), and the direction of the principal axes of the
+ body (np.array[3,3])."""
+ inertia = self.full_inertia
+ evals, evecs = np.linalg.eigh(inertia)
+ return evals, evecs
+
+ @property
+ def principal_axes(self):
+ """Return the directions of the principal axes of the body as a 3x3 matrix where each column represents an
+ axis."""
+ inertia = self.full_inertia
+ evals, evecs = np.linalg.eigh(inertia)
+ return evecs
+
@property
def ixx(self):
return self._ixx
@ixx.setter
def ixx(self, ixx):
- if ixx is not None:
+ if ixx is None:
ixx = float(ixx)
self._ixx = ixx
@@ -257,7 +517,16 @@ class Inertia(object):
class Inertial(object):
r"""Inertial parameters."""
- def __init__(self, mass=None, inertia=None, position=None, orientation=None):
+ def __init__(self, mass=None, inertia=None, position=(0., 0., 0.), orientation=(0., 0., 0.)):
+ """
+
+ Args:
+ mass (float): mass value (in kg)
+ inertia (str, list / tuple of 3/6/9 float, np.ndarray[3/6/9], np.ndarray[3,3]): inertia matrix represented
+ in the body frame.
+ position (np.array[3], str): position of the center of mass.
+ orientation (np.array[3], str): rotation expressed as roll-pitch-yaw angles.
+ """
self.mass = mass
self.inertia = inertia
self.position = position
@@ -302,8 +571,35 @@ class Inertial(object):
self._inertia = inertia
@property
- def aligned_inertia(self):
- raise NotImplementedError
+ def full_inertia(self):
+ rot = self.rot
+ return rot.dot(self._inertia.full_inertia).dot(rot.T)
+
+ @property
+ def principal_inertia(self):
+ """Return the principal moments of the inertia (np.array[3]), and the direction of the principal axes of the
+ body (np.array[3,3])."""
+ evals, evecs = self.inertia.principal_inertia
+ return evals, self.rot.dot(evecs)
+
+ @property
+ def diagonal_inertia(self):
+ """Aligned inertia.
+
+ Returns:
+ np.array[3]: principal moments of the inertia.
+
+ References:
+ - [1] https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_matrix_in_different_reference_frames
+ """
+ return self.inertia.diagonal_inertia
+
+ @property
+ def principal_axes(self):
+ """Return the directions of the principal axes of the body as a 3x3 matrix where each column represents an
+ axis."""
+ evecs = self.inertia.principal_axes
+ return self.rot.dot(evecs)
@property
def position(self):
@@ -341,20 +637,41 @@ class Inertial(object):
def quaternion(self):
return get_quaternion_from_rpy(self._orientation)
+ @property
+ def rot(self):
+ return get_matrix_from_rpy(self.rpy)
-class Visual(object):
- r"""visual parameters for body."""
+ @property
+ def pose(self):
+ return self.position, self.orientation
- def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None,
- material=None):
- self.name = name
+ @pose.setter
+ def pose(self, pose):
+ if pose is not None:
+ if isinstance(pose, str):
+ pose = pose.split()
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ elif isinstance(pose, (tuple, list, np.ndarray)):
+ if len(pose) == 2:
+ self.position = pose[0]
+ self.orientation = pose[1]
+ elif len(pose) == 6:
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ else:
+ raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+ else:
+ raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
+
+class Geometry(object): # Shape
+ """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, heightmap, etc."""
+
+ def __init__(self, dtype=None, size=None, filename=None):
self.dtype = dtype
- self.size = size # depending on the type it can be different size
- self.color = color
+ self.size = size # depending on the type it can be different size
self.filename = filename
- self.position = position
- self.orientation = orientation
- self.material = material
@property
def size(self):
@@ -365,12 +682,63 @@ class Visual(object):
if size is not None:
if isinstance(size, str):
size = [float(s) for s in size.split()]
+ if len(size) == 1:
+ size = size[0]
elif isinstance(size, (tuple, list, np.ndarray)):
size = [float(s) for s in size]
+ if len(size) == 1:
+ size = size[0]
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):
+ """Return the filename format extension for the mesh."""
+ if self.filename is not None:
+ return self.filename.split('.')[-1]
+
+
+# alias
+Shape = Geometry
+
+
+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.geometry = Geometry(dtype=dtype, size=size, filename=filename)
+ self.color = color
+ self.position = position
+ self.orientation = orientation
+ self.material = material
+
+ @property
+ def dtype(self):
+ return self.geometry.dtype
+
+ @dtype.setter
+ def dtype(self, dtype):
+ self.geometry.dtype = dtype
+
+ @property
+ def size(self):
+ return self.geometry.size
+
+ @size.setter
+ def size(self, size):
+ self.geometry.size = size
+
+ @property
+ def filename(self):
+ return self.geometry.filename
+
+ @filename.setter
+ def filename(self, filename):
+ self.geometry.filename = filename
+
@property
def color(self):
return self._color
@@ -428,32 +796,66 @@ class Visual(object):
def quaternion(self):
return get_quaternion_from_rpy(self._orientation)
+ @property
+ def rot(self):
+ return get_matrix_from_rpy(self.rpy)
+
+ @property
+ def pose(self):
+ return self.position, self.orientation
+
+ @pose.setter
+ def pose(self, pose):
+ if pose is not None:
+ if isinstance(pose, str):
+ pose = pose.split()
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ elif isinstance(pose, (tuple, list, np.ndarray)):
+ if len(pose) == 2:
+ self.position = pose[0]
+ self.orientation = pose[1]
+ elif len(pose) == 6:
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ else:
+ raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+ else:
+ raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
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.geometry = Geometry(dtype=dtype, size=size, filename=filename)
self.position = position
self.orientation = orientation
+ @property
+ def dtype(self):
+ return self.geometry.dtype
+
+ @dtype.setter
+ def dtype(self, dtype):
+ self.geometry.dtype = dtype
+
@property
def size(self):
- return self._size
+ return self.geometry.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
+ self.geometry.size = size
+
+ @property
+ def filename(self):
+ return self.geometry.filename
+
+ @filename.setter
+ def filename(self, filename):
+ self.geometry.filename = filename
@property
def format(self):
@@ -496,13 +898,41 @@ class Collision(object):
def quaternion(self):
return get_quaternion_from_rpy(self._orientation)
+ @property
+ def rot(self):
+ return get_matrix_from_rpy(self.rpy)
+
+ @property
+ def pose(self):
+ return self.position, self.orientation
+
+ @pose.setter
+ def pose(self, pose):
+ if pose is not None:
+ if isinstance(pose, str):
+ pose = pose.split()
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ elif isinstance(pose, (tuple, list, np.ndarray)):
+ if len(pose) == 2:
+ self.position = pose[0]
+ self.orientation = pose[1]
+ elif len(pose) == 6:
+ self.position = pose[:3]
+ self.orientation = pose[3:]
+ else:
+ raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+ else:
+ raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
class Material(object):
r"""Material info."""
- def __init__(self, name=None, color=None):
+ def __init__(self, name=None, color=None, texture=None):
self.name = name
self.color = color
+ self.texture = texture
@property
def color(self):
@@ -532,3 +962,11 @@ class Material(object):
if len(self.color) == 3:
return tuple(self.color) + (1.,)
return tuple(self.color)
+
+
+class Sensor(object):
+ pass
+
+
+class Heightmap(object):
+ pass
diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
index ef05f5f..20157f3 100644
--- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py
+++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
@@ -4,10 +4,9 @@
# 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
+from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
+from pyrobolearn.utils.parsers.robots.data_structures import Tree, World
__author__ = "Brian Delhaisse"
@@ -20,8 +19,8 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
-class MuJoCoParser(RobotParser):
- r"""MuJoCo Parser"""
+class MuJoCoParser(WorldParser):
+ r"""MuJoCo Parser and Generator"""
def __init__(self, filename=None):
"""
@@ -40,23 +39,69 @@ class MuJoCoParser(RobotParser):
filename (str): path to the MuJoCo XML file.
"""
# load and parse the XML file
- tree = ET.parse(filename)
+ tree_xml = ET.parse(filename)
# get the root
- root = tree.getroot()
+ root = tree_xml.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
+ # build the world
+ world = World()
- def get_tree(self):
+ # check default (this is the default configuration when they are not specified)
+ default_tag = root.find('default')
+ if default_tag is not None:
+ pass
+
+ # check physics
+
+ # check assets
+ asset_tag = root.find('asset')
+ if asset_tag is not None:
+ pass
+
+ # check world body
+ worldbody_tag = root.find('worldbody')
+ if worldbody_tag is not None:
+ pass
+
+ # check contact
+
+ # check equality constraint
+
+ # check actuator
+
+ # check sensor
+
+ # set the world
+ self.world = world
+
+ def _check_body(self, body_tag, idx):
"""
- Return the Tree containing all the elements.
+ Return Body instance from a .
+
+ Args:
+ body_tag (ET.Element): body XML element.
+ idx (int): link index.
Returns:
- Tree: tree data structure.
+ Body: body data structure.
+ """
+ pass
+
+ def _check_joint(self, joint_tag, idx):
+ """
+ Return Joint instance from a tag.
+
+ Args:
+ joint_tag (ET.Element): joint XML element.
+ idx (int): joint index.
+
+ Returns:
+ Joint: joint data structure.
"""
pass
diff --git a/pyrobolearn/utils/parsers/robots/proto_parser.py b/pyrobolearn/utils/parsers/robots/proto_parser.py
index 1ca5d71..fadd992 100644
--- a/pyrobolearn/utils/parsers/robots/proto_parser.py
+++ b/pyrobolearn/utils/parsers/robots/proto_parser.py
@@ -19,23 +19,23 @@ __status__ = "Development"
class ProtoParser(RobotParser):
- r"""Proto Parser"""
+ r"""Proto Parser and Generator."""
def __init__(self, filename=None):
"""
Initialize the Proto parser.
Args:
- filename (str, None): path to the MuJoCo XML file.
+ filename (str, None): path to the proto file.
"""
super().__init__(filename)
def parse(self, filename):
"""
- Load and parse the given URDF file.
+ Load and parse the given proto file.
Args:
- filename (str): path to the MuJoCo XML file.
+ filename (str): path to the proto file.
"""
pass
diff --git a/pyrobolearn/utils/parsers/robots/robot_parser.py b/pyrobolearn/utils/parsers/robots/robot_parser.py
index bd97f70..b554ada 100644
--- a/pyrobolearn/utils/parsers/robots/robot_parser.py
+++ b/pyrobolearn/utils/parsers/robots/robot_parser.py
@@ -19,14 +19,14 @@ __status__ = "Development"
class RobotParser(object):
- r"""Robot Parser"""
+ r"""Robot Parser and Generator."""
def __init__(self, filename=None):
"""
Initialize the robot parser.
Args:
- filename (str, None): path to the MuJoCo XML file.
+ filename (str, None): path to the file to parse.
"""
self.root = None
self.tree = None
@@ -34,12 +34,34 @@ class RobotParser(object):
if filename is not None:
self.parse(filename)
+ @property
+ def root(self):
+ return self._root
+
+ @root.setter
+ def root(self, root):
+ if root is not None and not isinstance(root, ET.Element):
+ raise TypeError("Expecting the root to be an instance of `ET.Element`, but got instead: "
+ "{}".format(type(root)))
+ self._root = root
+
+ @property
+ def tree(self):
+ return self._tree
+
+ @tree.setter
+ def tree(self, tree):
+ if tree is not None and not isinstance(tree, Tree):
+ raise TypeError("Expecting the given tree to be an instance of `Tree`, but got instead: "
+ "{}".format(type(tree)))
+ self._tree = tree
+
def parse(self, filename):
"""
- Load and parse a given MuJoCo XML filename.
+ Load and parse a given file.
Args:
- filename (str): path to the MuJoCo XML file.
+ filename (str): path to the file to parse.
"""
pass
diff --git a/pyrobolearn/utils/parsers/robots/sdf_parser.py b/pyrobolearn/utils/parsers/robots/sdf_parser.py
index 51d706a..2719544 100644
--- a/pyrobolearn/utils/parsers/robots/sdf_parser.py
+++ b/pyrobolearn/utils/parsers/robots/sdf_parser.py
@@ -7,8 +7,8 @@ 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
+from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
+from pyrobolearn.utils.parsers.robots.data_structures import *
__author__ = "Brian Delhaisse"
@@ -21,61 +21,350 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
-class SDFParser(RobotParser):
- r"""SDF Parser"""
+class SDFParser(WorldParser):
+ r"""SDF Parser and Generator"""
def __init__(self, filename=None):
"""
Initialize the SDF parser.
Args:
- filename (str, None): path to the MuJoCo XML file.
+ filename (str, None): path to the SDF file.
"""
super().__init__(filename)
+ self.worlds = []
def parse(self, filename):
"""
- Load and parse the given URDF file.
+ Load and parse the given SDF file.
Args:
- filename (str): path to the MuJoCo XML file.
+ filename (str): path to the SDF file.
"""
# load and parse the XML file
- tree = ET.parse(filename)
+ tree_xml = ET.parse(filename)
# get the root
- root = tree.getroot()
+ root = tree_xml.getroot()
- # check that the root is
+ # 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
+ # check world(s)
+ for i, world_tag in enumerate(root.findall('world')):
+ # build the world
+ world = World(name=world_tag.attrib.get('name', 'world_' + str(i)))
- def get_tree(self):
- """
- Return the Tree containing all the elements.
+ # check model
+ for idx, model_tag in enumerate(root.findall('model')):
+ tree = self._check_model(model_tag, idx=idx)
+ world.trees[tree.name] = tree
- Returns:
- Tree: tree data structure.
- """
- pass
+ # check physics
- def get_world(self):
- """
- Return the world (which is basically a list of Tree).
- """
- pass
+ # append the world to the list of worlds
+ self.worlds.append(world)
- def generate(self, tree=None):
+ # check model
+ models = root.findall('model')
+ if len(models) > 0:
+ world = World()
+ for i, model_tag in enumerate(models):
+ tree = self._check_model(model_tag, idx=i)
+ world.trees[tree.name] = tree
+ if len(models) > 0:
+ self.worlds.append(world)
+
+ def _check_model(self, model_tag, idx):
"""
- Generate the XML tree from the `Tree` data structure.
+ Return the Tree instance from a .
Args:
- tree (Tree): Tree data structure.
+ model_tag (ET.Element): model XML element
+ idx (int): model index.
+
+ Returns:
+ Tree: tree data structure containing the model.
+ """
+ # create tree
+ tree = Tree(name=model_tag.attrib.get('name'))
+
+ # check bodies/links
+ for i, link_tag in enumerate(model_tag.findall('link')):
+ body = self._check_body(link_tag, idx=i)
+ # add body to tree
+ tree.bodies[body.name] = body
+
+ # check joints
+ for i, joint_tag in enumerate(root.findall('joint')):
+ # get joint instance from tag
+ joint = self._check_joint(joint_tag, idx=i)
+
+ # add joint in trees
+ tree.joints[joint.name] = joint
+
+ # add joint in parent body
+ parent_body = tree.bodies[joint.parent]
+ parent_body.joints[joint.name] = joint
+
+ return tree
+
+ @staticmethod
+ def _check_body(body_tag, idx):
+ """
+ Return Body instance from a .
+
+ Args:
+ body_tag (ET.Element): link XML element.
+ idx (int): link index.
+
+ Returns:
+ Body: body data structure.
+ """
+ # create body/link
+ body = Body(body_id=idx, name=body_tag.attrib.get('name', 'body_' + str(idx)))
+
+ # check tag
+ inertial_tag = body_tag.find('inertial')
+ if inertial_tag is not None:
+ inertial = Inertial()
+
+ # pose
+ pose_tag = inertial_tag.find('pose')
+ if pose_tag is not None:
+ inertial.pose = pose_tag.text
+
+ # mass
+ mass_tag = inertial_tag.find('mass')
+ if mass_tag is not None:
+ inertial.mass = mass_tag.text
+
+ # inertia
+ inertia_tag = inertial_tag.find('inertia')
+ if inertia_tag is not None:
+ ixx = inertia_tag.find('ixx')
+ if ixx is not None:
+ ixx = ixx.text
+ ixy = inertia_tag.find('ixy')
+ if ixy is not None:
+ ixy = ixy.text
+ ixz = inertia_tag.find('ixz')
+ if ixz is not None:
+ ixz = ixz.text
+ iyy = inertia_tag.find('iyy')
+ if iyy is not None:
+ iyy = iyy.text
+ iyz = inertia_tag.find('iyz')
+ if iyz is not None:
+ iyz = iyz.text
+ izz = inertia_tag.find('izz')
+ if izz is not None:
+ izz = izz.text
+ inertial.inertia = {'ixx': ixx, 'ixy': ixy, 'ixz': ixz, 'iyy': iyy, 'iyz': iyz, 'izz': izz}
+
+ # set inertial to body
+ body.inertial = inertial
+
+ # check tag
+ visual_tag = body_tag.find('visual')
+ if visual_tag is not None:
+ visual = Visual()
+
+ # name
+ visual.name = visual_tag.attrib.get('name')
+
+ # pose
+ pose_tag = visual_tag.find('pose')
+ if pose_tag is not None:
+ visual.pose = pose_tag.text
+
+ # geometry
+ geometry_tag = visual_tag.find('geometry')
+ if geometry_tag is not None:
+ for geometry_type in ['box', 'mesh', 'cylinder', 'sphere', 'plane', 'heightmap']: # polyline, image
+ geometry_type_tag = geometry_tag.find(geometry_type)
+ if geometry_type_tag is not None:
+ dtype = geometry_type
+ visual.dtype = dtype
+ if dtype == 'box':
+ size_tag = geometry_type_tag.find('size')
+ visual.size = size_tag.text
+ elif dtype == 'sphere':
+ radius_tag = geometry_type_tag.find('radius')
+ visual.size = radius_tag.text
+ elif dtype == 'cylinder':
+ radius_tag = geometry_type_tag.find('radius')
+ length_tag = geometry_type_tag.find('length')
+ visual.size = (radius_tag.text, length_tag.text)
+ elif dtype == 'mesh':
+ uri_tag = geometry_type_tag.find('uri')
+ scale_tag = geometry_type_tag.find('scale')
+ visual.filename = uri_tag.text
+ visual.size = scale_tag.text
+
+ # 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
+ body.visual = visual
+
+ # check tag
+ collision_tag = body_tag.find('collision')
+ if collision_tag is not None:
+ collision = Collision()
+
+ # name
+ collision.name = collision_tag.attrib.get('name')
+
+ # origin
+ pose_tag = collision_tag.find('pose')
+ if pose_tag is not None:
+ collision.pose = pose_tag.text
+
+ # geometry
+ geometry_tag = collision_tag.find('geometry')
+ if geometry_tag is not None:
+ for geometry_type in ['box', 'mesh', 'cylinder', 'sphere', 'plane', 'heightmap']: # polyline, image
+ geometry_type_tag = geometry_tag.find(geometry_type)
+ if geometry_type_tag is not None:
+ dtype = geometry_type
+ collision.dtype = dtype
+ if dtype == 'box':
+ size_tag = geometry_type_tag.find('size')
+ collision.size = size_tag.text
+ elif dtype == 'sphere':
+ radius_tag = geometry_type_tag.find('radius')
+ collision.size = radius_tag.text
+ elif dtype == 'cylinder':
+ radius_tag = geometry_type_tag.find('radius')
+ length_tag = geometry_type_tag.find('length')
+ collision.size = (radius_tag.text, length_tag.text)
+ elif dtype == 'mesh':
+ uri_tag = geometry_type_tag.find('uri')
+ scale_tag = geometry_type_tag.find('scale')
+ collision.filename = uri_tag.text
+ collision.size = scale_tag.text
+
+ # set collision to body
+ body.collision = collision
+
+ # return the body instance
+ return body
+
+ @staticmethod
+ def _check_joint(joint_tag, idx):
+ """
+ Return Joint instance from a tag.
+
+ Args:
+ joint_tag (ET.Element): joint XML element.
+ idx (int): joint index.
+
+ Returns:
+ Joint: joint data structure.
+ """
+ attrib = joint_tag.attrib
+ joint = Joint(joint_id=idx, name=attrib.get('name', 'joint_' + str(idx)), dtype=attrib['type'])
+
+ # add parent and child body/link
+ parent_tag = joint_tag.find('parent')
+ if parent_tag is None:
+ raise RuntimeError("Expecting the joint '" + joint.name + "' to have a parent link/body")
+ joint.parent = parent_tag.text
+
+ child_tag = joint_tag.find('child')
+ if child_tag is None:
+ raise RuntimeError("Expecting the joint '" + joint.name + "' to have a child link/body")
+ joint.child = child_tag.text
+
+ # pose
+ pose_tag = joint_tag.find('pose')
+ if pose_tag is not None:
+ joint.pose = pose_tag.text
+
+ # axis
+ axis_tag = joint_tag.find('axis')
+ if axis_tag is not None:
+ axis_xyz_tag = axis_tag.find('xyz')
+ if axis_xyz_tag is not None:
+ joint.axis = axis_xyz_tag.text
+
+ # dynamics
+ dynamics_tag = axis_tag.find('dynamics')
+ if dynamics_tag is not None:
+ damping_tag = dynamics_tag.find('damping')
+
+ # damping
+ if damping_tag is not None:
+ joint.damping = damping_tag.text
+
+ # friction
+ friction_tag = dynamics_tag.find('friction')
+ if friction_tag is not None:
+ joint.friction = friction_tag.text
+
+ # limits
+ limits_tag = axis_tag.find('limits')
+ if limits_tag is not None:
+ effort_tag = limits_tag.find('effort')
+ if effort_tag is not None:
+ joint.effort = effort_tag.text
+
+ velocity_tag = limits_tag.find('velocity')
+ if velocity_tag is not None:
+ joint.velocity = velocity_tag.text
+
+ lower_limit_tag = limits_tag.find('lower')
+ upper_limit_tag = limits_tag.find('upper')
+ if lower_limit_tag is not None and upper_limit_tag is not None: # TODO: check if we can have one limit
+ joint.limits = [lower_limit_tag.text, upper_limit_tag.text]
+
+ return joint
+
+ def generate(self, world=None):
+ """
+ Generate the XML world from the `World` data structure.
+
+ Args:
+ world (World, Tree): world / tree data structure.
Returns:
ET.Element: root element in the XML file.
"""
- pass
+ if world is None:
+ world = self.worlds[0]
+ # create root element
+ root = ET.Element('sdf', attrib={'version': '1.6'})
+
+ # create world tag
+ name = world.name if world.name is not None else 'default'
+ world_tag = ET.SubElement(root, 'world', attrib={'name': name})
+
+ # create models
+ for tree in world.trees:
+ model_tag = ET.SubElement(world_tag, 'model', attrib={'name': tree.name})
+ if tree.position is not None or tree.orientation is not None:
+ pose_tag = ET.SubElement(model_tag, 'pose')
+ pose_tag.text = str(np.asarray(tree.pose))[1:-1]
+
+ # create links
+ for body in tree.bodies: # TODO
+ pass
+
+ # create joints
+ for joint in tree.joints: # TODO
+ pass
+
+ # return root XML element
+ return root
diff --git a/pyrobolearn/utils/parsers/robots/urdf_parser.py b/pyrobolearn/utils/parsers/robots/urdf_parser.py
index ea9b8af..3d0f1ea 100644
--- a/pyrobolearn/utils/parsers/robots/urdf_parser.py
+++ b/pyrobolearn/utils/parsers/robots/urdf_parser.py
@@ -22,14 +22,14 @@ __status__ = "Development"
class URDFParser(RobotParser):
- r"""URDF Parser"""
+ r"""URDF Parser and Generator"""
def __init__(self, filename=None):
"""
Initialize the URDF parser.
Args:
- filename (str, None): path to the MuJoCo XML file.
+ filename (str, None): path to the URDF XML file.
"""
super().__init__(filename)
@@ -38,7 +38,7 @@ class URDFParser(RobotParser):
Load and parse the given URDF file.
Args:
- filename (str): path to the MuJoCo XML file.
+ filename (str): path to the URDF XML file.
"""
# load and parse the XML file
tree_xml = ET.parse(filename)
@@ -54,177 +54,227 @@ class URDFParser(RobotParser):
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
+ for i, material_tag in enumerate(root.findall('material')):
+ attrib = material_tag.attrib
+ material = Material(name=attrib.get('name', 'material_' + str(i)))
+ color_tag = material_tag.find('color')
+ if color_tag is not None:
+ material.color = color_tag.attrib.get('rgba')
+ texture_tag = material_tag.find('texture')
+ if texture_tag is not None:
+ material.texture = texture_tag.attrib.get('filename')
+ tree.materials[material.name] = material
# 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
+ for i, body_tag in enumerate(root.findall('link')):
+ # get body instance from tag
+ body = self._check_body(tree, body_tag, idx=i)
# add body to tree
- tree.bodies[b.name] = b
+ tree.bodies[body.name] = body
# 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]
+ for i, joint_tag in enumerate(root.findall('joint')):
+ # get joint instance from tag
+ joint = self._check_joint(joint_tag, idx=i)
# add joint in trees
- tree.joints[j.name] = j
+ tree.joints[joint.name] = joint
# add joint in parent body
- tree.bodies[j.parent] = j
+ parent_body = tree.bodies[joint.parent]
+ parent_body.joints[joint.name] = joint
+
+ # TODO: check sensor, plugins, transmission, etc
# set the tree
self.tree = tree
+ @staticmethod
+ def _check_body(tree, body_tag, idx):
+ """
+ Return Body instance from a tag.
+
+ Args:
+ tree (Tree): Tree data structure.
+ body_tag (ET.Element): link XML element.
+ idx (int): link index.
+
+ Returns:
+ Body: body data structure.
+ """
+ attrib = body_tag.attrib
+ body = Body(body_id=idx, name=attrib.get('name', 'body_' + str(idx)))
+
+ # check tag
+ inertial_tag = body_tag.find('inertial')
+ if inertial_tag is not None:
+ inertial = Inertial()
+
+ # origin
+ origin_tag = inertial_tag.find('origin')
+ if origin_tag is not None:
+ inertial.position = origin_tag.attrib.get('xyz')
+ inertial.orientation = origin_tag.attrib.get('rpy')
+
+ # mass
+ mass_tag = inertial_tag.find('mass')
+ if mass_tag is not None:
+ inertial.mass = mass_tag.attrib.get('value')
+
+ # inertia
+ inertia_tag = inertial_tag.find('inertia')
+ if inertia_tag is not None:
+ inertial.inertia = {name: inertia_tag.attrib.get(name)
+ for name in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']}
+
+ # set inertial to body
+ body.inertial = inertial
+
+ # check tag
+ visual_tag = body_tag.find('visual')
+ if visual_tag is not None:
+ visual = Visual()
+
+ # name
+ visual.name = visual_tag.attrib.get('name')
+
+ # origin
+ origin_tag = visual_tag.find('origin')
+ if origin_tag is not None:
+ visual.position = origin_tag.attrib.get('xyz')
+ visual.orientation = origin_tag.attrib.get('rpy')
+
+ # geometry
+ geometry_tag = visual_tag.find('geometry')
+ if geometry_tag is not None:
+ for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+ geometry_type_tag = geometry_tag.find(geometry_type)
+ if geometry_type_tag is not None:
+ dtype = geometry_type
+ visual.dtype = dtype
+ if dtype == 'box':
+ visual.size = geometry_type_tag.attrib['size']
+ elif dtype == 'sphere':
+ visual.size = geometry_type_tag.attrib['radius']
+ elif dtype == 'cylinder':
+ visual.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+ elif dtype == 'mesh':
+ visual.filename = geometry_type_tag.attrib['filename']
+ visual.size = geometry_type_tag.attrib.get('scale')
+
+ # material
+ material_tag = visual_tag.find('material')
+ if material_tag is not None:
+ material = Material()
+ name = material_tag.attrib.get('name')
+ color = material_tag.find('color')
+ texture = material_tag.find('texture')
+ if color is not None or texture is not None:
+ material.name = name
+ if color is not None:
+ material.color = color.attrib['rgba']
+ elif texture is not None:
+ material.texture = texture.attrib['filename']
+ else:
+ material = tree.materials.get(name)
+ visual.material = material
+
+ # set visual to body
+ body.visual = visual
+
+ # check tag
+ collision_tag = body_tag.find('collision')
+ if collision_tag is not None:
+ collision = Collision()
+
+ # name
+ collision.name = collision_tag.attrib.get('name')
+
+ # origin
+ origin_tag = collision_tag.find('origin')
+ if origin_tag is not None:
+ collision.position = origin_tag.attrib.get('xyz')
+ collision.orientation = origin_tag.attrib.get('rpy')
+
+ # geometry
+ geometry_tag = collision_tag.find('geometry')
+ if geometry_tag is not None:
+ for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+ geometry_type_tag = geometry_tag.find(geometry_type)
+ if geometry_type_tag is not None:
+ dtype = geometry_type
+ collision.dtype = dtype
+ if dtype == 'box':
+ collision.size = geometry_type_tag.attrib['size']
+ elif dtype == 'sphere':
+ collision.size = geometry_type_tag.attrib['radius']
+ elif dtype == 'cylinder':
+ collision.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+ elif dtype == 'mesh':
+ collision.filename = geometry_type_tag.attrib['filename']
+ collision.size = geometry_type_tag.attrib.get('scale')
+
+ # set collision to body
+ body.collision = collision
+
+ return body
+
+ @staticmethod
+ def _check_joint(joint_tag, idx):
+ """
+ Return Joint instance from a tag.
+
+ Args:
+ joint_tag (ET.Element): joint XML element.
+ idx (int): joint index.
+
+ Returns:
+ Joint: joint data structure.
+ """
+ attrib = joint_tag.attrib
+ joint = Joint(joint_id=idx, name=attrib.get('name', 'joint_' + str(idx)), dtype=attrib['type'])
+
+ # add parent and child body/link
+ parent_tag = joint_tag.find('parent')
+ if parent_tag is None:
+ raise RuntimeError("Expecting the joint '" + joint.name + "' to have a parent link/body")
+ joint.parent = parent_tag.attrib['link']
+
+ child_tag = joint_tag.find('child')
+ if child_tag is None:
+ raise RuntimeError("Expecting the joint '" + joint.name + "' to have a child link/body")
+ joint.child = child_tag.attrib['link']
+
+ # origin
+ origin_tag = joint_tag.find('origin')
+ if origin_tag is not None:
+ joint.position = origin_tag.attrib.get('xyz')
+ joint.orientation = origin_tag.attrib.get('rpy')
+
+ # axis
+ axis_tag = joint_tag.find('axis')
+ if axis_tag is not None:
+ joint.axis = axis_tag.attrib.get('xyz')
+
+ # dynamics
+ dynamics_tag = joint_tag.find('dynamics')
+ if dynamics_tag is not None:
+ joint.damping = dynamics_tag.attrib.get('damping')
+ joint.friction = dynamics_tag.attrib.get('friction')
+
+ # limits
+ limits_tag = joint_tag.find('limits')
+ if limits_tag is not None:
+ joint.effort = limits_tag.attrib.get('effort')
+ joint.velocity = limits_tag.attrib.get('velocity')
+ lower_limit = limits_tag.attrib.get('lower')
+ upper_limit = limits_tag.attrib.get('upper')
+ if lower_limit is not None and upper_limit is not None: # TODO: check if we can have one limit
+ joint.limits = [lower_limit, upper_limit]
+
+ return joint
+
def generate(self, tree=None):
"""
Generate the XML tree from the `Tree` data structure.
@@ -238,4 +288,161 @@ class URDFParser(RobotParser):
if tree is None:
tree = self.tree
- pass
+ # create root element
+ root = ET.Element('robot')
+
+ # generate material tags
+ for material in tree.materials:
+ material_tag = ET.SubElement(root, 'material', attrib={'name': material.name})
+ if material.color is not None:
+ ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
+ if material.texture is not None:
+ ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})
+
+ # define some common functions
+ def set_name(parent_tag, tag, item):
+ attrib = {}
+ if item.name is not None:
+ attrib['name'] = item.name
+ new_tag = ET.SubElement(parent_tag, tag, attrib=attrib)
+ return new_tag
+
+ def set_origin(tag, item):
+ origin = {}
+ if item.position is not None:
+ origin['xyz'] = str(np.asarray(item.position))[1:-1]
+ if item.orientation is not None:
+ origin['rpy'] = str(np.asarray(item.orientation))[1:-1]
+ if len(origin) > 0:
+ ET.SubElement(tag, 'origin', attrib=origin)
+
+ def set_geometry(tag, item):
+ if item.geometry is not None:
+ geometry_tag = ET.SubElement(tag, 'geometry')
+ geometry = item.geometry
+ dtype = geometry.dtype
+
+ if dtype in {'box', 'sphere', 'cylinder', 'mesh'}:
+ attrib = {}
+ if dtype == 'box':
+ attrib['size'] = str(np.asarray(geometry.size))[1:-1]
+ elif dtype == 'sphere':
+ attrib['radius'] = str(geometry.size)
+ elif dtype == 'cylinder':
+ attrib['radius'] = str(geometry.size[0])
+ attrib['length'] = str(geometry.size[1])
+ else: # mesh
+ attrib['filename'] = geometry.filename
+ attrib['scale'] = str(np.asarray(geometry.size))[1:-1]
+
+ ET.SubElement(geometry_tag, dtype, attrib=attrib)
+
+ # generate
+ for link in tree.bodies:
+ link_tag = ET.SubElement(root, 'link', attrib={'name': link.name})
+
+ # create tag
+ inertial = link.inertial
+ if inertial is not None:
+ inertial_tag = ET.SubElement(link_tag, 'inertial')
+
+ #
+ set_origin(inertial_tag, inertial)
+
+ #
+ if inertial.mass is not None:
+ ET.SubElement(inertial_tag, 'mass', attrib={'value': str(inertial.mass)})
+
+ #
+ if inertial.inertia is not None:
+ I = inertial.inertia
+ inertia = {}
+ if I.ixx is not None:
+ inertia['ixx'] = str(I.ixx)
+ if I.iyy is not None:
+ inertia['iyy'] = str(I.iyy)
+ if I.izz is not None:
+ inertia['izz'] = str(I.izz)
+ if I.ixy is not None:
+ inertia['ixy'] = str(I.ixy)
+ if I.ixz is not None:
+ inertia['ixz'] = str(I.ixz)
+ if I.iyz is not None:
+ inertia['iyz'] = str(I.iyz)
+ ET.SubElement(inertial_tag, 'inertia', attrib=inertia)
+
+ # create tag
+ visual = link.visual
+ if visual is not None:
+ # create visual tag with name
+ visual_tag = set_name(link_tag, 'visual', visual)
+
+ #
+ set_origin(visual_tag, visual)
+
+ #
+ set_geometry(visual_tag, visual)
+
+ #
+ if visual.material is not None:
+ material = visual.material
+ material_tag = ET.SubElement(visual, 'material', attrib={'name': material.name})
+ if material.color is not None:
+ ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
+ if material.texture is not None:
+ ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})
+
+ # create tag
+ collision = link.collision
+ if collision is not None:
+ # create collision tag with name
+ collision_tag = set_name(link_tag, 'collision', collision)
+
+ #
+ set_origin(collision_tag, collision)
+
+ #
+ set_geometry(collision_tag, collision)
+
+ def set_name_and_type(parent_tag, tag, item):
+ kwargs = {}
+ if item.name is not None:
+ kwargs['name'] = item.name
+ if item.dtype is not None:
+ kwargs['type'] = item.dtype
+ return ET.SubElement(parent_tag, tag, attrib=kwargs)
+
+ # generate
+ for joint in tree.joints:
+ # set joint name and type
+ joint_tag = set_name_and_type(root, 'joint', joint)
+
+ #
+ set_origin(joint_tag, joint)
+
+ #
+ if joint.parent is not None:
+ ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent})
+
+ #
+ if joint.child is not None:
+ ET.SubElement(joint_tag, 'child', attrib={'link': joint.child})
+
+ #
+ if joint.axis is not None:
+ ET.SubElement(joint_tag, 'axis', attrib={'xyz': str(np.asarray(joint.axis))[1:-1]})
+
+ #
+ if joint.limits is not None or joint.effort is not None or joint.velocity is not None:
+ kwargs = {}
+ if joint.effort is not None:
+ kwargs['effort'] = str(joint.effort)
+ if joint.velocity is not None:
+ kwargs['velocity'] = str(joint.velocity)
+ if joint.limits is not None:
+ kwargs['lower'] = str(joint.limits[0])
+ kwargs['upper'] = str(joint.limits[1])
+ ET.SubElement(joint_tag, 'limit', attrib=kwargs)
+
+ # return root XML element
+ return root
diff --git a/pyrobolearn/utils/parsers/robots/world_parser.py b/pyrobolearn/utils/parsers/robots/world_parser.py
new file mode 100644
index 0000000..fc5ca94
--- /dev/null
+++ b/pyrobolearn/utils/parsers/robots/world_parser.py
@@ -0,0 +1,129 @@
+#!/usr/bin/env python
+"""Define the abstract world 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 World, 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 WorldParser(object):
+ r"""World Parser and Generator."""
+
+ def __init__(self, filename=None):
+ """
+ Initialize the world parser.
+
+ Args:
+ filename (str, None): path to the file to parse.
+ """
+ self.root = None
+ self.world = None
+ self.filename = filename
+ if filename is not None:
+ self.parse(filename)
+
+ @property
+ def root(self):
+ return self._root
+
+ @root.setter
+ def root(self, root):
+ if root is not None and not isinstance(root, ET.Element):
+ raise TypeError("Expecting the root to be an instance of `ET.Element`, but got instead: "
+ "{}".format(type(root)))
+ self._root = root
+
+ @property
+ def world(self):
+ return self._world
+
+ @world.setter
+ def world(self, world):
+ if world is not None and not isinstance(world, World):
+ raise TypeError("Expecting the given world to be an instance of `World` but got instead: "
+ "{}".format(type(world)))
+ self._world = world
+
+ def parse(self, filename):
+ """
+ Load and parse a given file.
+
+ Args:
+ filename (str): path to the file to parse.
+ """
+ pass
+
+ def get_tree(self, index=None, tag=None):
+ """
+ Get the specified tree(s).
+
+ Args:
+ index (int, None): tree index. If None, it will return all the trees.
+ tag (str, None): tag of the root that we want.
+
+ Returns:
+ (list of) Tree: tree data structure(s).
+ """
+ pass
+
+ def get_world(self):
+ """
+ Return the world containing all the elements that compose that world.
+
+ Returns:
+ World: World data structure.
+ """
+ return self.world
+
+ def generate(self, world=None):
+ """
+ Generate the XML world from the `World` data structure.
+
+ Args:
+ world (World): world 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 world 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'))