diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py
index fb4180d..7c5aa0c 100644
--- a/pyrobolearn/simulators/bullet.py
+++ b/pyrobolearn/simulators/bullet.py
@@ -84,7 +84,7 @@ class Bullet(Simulator):
Erwin Coumans and Yunfei Bai, 2017/2018
"""
- def __init__(self, render=True, num_instances=1, **kwargs):
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
"""
Initialize the PyBullet simulator.
@@ -92,6 +92,7 @@ class Bullet(Simulator):
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
**kwargs (dict): optional arguments (this is not used here).
+ middleware (MiddleWare, None): middleware instance.
"""
# try to import the pybullet library
# normally that should be done outside the class but because it might have some conflicts with other libraries
@@ -99,7 +100,7 @@ class Bullet(Simulator):
# import pybullet_data
# from pybullet_envs.bullet.bullet_client import BulletClient
- super(Bullet, self).__init__(render=render, **kwargs)
+ super(Bullet, self).__init__(render=render, num_instances=num_instances, middleware=middleware, **kwargs)
# parse the kwargs
diff --git a/pyrobolearn/simulators/dart.py b/pyrobolearn/simulators/dart.py
index 8470282..988df40 100644
--- a/pyrobolearn/simulators/dart.py
+++ b/pyrobolearn/simulators/dart.py
@@ -66,7 +66,7 @@ if sys.version_info[0] < 3:
raise RuntimeError("You must use Python 3 with the Dart simulator.")
__author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["DART", "Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -101,16 +101,17 @@ class Dart(Simulator):
- [2] PEP8: https://www.python.org/dev/peps/pep-0008/
"""
- def __init__(self, render=True, num_instances=1, dt=0.001, **kwargs):
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
"""
Initialize the Dart simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
+ middleware (MiddleWare, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
- super(Dart, self).__init__(render, **kwargs)
+ super(Dart, self).__init__(render=render, num_instances=num_instances, middleware=middleware, **kwargs)
# dart = {'collision': ['BulletCollisionDetector', 'BulletCollisionGroup', 'CollisionDetector',
# 'CollisionGroup', 'CollisionOption', 'CollisionResult', 'Contact',
diff --git a/pyrobolearn/simulators/isaac.py b/pyrobolearn/simulators/isaac.py
index 8a44d72..4daea3b 100644
--- a/pyrobolearn/simulators/isaac.py
+++ b/pyrobolearn/simulators/isaac.py
@@ -61,16 +61,17 @@ class Isaac(Simulator):
- [4] Slides: https://developer.download.nvidia.com/video/gputechconf/gtc/2019/presentation/s9918-isaac-gym.pdf
"""
- def __init__(self, render=True, num_instances=1, **kwargs):
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
"""
Initialize Isaac gym simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
+ middleware (MiddleWare, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
- super(Isaac, self).__init__(render=render)
+ super(Isaac, self).__init__(render=render, num_instances=num_instances, middleware=middleware, **kwargs)
# define variables
self.gym = gymapi.acquire_gym()
diff --git a/pyrobolearn/simulators/middlewares/README.rst b/pyrobolearn/simulators/middlewares/README.rst
index 4859cd9..4071c00 100644
--- a/pyrobolearn/simulators/middlewares/README.rst
+++ b/pyrobolearn/simulators/middlewares/README.rst
@@ -1,7 +1,9 @@
Middlewares
===========
+THIS SECTION IS UNDER CONSTRUCTION
+
This folder provides interfaces to the middlewares that are used in robotics (such as ROS, YARP, etc). All these
classes inherit from the ``Middleware`` abstract class. Middlewares can be provided to simulators which can then use
-them to send/receive messages.
+them to send/receive messages. This allows to communicate with real platforms as well.
diff --git a/pyrobolearn/simulators/middlewares/middleware.py b/pyrobolearn/simulators/middlewares/middleware.py
index 32b556f..9d6cbba 100644
--- a/pyrobolearn/simulators/middlewares/middleware.py
+++ b/pyrobolearn/simulators/middlewares/middleware.py
@@ -29,4 +29,43 @@ class MiddleWare(object):
Middlewares can be provided to simulators which can then use them to send/receive messages.
"""
- pass
+
+ def __init__(self, subscribe=False, publish=False, teleoperate=False):
+ """
+ Initialize the middleware to communicate.
+
+ Args:
+ subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+ the values published on these topics.
+ publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+ teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+ previous attributes :attr:`subscribe` and :attr:`publish`.
+ """
+ # set variables
+ self.subscribe = subscribe
+ self.publish = publish
+ self.teleoperate = teleoperate
+
+ @property
+ def subscribe(self):
+ return self._subscribe
+
+ @subscribe.setter
+ def subscribe(self, subscribe):
+ self._subscribe = bool(subscribe)
+
+ @property
+ def publish(self):
+ return self._publish
+
+ @publish.setter
+ def publish(self, publish):
+ self._publish = bool(publish)
+
+ @property
+ def teleoperate(self):
+ return self._teleoperate
+
+ @teleoperate.setter
+ def teleoperate(self, teleoperate):
+ self._teleoperate = bool(teleoperate)
diff --git a/pyrobolearn/simulators/middlewares/ros.py b/pyrobolearn/simulators/middlewares/ros.py
index b51048f..0ab1e61 100644
--- a/pyrobolearn/simulators/middlewares/ros.py
+++ b/pyrobolearn/simulators/middlewares/ros.py
@@ -2,10 +2,9 @@
"""Define the ROS middleware API.
Dependencies in PRL:
-* `pyrobolearn.simulators.simulator.Simulator`
+* `pyrobolearn.simulators.middlewares.middleware.MiddleWare`
"""
-
# TODO
import os
import subprocess
@@ -14,11 +13,11 @@ import signal
import importlib
import inspect
-from pyrobolearn.simulators.simulator import Simulator
+from pyrobolearn.simulators.middlewares.middleware import MiddleWare
__author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["ROS (Willow Garage)", "Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -27,15 +26,25 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
-# TODO: maybe I should inherit from MiddleWare instead of Simulator... Then we can give these MiddleWare to different
-# simulators. Other communication middleware layer includes YARP, etc.
+class ROS(MiddleWare):
+ r"""ROS Interface middleware
-class ROS(Simulator):
- r"""ROS Interface
+ This middleware can be given to the simulator which can then interact with robots.
"""
def __init__(self, subscribe=False, publish=False, teleoperate=False, master_uri=11311, **kwargs):
- super(ROS, self).__init__(render=False)
+ """
+ Initialize the ROS middleware.
+
+ Args:
+ subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+ the values published on these topics.
+ publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+ teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+ previous attributes :attr:`subscribe` and :attr:`publish`.
+ master_uri (int): ROS master URI.
+ """
+ super(ROS, self).__init__(subscribe=subscribe, publish=publish, teleoperate=teleoperate)
# Environment variable
self.env = os.environ.copy()
diff --git a/pyrobolearn/simulators/middlewares/yarp_.py b/pyrobolearn/simulators/middlewares/yarp_.py
index e69de29..dee16ed 100644
--- a/pyrobolearn/simulators/middlewares/yarp_.py
+++ b/pyrobolearn/simulators/middlewares/yarp_.py
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+"""Define the YARP middleware API.
+
+Dependencies in PRL:
+* `pyrobolearn.simulators.middlewares.middleware.MiddleWare`
+"""
+
+# TODO
+import os
+import subprocess
+import psutil
+import signal
+import importlib
+import inspect
+
+from pyrobolearn.simulators.middlewares.middleware import MiddleWare
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["YARP (IIT)", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class YARP(MiddleWare):
+ r"""YARP Interface middleware
+
+ This middleware can be given to the simulator which can then interact with robots.
+ """
+
+ def __init__(self, subscribe=False, publish=False, teleoperate=False, **kwargs):
+ """
+ Initialize the YARP middleware.
+
+ Args:
+ subscribe (bool): if True, it will subscribe to the topics associated to the loaded robots, and will read
+ the values published on these topics.
+ publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
+ teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
+ previous attributes :attr:`subscribe` and :attr:`publish`.
+ """
+ super(YARP, self).__init__(subscribe=subscribe, publish=publish, teleoperate=teleoperate)
diff --git a/pyrobolearn/simulators/raisim.py b/pyrobolearn/simulators/raisim.py
index e610bc9..ac21ed2 100644
--- a/pyrobolearn/simulators/raisim.py
+++ b/pyrobolearn/simulators/raisim.py
@@ -77,16 +77,17 @@ class Raisim(Simulator):
- [6] RaiSimPy - A Python wrapper for Raisim: https://github.com/robotlearn/raisimpy
"""
- def __init__(self, render=True, num_instances=1, **kwargs):
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
"""
Initialize the Raisim simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
+ middleware (MiddleWare, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
- super(Raisim, self).__init__(render, **kwargs)
+ super(Raisim, self).__init__(render=render, num_instances=num_instances, middleware=middleware, **kwargs)
# create world
self.world = raisim.World()
diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py
index 3423f85..dc2d837 100644
--- a/pyrobolearn/simulators/simulator.py
+++ b/pyrobolearn/simulators/simulator.py
@@ -184,18 +184,21 @@ class Simulator(object):
URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16
URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192
- def __init__(self, render=True, num_instances=1, **kwargs):
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
r"""
Initialize the Simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
+ middleware (MiddleWare, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
self._render = render
self.real_time = False
self.kwargs = kwargs
+ self._num_instances = num_instances
+ self._middleware = middleware
# main camera in the simulator
self._camera = None
@@ -306,7 +309,7 @@ class Simulator(object):
@staticmethod
def has_middleware_communication_layer():
"""Return True if the simulator has a middleware communication layer (like ROS, YARP, etc)."""
- return False
+ return self._middleware is not None
@staticmethod
def supports_dynamic_loading():
diff --git a/pyrobolearn/simulators/vrep.py b/pyrobolearn/simulators/vrep.py
index ff820d0..2135ead 100644
--- a/pyrobolearn/simulators/vrep.py
+++ b/pyrobolearn/simulators/vrep.py
@@ -70,8 +70,17 @@ class VREP(Simulator):
- [2] PyRep: https://github.com/stepjam/PyRep
"""
- def __init__(self, render=True):
- super(VREP, self).__init__(render=render)
+ def __init__(self, render=True, num_instances=1, middleware=None, **kwargs):
+ """
+ Initialize the VREP simulator.
+
+ Args:
+ render (bool): if True, it will open the GUI, otherwise, it will just run the server.
+ num_instances (int): number of simulator instances.
+ middleware (MiddleWare, None): middleware instance.
+ **kwargs (dict): optional arguments (this is not used here).
+ """
+ super(VREP, self).__init__(render=render, num_instances=num_instances, middleware=middleware)
# create simulator
self.sim = pyrep.PyRep()
diff --git a/pyrobolearn/utils/inertia.py b/pyrobolearn/utils/inertia.py
index 31c1aa9..14e2f6a 100644
--- a/pyrobolearn/utils/inertia.py
+++ b/pyrobolearn/utils/inertia.py
@@ -33,7 +33,7 @@ def get_full_inertia(inertia):
already to be the principal moments of inertia.
Returns:
- np.array[3,3]: full inertia matrix.
+ np.array[float[3,3]]: full inertia matrix.
"""
# make sure inertia is a numpy array
inertia = np.asarray(inertia)
@@ -359,7 +359,7 @@ def get_inertia_of_ellipsoid(mass, a, b, c, full=False):
mass (float): mass of the ellipsoid.
a (float): first semi-axis of the ellipsoid.
b (float): second semi-axis of the ellipsoid.
- c (float): thirs semi-axis of the ellipsoid.
+ c (float): third semi-axis of the ellipsoid.
full (bool): if we should return the full inertia matrix, or just the diagonal elements.
Returns:
@@ -378,15 +378,18 @@ def get_inertia_of_ellipsoid(mass, a, b, c, full=False):
return inertia
-def get_inertia_of_mesh(filename, mass=None, density=1000, full=False):
+def get_inertia_of_mesh(mesh, mass=None, scale=1., density=1000, full=False):
r"""
Return the principal moments of the inertia matrix of a mesh.
Warnings: the mesh has to be watertight.
Args:
- filename (str): path to the mesh file. Note that the mesh
+ mesh (str, trimesh.Trimesh): path to the mesh file, or a Trimesh instance. Note that the mesh has to be
+ watertight.
mass (float, None): mass of the mesh (in kg). If None, it will use the density.
+ scale (float): scaling factor. If you have a mesh in meter but you want to scale it into centimeters, you need
+ to provide a scaling factor of 0.01.
density (float): density of the mesh (in kg/m^3). By default, it uses the density of the water 1000kg / m^3.
full (bool): if we should return the full inertia matrix, or just the diagonal elements.
@@ -397,7 +400,45 @@ def get_inertia_of_mesh(filename, mass=None, density=1000, full=False):
else:
np.array[float[3]]: principal moments of inertia.
"""
- inertia = get_mesh_body_inertia(filename, mass=mass, density=density)
+ inertia = get_mesh_body_inertia(mesh, mass=mass, density=density, scale=scale)
if full:
return np.diag(inertia)
return inertia
+
+
+def combine_inertias(coms, masses, inertias, rotations=None):
+ r"""
+ This combines the inertia matrices together to form the combined body frame inertia matrix relative to the
+ combined center of mass.
+
+ Args:
+ coms (list[np.array[float[3]]): list of center of masses.
+ masses (list[float]): list of total body masses.
+ inertias (list[np.array[float[3,3]]]): list of body frame inertia matrices relative to their center of mass.
+ rotations (list[np.array[float[3,3]]]): list of rotation matrices where each rotation has to be applied on
+ the inertia matrix before translating it.
+
+ Returns:
+ float: total mass.
+ np.array[float[3]]: combined center of mass.
+ np.array[float[3,3]]: combined inertia matrix.
+ """
+ if len(coms) != len(masses) or len(coms) != len(inertias):
+ raise ValueError("The given lists do not have the same length: len(coms)={}, len(masses)={}, "
+ "len(inertias)={}".format(len(coms), len(masses), len(inertias)))
+ if len(coms) == 0:
+ raise ValueError("Expecting the length of the provided parameters to be bigger than 0")
+ if rotations is not None and len(rotations) != len(coms):
+ raise ValueError("The given list of rotations do not have the same length (={}) as the other arguments (={})"
+ ".".format(len(rotations), len(masses)))
+
+ total_mass = np.sum(masses)
+ new_com = np.sum([mass * com for mass, com in zip(masses, coms)], axis=0)
+ new_com /= total_mass
+ if rotations is None:
+ inertia = np.sum([translate_inertia_matrix(inertia, vector=new_com-com, mass=mass)
+ for mass, com, inertia in zip(masses, coms, inertias)], axis=0)
+ else:
+ inertia = np.sum([translate_inertia_matrix(rotate_inertia_matrix(inertia, rot), vector=new_com-com, mass=mass)
+ for mass, com, inertia, rot in zip(masses, coms, inertias, rotations)], axis=0)
+ return total_mass, new_com, inertia
diff --git a/pyrobolearn/utils/mesh.py b/pyrobolearn/utils/mesh.py
index 22e37b6..7638cbb 100644
--- a/pyrobolearn/utils/mesh.py
+++ b/pyrobolearn/utils/mesh.py
@@ -283,6 +283,17 @@ def get_mesh_body_inertia(mesh, mass=None, density=1000, scale=1.):
"""
mesh = get_mesh(mesh)
+ # volume = mesh.volume # in m^3 (in trimesh: mash.mass = mash.volume, i.e. density = 1)
+ # volume *= scale ** 3 # the scale is for each dimension
+ # inertia = mesh.moment_inertia * scale ** 2 # I ~ mr^2
+ #
+ # # the previous inertia is based on the assumption that mesh.mass = mesh.volume
+ # density = mass / volume # density = new_mass / old_mass
+ # inertia *= density
+ #
+ # # com = mesh.center_mass * scale # uniform density assumption
+ # # (mesh.center_mass is a bit different from mesh.centroid)
+
mesh.apply_scale(scale) # note: this is an inplace operation
default_density = mesh.density
diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py
index d345953..5f81e21 100644
--- a/pyrobolearn/utils/parsers/robots/data_structures.py
+++ b/pyrobolearn/utils/parsers/robots/data_structures.py
@@ -2,11 +2,15 @@
"""Provide the common data structures that are shared among the various parsers, generators, and converters.
"""
+import copy
import numpy as np
+import trimesh
from collections import OrderedDict, Iterable
from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy, get_matrix_from_rpy, \
get_rpy_from_matrix, get_matrix_from_axis_angle
+from pyrobolearn.utils.inertia import get_inertia_of_box, get_inertia_of_capsule, get_inertia_of_cylinder, \
+ get_inertia_of_ellipsoid, get_inertia_of_mesh, get_inertia_of_sphere, combine_inertias
__author__ = "Brian Delhaisse"
@@ -22,31 +26,47 @@ __status__ = "Development"
class Simulator(object):
r"""Simulator data structure."""
- def __init__(self, world=None, physics_engine=None, physics_properties=None):
+ def __init__(self, name=None, worlds=None, physics_engine=None, physics_properties=None):
"""
Initialize the simulator data structure.
Args:
- world (World, None): world data structure instance.
+ name (str, None): name of the simulator.
+ worlds (list[World], None): world data structure instances.
physics_engine (PhysicsEngine): physics engine instance.
physics_properties (Physics): the physics properties (gravity, viscosity, friction, etc).
"""
- self.world = world
+ self.worlds = worlds
self.engine = physics_engine
self.physics = physics_properties
@property
- def world(self):
- """Return the world."""
- return self._world
+ def worlds(self):
+ """Return the worlds."""
+ return self._worlds
- @world.setter
- def world(self, world):
- """Set the world data structure instance."""
- 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
+ @worlds.setter
+ def worlds(self, worlds):
+ """Set the world data structure instances."""
+ if worlds is None:
+ worlds = []
+ elif isinstance(worlds, World):
+ worlds = [worlds]
+
+ if not isinstance(worlds, (list, tuple)):
+ raise TypeError("Expecting the given 'worlds' to be a list/tuple of `World` instances, but got instead:"
+ " {}".format(type(worlds)))
+ for world in worlds:
+ if not isinstance(world, World):
+ raise TypeError("Expecting the world to be an instance of `World`, but got instead: "
+ "{}".format(type(world)))
+ self._worlds = worlds
+
+ @property
+ def world(self):
+ """Return the first world."""
+ if len(self._worlds) > 0:
+ return self._worlds[0]
@property
def engine(self):
@@ -74,11 +94,26 @@ class Simulator(object):
"{}".format(type(physics)))
self._physics = physics
+ def add_world(self, world):
+ r"""
+ Append a world to the list of worlds.
+
+ Args:
+ world (World): world instance.
+ """
+ if not isinstance(world, World):
+ raise TypeError("Expecting the given 'world' to be an instance of `World`, but got instead: "
+ "{}".format(type(world)))
+ self.worlds.append(world)
+
class PhysicsEngine(object):
r"""Physics Engine properties.
This include number of iterations, solver used, tolerance, timesteps, etc.
+
+ MuJoCo:
+ - solver: PGS, CG, Newton.
"""
def __init__(self, timestep=None):
@@ -86,13 +121,25 @@ class PhysicsEngine(object):
Initialize the Physics engine parameters.
Args:
- timestep (float, str): time step.
+ timestep (float, str): simulation time step in seconds.
"""
self.timestep = timestep
self.num_iterations = None
self.solver = None
self.tolerance = None
+ @property
+ def timestep(self):
+ """Return the simulation time step."""
+ return self._timestep
+
+ @timestep.setter
+ def timestep(self, timestep):
+ """Set the simulation time step."""
+ if timestep is not None:
+ timestep = float(timestep)
+ self._timestep = timestep
+
class Frame(object):
r"""Reference Frame"""
@@ -172,12 +219,14 @@ class Frame(object):
@property
def quaternion(self):
"""Return the frame orientation expressed as a quaternion [x,y,z,w]."""
- return get_quaternion_from_rpy(self.orientation)
+ if self._orientation is not None:
+ return get_quaternion_from_rpy(self.orientation)
@property
def rot(self):
"""Return the frame orientation expressed as a rotation matrix."""
- return get_matrix_from_rpy(self.rpy)
+ if self._orientation is not None:
+ return get_matrix_from_rpy(self.rpy)
@property
def pose(self):
@@ -219,7 +268,7 @@ class Physics(object):
Args:
gravity (list/tuple/np.array[float[3]], str): gravity vector.
- timestep (float, str): time step.
+ timestep (float, str): simulation time step in seconds.
"""
# gravity depends on the world frame; the frame axis convention that we use.
# By default, x points forward, y on the left, and z upward.
@@ -245,12 +294,12 @@ class Physics(object):
@property
def timestep(self):
- """Return the time step."""
+ """Return the simulation time step."""
return self._timestep
@timestep.setter
def timestep(self, timestep):
- """Set the time step."""
+ """Set the simulation time step."""
if timestep is not None:
timestep = float(timestep)
self._timestep = timestep
@@ -280,6 +329,18 @@ class World(object):
self.physics = None
self.lights = OrderedDict()
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def physics(self):
"""Return the physical properties set in the world."""
@@ -329,6 +390,18 @@ class Light(object):
self.frame = Frame(position=position, orientation=orientation)
self.material = Material(color=ambient, diffuse=diffuse, specular=specular)
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def shadows(self):
"""Return True if we should cast shadows."""
@@ -441,22 +514,116 @@ class Light(object):
self.material.specular = specular
-class Tree(object):
- r"""Tree data structure.
+class Floor(object):
+ r"""Floor data structure.
- The tree data structure starts with a root element (=base link) and contains each bodies / joints. Each tree
- represents a multi-body in the world. Its position / orientation is expressed in the world frame.
+ """
+
+ def __init__(self, name=None, dimensions=None, position=None, orientation=None):
+ """
+ Initialize the floor data structure.
+
+ Args:
+ name (str): name of the floor.
+ dimensions (list/tuple/np.array[float[:3]], str): dimensions of the floor. By default, the given arguments
+ are expected to be the (X/2, Y/2, space between cells).
+ position (list/tuple/np.array[float[3]], str): frame position in the world.
+ orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): frame orientation in the world.
+ """
+ self.name = name
+ self.dimensions = dimensions
+ self.frame = Frame(position, orientation, dtype='world')
+
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
+ @property
+ def dimensions(self):
+ """Return the dimensions of the floor."""
+ return self._dims
+
+ @dimensions.setter
+ def dimensions(self, dims):
+ if dims is not None:
+ if isinstance(dims, str):
+ dims = [float(p) for p in dims.split()]
+ dims = np.asarray(dims)
+ if len(dims) > 3:
+ raise ValueError("Expecting the given dims to have a length below 3, but got instead a length of: "
+ "{}".format(len(dims)))
+ self._dims = dims
+
+ @property
+ def position(self):
+ """Return the floor frame position."""
+ return self.frame.position
+
+ @position.setter
+ def position(self, position):
+ """Set the floor frame position."""
+ self.frame.position = position
+
+ @property
+ def orientation(self):
+ """Return the floor frame orientation."""
+ return self.frame.orientation
+
+ @orientation.setter
+ def orientation(self, orientation):
+ """Set the floor frame orientation expressed as RPY angles."""
+ self.frame.orientation = orientation
+
+ @property
+ def rpy(self):
+ """Return the floor frame orientation expressed as RPY angles."""
+ return self.frame.rpy
+
+ @property
+ def quaternion(self):
+ """Return the floor frame orientation expressed as a quaternion [x,y,z,w]."""
+ return self.frame.quaternion
+
+ @property
+ def rot(self):
+ """Return the floor frame orientation expressed as a rotation matrix."""
+ return self.frame.rot
+
+ @property
+ def pose(self):
+ """Return the floor frame pose."""
+ return self.frame.pose
+
+ @pose.setter
+ def pose(self, pose):
+ """Set the floor frame pose."""
+ self.frame.pose = pose
+
+
+class MultiBody(object):
+ r"""Multi-body / Tree data structure.
+
+ The multi-body / tree data structure starts with a root element (=base link) and contains each bodies / joints.
+ Each tree represents a multi-body in the world. Its position / orientation is expressed in the world frame.
"""
def __init__(self, name=None, root=None, position=None, orientation=None):
"""
- Initialize the Tree data structure.
+ Initialize the Multi-body / Tree data structure.
Args:
name (str): name of the tree.
- root (root): root element in the tree.
- position (list/tuple/np.array[float[3]], str): frame position.
- orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): frame orientation.
+ root (Body): root element in the tree.
+ position (list/tuple/np.array[float[3]], str): frame position in the world.
+ orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): frame orientation in the world.
"""
self.name = name
self.root = root
@@ -465,6 +632,45 @@ class Tree(object):
self.materials = {}
self.frame = Frame(position, orientation, dtype='world')
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
+ @property
+ def num_dofs(self):
+ """Return the total number of degrees of freedom."""
+ num_dofs = 0
+ for joint in self.joints:
+ num_dofs += joint.num_dofs
+ return num_dofs
+
+ @property
+ def root(self):
+ """Return the root body element."""
+ return self._root
+
+ @root.setter
+ def root(self, root):
+ """Set the root body element."""
+ if root is not None and not isinstance(root, Body):
+ raise TypeError("Expecting the given 'body' to be an instance of `Body`, but instead got: "
+ "{}".format(type(root)))
+ self._root = root
+
+ @property
+ def static(self):
+ """Return if the root element in the tree is static or not."""
+ if self.root is not None:
+ return self.root.static
+
@property
def position(self):
"""Return the tree frame position."""
@@ -511,10 +717,14 @@ class Tree(object):
self.frame.pose = pose
+# alias
+Tree = MultiBody
+
+
class Body(object):
r"""Body / Link data structure."""
- def __init__(self, body_id, name=None, inertial=None, visual=None, collision=None, static=False,
+ def __init__(self, body_id, name=None, inertials=None, visuals=None, collisions=None, static=False,
position=None, orientation=None, frame_type=None):
"""
Initialize the Body / Link data structure.
@@ -522,16 +732,20 @@ class Body(object):
Args:
body_id (int): body unique id.
name (str, None): body name.
- inertial (Inertial, None): inertial component.
- visual (Visual, None): visual shape.
- collision (Collision, None): collision shape.
+ inertials (Inertial, list[Inertial], None): inertial components. Multiple inertial components can be
+ provided and by calling the `inertia` property they will be combined together to only form one inertial
+ component.
+ visuals (Visual, list[Visual], None): visual shapes. Multiple visual shape instances can be provided for a
+ specific body.
+ collisions (Collision, list[Collision], None): collision shapes. Multiple collision shape instances can
+ be provided for a specific body.
static (bool): if the body is static in the world or not.
position (list/tuple/np.array[float[3]], str): body frame position. If None, it will look at the visual
and collision shapes. By default, if the visual shape is defined it will return its position.
orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): body frame orientation. If
None, it will look at the collision shapes. By default, if the visual shape is defined it will return
its orientation.
- frame_type (str):
+ frame_type (str): frame type. It can be a {'world', 'body', 'joint', 'inertial'} frame.
"""
self.id = int(body_id)
self.name = name
@@ -541,51 +755,109 @@ class Body(object):
self.parent_joints = OrderedDict() # parent joints
# set body properties
- self.inertial = inertial
- self.visual = visual
- self.collision = collision
+ self.inertials = inertials
+ self.visuals = visuals
+ self.collisions = collisions
self.static = static
- self.frame = Frame(position=position, orientation=orientation)
+ self.frame = Frame(position=position, orientation=orientation, dtype=frame_type)
+
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
+ @property
+ def inertials(self):
+ """Return the inertial components of the body."""
+ return self._inertial
+
+ @inertials.setter
+ def inertials(self, inertials):
+ """Set the inertial components of the body."""
+ if inertials is None:
+ inertials = []
+ elif isinstance(inertials, Inertial):
+ inertials = [inertials]
+ elif isinstance(inertials, (list, tuple)):
+ for i, inertial in enumerate(inertials):
+ if not isinstance(inertial, Inertial):
+ raise TypeError("The {}th element in the given inertials is not an instance of `Inertial`, but: "
+ "{}".format(i, type(inertial)))
+ else:
+ raise TypeError("Expecting the given 'inertials' to be a list of `Inertial`, or an instance of "
+ "`Inertial`, but got instead: {}".format(type(inertials)))
+ self._inertials = inertials
@property
def inertial(self):
"""Return the inertial component of the body."""
- return self._inertial
+ if len(self.inertials) == 1:
+ return self.inertials[0]
+ return self.combine_inertials(self.inertials)
- @inertial.setter
- def inertial(self, inertial):
- """Set the inertial component of the body."""
- if inertial is not None and not isinstance(inertial, Inertial):
- raise TypeError("Expecting inertial to be an instance of `Inertial`, but got instead: "
- "{}".format(type(inertial)))
- self._inertial = inertial
+ @property
+ def visuals(self):
+ """Return the visual shapes of the body."""
+ return self._visuals
+
+ @visuals.setter
+ def visuals(self, visuals):
+ """Set the visual shapes of the body."""
+ if visuals is None:
+ visuals = []
+ elif isinstance(visuals, Visual):
+ visuals = [visuals]
+ elif isinstance(visuals, (list, tuple)):
+ for i, visual in enumerate(visuals):
+ if not isinstance(visual, Visual):
+ raise TypeError("The {}th element in the given visuals is not an instance of `Visual`, but: "
+ "{}".format(i, type(visual)))
+ else:
+ raise TypeError("Expecting the given 'visuals' to be a list of `Visual`, or an instance of "
+ "`Visual`, but got instead: {}".format(type(visuals)))
+ self._visuals = visuals
@property
def visual(self):
- """Return the visual shape of the body."""
- return self._visual
+ """Return the first visual shape of the body."""
+ if len(self._visuals) > 0:
+ return self._visuals[0]
- @visual.setter
- def visual(self, visual):
- """Set the visual shape of the body."""
- if visual is not None and not isinstance(visual, Visual):
- raise TypeError("Expecting visual to be an instance of `Visual`, but got instead: "
- "{}".format(type(visual)))
- self._visual = visual
+ @property
+ def collisions(self):
+ """Return the collision shapes of the body."""
+ return self._collisions
+
+ @collisions.setter
+ def collisions(self, collisions):
+ """Set the collision shape of the body."""
+ if collisions is None:
+ collisions = []
+ elif isinstance(collisions, Collision):
+ collisions = [collisions]
+ elif isinstance(collisions, (list, tuple)):
+ for i, collision in enumerate(collisions):
+ if not isinstance(collision, Collision):
+ raise TypeError("The {}th element in the given collisions is not an instance of `Collision`, but: "
+ "{}".format(i, type(collision)))
+ else:
+ raise TypeError("Expecting the given 'collisions' to be a list of `Collision`, or an instance of "
+ "`Collision`, but got instead: {}".format(type(collisions)))
+ self._collisions = collisions
@property
def collision(self):
- """Return the collision shape of the body."""
- return self._collision
-
- @collision.setter
- def collision(self, collision):
- """Set the collision shape of the body."""
- if collision is not None and not isinstance(collision, Collision):
- raise TypeError("Expecting collision to be an instance of `Collision`, but got instead: "
- "{}".format(type(collision)))
- self._collision = collision
+ """Return the first collision shape of the body."""
+ if len(self._collisions) > 0:
+ return self._collisions[0]
@property
def static(self):
@@ -597,6 +869,136 @@ class Body(object):
"""Set if the body is static in the world or not."""
self._static = bool(static)
+ @property
+ def position(self):
+ """Return the body frame position."""
+ if self.frame.position is not None:
+ return self.frame.position
+ if self.visual is not None and self.visual.position is not None:
+ return self.visual.position
+ if self.collision is not None:
+ return self.collision.position
+
+ @position.setter
+ def position(self, position):
+ """Set the body frame position."""
+ self.frame.position = position
+
+ @property
+ def orientation(self):
+ """Return the body frame orientation expressed as RPY angles."""
+ if self.frame.orientation is not None:
+ return self.frame.orientation
+ if self.visual is not None and self.visual.orientation is not None:
+ return self.visual.orientation
+ if self.collision is not None:
+ return self.collision.orientation
+
+ @orientation.setter
+ def orientation(self, orientation):
+ """Set the body frame orientation (which can be expressed as a rotation matrix, RPY angles. or a quaternion
+ [x,y,z,w]."""
+ self.frame.orientation = orientation
+
+ @property
+ def rpy(self):
+ """Return the body frame orientation expressed as RPY angles."""
+ if self.frame.orientation is not None:
+ return self.frame.rpy
+ if self.visual is not None and self.visual.orientation is not None:
+ return self.visual.rpy
+ if self.collision is not None:
+ return self.collision.rpy
+
+ @property
+ def quaternion(self):
+ """Return the body frame orientation expressed as a quaternion [x,y,z,w]."""
+ if self.frame.orientation is not None:
+ return self.frame.quaternion
+ if self.visual is not None and self.visual.orientation is not None:
+ return self.visual.quaternion
+ if self.collision is not None:
+ return self.collision.quaternion
+
+ @property
+ def rot(self):
+ """Return the body frame orientation expressed as a rotation matrix."""
+ if self.frame.orientation is not None:
+ return self.frame.rot
+ if self.visual is not None and self.visual.orientation is not None:
+ return self.visual.rot
+ if self.collision is not None:
+ return self.collision.rot
+
+ @property
+ def pose(self):
+ """Return the body frame pose."""
+ if self.frame.pose is not None:
+ return self.frame.pose
+ if self.visual is not None and self.visual.pose is not None:
+ return self.visual.pose
+ if self.collision is not None:
+ return self.collision.pose
+
+ @pose.setter
+ def pose(self, pose):
+ """Set the body frame pose."""
+ self.frame.pose = pose
+
+ def add_collision(self, collision):
+ """
+ Add a collision shape to the list of collision shapes.
+
+ Args:
+ collision (Collision): collision instance.
+ """
+ if not isinstance(collision, Collision):
+ raise TypeError("Expecting the given 'collision' to be an instance of `Collision`, but got instead: "
+ "{}".format(type(collision)))
+ self.collisions.append(collision)
+
+ def add_visual(self, visual):
+ """
+ Add a visual shape to the list of visual shapes.
+
+ Args:
+ visual (Collision): visual instance.
+ """
+ if not isinstance(visual, Visual):
+ raise TypeError("Expecting the given 'visual' to be an instance of `Visual`, but got instead: "
+ "{}".format(type(visual)))
+ self.visuals.append(visual)
+
+ def add_inertial(self, inertial):
+ """
+ Add an inertial element to the list of inertials.
+
+ Args:
+ inertial (Inertial): inertial element.
+ """
+ if not isinstance(inertial, Inertial):
+ raise TypeError("Expecting the given 'inertial' to be an instance of `Inertial`, but got instead: "
+ "{}".format(type(inertial)))
+ self.inertials.append(inertial)
+
+ @staticmethod
+ def combine_inertials(inertials):
+ """
+ Combine the given inertial elements.
+
+ Args:
+ inertials (list[Inertial]): list of Inertial elements.
+
+ Returns:
+ Inertial: combined inertial element.
+ """
+ if len(inertials) == 0:
+ return None
+ inertial = copy.deepcopy(inertials[0])
+ for i in range(1, len(inertials)):
+ inertial += inertials[i]
+ return inertial
+
class Joint(object):
r"""Joint data structure.
@@ -655,11 +1057,23 @@ class Joint(object):
self.damping = damping
self.effort = effort
self.velocity = velocity
- self.num_dofs = None
+ self.num_dofs = 0
self.init_position = None
self.init_velocity = None
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def dtype(self):
"""Return the joint type."""
@@ -676,7 +1090,7 @@ class Joint(object):
if dtype in {'fixed', 'weld'}:
dtype = 'fixed'
self.num_dofs = 0
- elif dtype == 'hinge':
+ elif dtype in {'hinge', 'revolute'}:
dtype = 'revolute'
self.num_dofs = 1
elif dtype in {'slide', 'prismatic'}:
@@ -867,24 +1281,56 @@ class Inertia(object):
This class represents an inertia matrix.
"""
- def __init__(self, ixx=1., iyy=1., izz=1., ixy=0., ixz=0., iyz=0.):
+ def __init__(self, ixx=1., iyy=1., izz=1., ixy=0., ixz=0., iyz=0., inertia=None):
"""
Initialize the Inertia.
Args:
ixx (float, str): Ixx component of the inertia.
- iyy (float, str): Iyy component of the inertia.:
- izz (float, str): Izz component of the inertia.:
- ixy (float, str): Ixy component of the inertia.:
- ixz (float, str): Ixz component of the inertia.:
- iyz (float, str): Iyz component of the inertia.:
+ iyy (float, str): Iyy component of the inertia.
+ izz (float, str): Izz component of the inertia.
+ ixy (float, str): Ixy component of the inertia.
+ ixz (float, str): Ixz component of the inertia.
+ iyz (float, str): Iyz component of the inertia.
+ inertia (list/np.array[float[3/6]], np.array[float[3,3]], str, None): inertia matrix. If specified, the
+ previous attributes won't be taken into account.
"""
- self.ixx = ixx
- self.iyy = iyy
- self.izz = izz
- self.ixy = ixy
- self.ixz = ixz
- self.iyz = iyz
+ if inertia is not None:
+ self.inertia = inertia
+ else:
+ self.ixx = ixx
+ self.iyy = iyy
+ self.izz = izz
+ self.ixy = ixy
+ self.ixz = ixz
+ self.iyz = iyz
+
+ @property
+ def inertia(self):
+ """Return the 6 components of the inertia [ixx, iyy, izz, ixy, ixz, iyz]."""
+ return np.array([self.ixx, self.iyy, self.izz, self.ixy, self.ixz, self.iyz])
+
+ @inertia.setter
+ def inertia(self, inertia):
+ """Set the inertia matrix."""
+ if isinstance(inertia, str):
+ inertia = [float(c) for c in inertia.split()]
+ elif not isinstance(inertia, (list, tuple, np.ndarray)):
+ raise TypeError("Expecting the given 'inertia' to be a tuple, list, np.array of 3/6/9 floats, but got "
+ "instead: {}".format(type(inertia)))
+ inertia = np.asarray(inertia)
+ if inertia.ndim == 2: # 3x3
+ inertia = inertia.reshape(-1)
+ if len(inertia) == 3:
+ self.ixx, self.iyy, self.izz = inertia
+ elif len(inertia) == 6:
+ self.ixx, self.iyy, self.izz, self.ixy, self.ixz, self.iyz = inertia
+ elif len(inertia) == 9:
+ I = inertia
+ self.ixx, self.iyy, self.izz, self.ixy, self.ixz, self.iyz = I[0], I[4], I[8], I[1], I[2], I[5]
+ else:
+ raise ValueError("Expecting the given 'inertia' to be have a length of 3, 6, or 9, but got instead a "
+ "length of: {}".format(len(inertia)))
@property
def full_inertia(self):
@@ -910,8 +1356,13 @@ class Inertia(object):
@property
def principal_inertia(self):
- """Return the principal moments of the inertia (np.array[float[3]]), and the direction of the principal axes
- of the body (np.array[float[3,3]])."""
+ """
+ Return the principal moments of the inertia, and the direction of the principal axes of the body.
+
+ Returns:
+ np.array[float[3]]: principal moments of the inertia.
+ np.array[float[3,3]]: principal axes of the body.
+ """
inertia = self.full_inertia
evals, evecs = np.linalg.eigh(inertia)
return evals, evecs
@@ -998,9 +1449,9 @@ class Inertia(object):
class Inertial(object):
- r"""Inertial parameters.
+ r"""Inertial properties.
- The inertial tag groups the mass, inertia, and the body CoM position and orientation.
+ The inertial element groups the mass, inertia, and the body CoM position and orientation.
Moments of inertia of popular shapes:
@@ -1018,7 +1469,7 @@ class Inertial(object):
``mesh.moment_inertia``.
"""
- def __init__(self, mass=None, inertia=None, position=(0., 0., 0.), orientation=(0., 0., 0.)):
+ def __init__(self, mass=None, inertia=None, position=(0., 0., 0.), orientation=None):
"""
Initialize the Inertial instance.
@@ -1026,8 +1477,8 @@ class Inertial(object):
mass (float, str): mass value (in kg)
inertia (str, list/tuple[float[3/6/9]], np.array[float[3/6/9]], np.array[float[3,3]], dict): inertia matrix
represented in the body frame.
- position (np.array[float[3]], str): position of the center of mass.
- orientation (np.array[float[3]], str): rotation expressed as roll-pitch-yaw angles.
+ position (np.array[float[3]], str): position of the inertial frame (center of mass).
+ orientation (np.array[float[3]], str): orientation of the inertial frame expressed as roll-pitch-yaw angles.
"""
self.mass = mass
self.inertia = inertia
@@ -1083,14 +1534,20 @@ class Inertial(object):
@property
def principal_inertia(self):
- """Return the principal moments of the inertia (np.array[float[3]]), and the direction of the principal axes
- of the body (np.array[float[3,3]])."""
+ """
+ Return the principal moments of the inertia, and the direction of the principal axes of the body.
+
+ Returns:
+ np.array[float[3]]: principal moments of the inertia.
+ np.array[float[3,3]]: principal axes of the body.
+ """
evals, evecs = self.inertia.principal_inertia
return evals, self.rot.dot(evecs)
@property
def diagonal_inertia(self):
- """Aligned inertia.
+ """
+ Return the aligned inertia = principal moments of inertia.
Returns:
np.array[float[3]]: principal moments of the inertia.
@@ -1152,6 +1609,180 @@ class Inertial(object):
"""Set the inertial pose."""
self.frame.pose = pose
+ @staticmethod
+ def compute_mass_from_density(shape, dimensions=None, density=1000, volume=None, mesh=None):
+ """
+ Compute the mass from the density and the shape type.
+
+ Args:
+ shape (str): shape type which can be selected from {'sphere', 'box', 'capsule', 'cylinder', 'ellipsoid',
+ 'mesh'}.
+ dimensions (list[float[:3]]): shape dimensions, or scale factor if mesh. If the volume is not provided,
+ it will use the specified dimensions.
+ density (float): density (by default, it is the density of water ~ 1000kg/m^3).
+ volume (float, None): if provided, the returned mass will be the given density times the volume.
+ mesh (str, trimesh.Trimesh): mesh filename or mesh instance. Only valid if :attr:`shape` = 'mesh'.
+
+ Returns:
+ float, None: mass (in kg). Return None if the specified shape is not supported.
+ """
+ if volume is None:
+ volume = Inertial.compute_volume(shape=shape, dimensions=dimensions, mesh=mesh)
+ if volume is not None:
+ return density * volume
+
+ @staticmethod
+ def compute_volume(shape, dimensions, mesh=None):
+ """
+ Compute the volume given the shape type and its dimensions.
+
+ Args:
+ shape (str): shape type which can be selected from {'sphere', 'box', 'capsule', 'cylinder', 'ellipsoid',
+ 'mesh'}.
+ dimensions (list[float[:3]]): shape dimensions, or scale factor if mesh.
+ mesh (str, trimesh.Trimesh): mesh filename or mesh instance. Only valid if :attr:`shape` = 'mesh'.
+
+ Returns:
+ float, None: total volume of the specified shape. Return None if the specified shape is not supported.
+ """
+ if shape == 'box':
+ w, h, d = dimensions # width, height, depth
+ volume = w * h * d
+ elif shape == 'capsule':
+ r, h = dimensions # radius, height
+ sphere_volume = 4. / 3 * np.pi * r ** 3
+ cylinder_volume = np.pi * r ** 2 * h
+ volume = sphere_volume + cylinder_volume
+ elif shape == 'cylinder':
+ r, h = dimensions # radius, height
+ volume = np.pi * r ** 2 * h
+ elif shape == 'ellipsoid':
+ a, b, c = dimensions
+ volume = 4. / 3 * np.pi * a * b * c
+ elif shape == 'mesh':
+ if isinstance(dimensions, (list, tuple, np.ndarray)):
+ dimensions = dimensions[0]
+ scale = dimensions # scale
+ if isinstance(mesh, str):
+ mesh = trimesh.load(mesh)
+ elif not isinstance(mesh, trimesh.Trimesh):
+ raise TypeError("Expecting the given 'mesh' to be an instance of `trimesh.Trimesh`, instead got: "
+ "{}".format(type(mesh)))
+ mesh.apply_scale(scale)
+ volume = mesh.volume # in m^3 (in trimesh: mash.mass = mash.volume, i.e. density = 1)
+ # volume *= scale ** 3 # the scale is for each dimension
+ mesh.apply_scale(1./scale)
+ elif shape == 'sphere':
+ if isinstance(dimensions, (list, tuple, np.ndarray)):
+ dimensions = dimensions[0]
+ r = dimensions # radius
+ volume = 4. / 3 * np.pi * r ** 3
+ else:
+ volume = None
+ return volume
+
+ @staticmethod
+ def compute_inertia(shape, dimensions, mass=None, density=1000, mesh=None):
+ """
+ Compute the principal moments of inertia for the specified shape.
+
+ Args:
+ shape (str): shape type, can be selected from {}
+ dimensions (list[float[:3]]): dimensions of the shape.
+ mass (float, None): mass of the shape. If not provided, the density will be used.
+ density (float): density (by default, it is the density of water ~ 1000kg/m^3).
+ mesh (str, trimesh.Trimesh): mesh filename or mesh instance. Only valid if :attr:`shape` = 'mesh'.
+
+ Returns:
+ np.array[float[3]], None: principal moments of inertia. None if the specified shape is not supported.
+ """
+ # compute mass if necessary
+ if mass is None:
+ mass = Inertial.compute_mass_from_density(shape=shape, dimensions=dimensions, density=density, mesh=mesh)
+
+ # compute inertia
+ if shape == 'box':
+ inertia = get_inertia_of_box(mass, size=dimensions, full=False)
+ elif shape == 'capsule':
+ r, h = dimensions # radius, height
+ inertia = get_inertia_of_capsule(mass, radius=r, height=h, full=False)
+ elif shape == 'cylinder':
+ r, h = dimensions # radius, height
+ inertia = get_inertia_of_cylinder(mass, radius=r, height=h, full=False)
+ elif shape == 'ellipsoid':
+ a, b, c = dimensions
+ inertia = get_inertia_of_ellipsoid(mass, a=a, b=b, c=c, full=False)
+ elif shape == 'mesh':
+ if isinstance(dimensions, (list, tuple, np.ndarray)):
+ dimensions = dimensions[0]
+ scale = dimensions # scale
+ inertia = get_inertia_of_mesh(mesh=mesh, mass=mass, scale=scale, full=False)
+ elif shape == 'sphere':
+ if isinstance(dimensions, (list, tuple, np.ndarray)):
+ dimensions = dimensions[0]
+ radius = dimensions
+ inertia = get_inertia_of_sphere(mass=mass, radius=radius, full=False)
+ else:
+ inertia = None
+ return inertia
+
+ def __add__(self, other):
+ """
+ Combine two Inertial elements together.
+
+ This is done in 3 steps:
+ 1. find the combined CoM
+ 2. find the moments of inertia of each object through that point using the parallel axis theorem [1]
+ 3. combine the moments by adding the new tensors.
+
+ Args:
+ other (Inertial): other inertial elements.
+
+ Returns:
+ Inertial: the combined inertial element.
+
+ References:
+ - [1] Parallel axis theorem: https://en.wikipedia.org/wiki/Parallel_axis_theorem
+ """
+ # check the type
+ if not isinstance(other, Inertial):
+ raise TypeError("Expecting the given 'other' inertial element to be an instance of `Inertial` but got "
+ "instead: {}".format(type(other)))
+
+ # check the attribute of each Inertial
+ m1, m2 = self.mass, other.mass
+ I1, I2 = self.inertia, other.inertia
+ p1, p2 = self.position, other.position
+ r1, r2 = self.rot, other.rot
+ if m1 is None or m2 is None:
+ raise ValueError("The mass is not specified for this inertial element or the other one.")
+ if I1 is None or I2 is None:
+ raise ValueError("The inertia is not specified for this inertial element or the other one.")
+ if p1 is None or p2 is None:
+ raise ValueError("The CoM position is not specified for this inertial element or the other one.")
+ if r1 is None or r2 is None:
+ rotations = None
+ else:
+ if r1 is None:
+ r1 = np.identity(3)
+ if r2 is None:
+ r2 = np.identity(3)
+ rotations = [r1, r2]
+
+ # combine inertial elements and return it
+ mass, com, inertia = combine_inertias(coms=[p1, p2], masses=[m1, m2], inertias=[I1, I2], rotations=rotations)
+ inertial = Inertial(mass=mass, inertia=inertia, position=com)
+ return inertial
+
+ def __radd__(self, other):
+ return self.__add__(other)
+
+ def __iadd__(self, other):
+ inertial = self.__add__(other)
+ self.mass = inertial.mass
+ self.inertia = inertial.inertia
+ self.frame = Frame(position=inertial.position, orientation=inertial.orientation)
+
class Geometry(object): # Shape
"""Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, cone, heightmap, etc.
@@ -1250,6 +1881,18 @@ class Visual(object):
self.material = Material(name=material_name, color=color, texture=texture, diffuse=diffuse, specular=specular,
emissive=emissive)
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def dtype(self):
"""Return the primitive shape type."""
@@ -1362,6 +2005,18 @@ class Collision(object):
self.geometry = Geometry(dtype=dtype, size=size, filename=filename)
self.frame = Frame(position=position, orientation=orientation)
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def dtype(self):
"""Return the primitive shape type."""
@@ -1478,6 +2133,18 @@ class Material(object):
self.specular = specular
self.emissive = emissive
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@staticmethod
def _check_color(color):
"""Check the given color (its type and length) and convert it to a tuple of float."""
@@ -1569,6 +2236,18 @@ class Sensor(object):
self.name = name
self.sensors = sensors
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def num_sensors(self):
"""Return the number of inner sensors."""
@@ -1591,6 +2270,18 @@ class Actuator(object): # Motor
self.name = name
self.actuators = actuators
+ @property
+ def name(self):
+ """Return the name."""
+ return self._name
+
+ @name.setter
+ def name(self, name):
+ """Set the name."""
+ if name is not None and not isinstance(name, str):
+ raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
+ self._name = name
+
@property
def num_actuators(self):
"""Return the number of inner actuators."""
diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
index 74f05e8..dce0fd8 100644
--- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py
+++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
@@ -10,7 +10,7 @@ References:
- MuJoCo XML format: http://www.mujoco.org/book/XMLreference.html
"""
-# import XML parser
+import numpy as np
import xml.etree.ElementTree as ET
# import mesh converter (from .obj to .stl)
@@ -24,10 +24,11 @@ try:
# 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`")
+ raise ImportError(str(e) + "\nTry to install trimesh pyassimp: `pip install trimesh pyassimp`")
from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
-from pyrobolearn.utils.parsers.robots.data_structures import *
+from pyrobolearn.utils.parsers.robots.data_structures import Simulator, World, Tree, Body, Joint, Inertial, \
+ Visual, Collision, Light
from pyrobolearn.utils.transformation import rotation_matrix_x, rotation_matrix_y, rotation_matrix_z
@@ -42,154 +43,84 @@ __status__ = "Development"
class MuJoCoParser(WorldParser):
- r"""MuJoCo Parser and Generator"""
+ r"""MuJoCo Parser and Generator
+
+ The MuJoCo parser and generator keeps track of two data structures:
+
+ 1. The XML tree describing the world (such that we can generate an XML file or string from it).
+ 2. The World data structure that we can pass to other generators to generate their corresponding world file.
+
+ Using this class, you can `parse` an XML file or XML string that will automatically generate the XML tree and
+ the `World` data structure. You can also build the tree from scratch by yourself using the provided methods.
+ However, note that with the latter approach, you will have to generate the `World` data structure by yourself if
+ you need it, by calling the `parse` method.
+
+
+ Example of a simple MuJoCo file from [1]:
+
+
+
+
+
+
+
+
+
+
+
+
+ Notes:
+ - MuJoCo only accepts STL meshes
+ - MuJoCo can load PNG files for textures and heightmap.
+
+ Parts of the documentation has been copied-pasted from [1, 2] for completeness purpose.
+
+ References:
+ - [1] MuJoCo overview: http://www.mujoco.org/book/index.html
+ - [2] MuJoCo XML format: http://www.mujoco.org/book/XMLreference.html
+ """
def __init__(self, filename=None):
"""
- Initialize the MuJoCo parser.
+ Initialize the MuJoCo parser and generator.
Args:
filename (str, None): path to the MuJoCo XML file.
"""
super().__init__(filename)
- self.simulator = None
+ self.simulator = Simulator()
self.compiler = dict() # set options for the built-in parser and compiler
self.options = dict() # simulation options
self.defaults = dict() # default values for the attributes when they are not specified
self.assets = dict() # assets (textures, meshes, etc)
- def parse(self, filename):
+ # create root XML element
+ self.create_root("mujoco")
+ self.worldbody = self.add_element(name="worldbody", parent_element=self.root)
+
+ # set some counters
+ self._world_cnt = 0
+ self._tree_cnt = 0
+ self._body_cnt = 0
+ self._joint_cnt = 0
+ # self._geom_cnt = 0
+ # self._site_cnt = 0
+
+ ##########
+ # Parser #
+ ##########
+
+ def _get_orientation(self, attrib):
"""
- Load and parse the given MuJoCo XML file.
+ Get the orientation based on the XML attributes.
Args:
- filename (str): path to the MuJoCo XML file.
+ attrib (dict): dictionary which contains 'quat', 'euler' (with possibly 'eulereq'), 'axiangle', 'xyaxes',
+ 'zaxis'}.
+
+ Returns:
+ np.array[float[3,3]], np.array[float[3]], str, None: orientation.
"""
- # 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 != 'mujoco':
- raise RuntimeError("Expecting the first XML tag to be 'mujoco' but found instead: {}".format(root.tag))
-
- # build the world
- world = World(name=root.attrib.get('model', 'world'))
-
- # check compiler
- compiler_tag = root.find('compiler') # TODO: check other
- if compiler_tag is not None:
-
- def update_compiler(attributes):
- for attribute in attributes:
- attrib = compiler_tag.attrib.get(attribute)
- if attrib is not None:
- self.compiler[attribute] = attrib
-
- coordinate = compiler_tag.attrib.get('coordinate')
- if coordinate == 'global':
- raise NotImplementedError("Currently, we only support local coordinate frames.")
-
- update_compiler(['coordinate', 'angle', 'meshdir', 'texturedir', 'eulerseq', 'discardvisual',
- 'convexhull', 'inertiafromgeom', 'fitaabb', 'fusestatic'])
-
- # check default (this is the default configuration when they are not specified)
- default_tag = root.find('default')
- if default_tag is not None:
-
- def update_default(tag, attributes):
- tag = default_tag.find(tag)
- if tag is not None:
- self.defaults[tag] = {}
- for attribute in attributes:
- item = tag.attrib.get(attribute)
- if item is not None:
- self.defaults['tag'][attribute] = item
-
- update_default('mesh', ['scale'])
- update_default('material', ['texture', 'emission', 'specular', 'shininess', 'reflectance', 'rgba'])
- update_default('joint', ['type', 'pos', 'axis', 'limited', 'range', 'springdamper', 'stiffness',
- 'damping', 'frictionloss', 'armature', 'margin', 'ref', 'springref'])
- update_default('geom', ['type', 'contype', 'conaffinity', 'condim', 'size', 'material', 'rgba',
- 'friction', 'mass', 'density', 'margin', 'fromto', 'pos', 'quat', 'axisangle',
- 'xyaxes', 'zaxis', 'euler', 'hfield', 'mesh'])
- update_default('site', ['type', 'material', 'rgba', 'size', 'fromto', 'pos', 'quat', 'axisangle',
- 'xyaxes', 'zaxis', 'euler'])
- update_default('camera', ['mode', 'target', 'fovy', 'ipd', 'pos', 'quat', 'axisangle', 'xyaxes', 'zaxis',
- 'euler'])
- update_default('light', ['mode', 'target', 'directional', 'castshadow', 'active', 'pos', 'dir',
- 'attenuation', 'cutoff', 'exponent', 'ambient', 'diffuse', 'specular'])
- update_default('pair', ['condim', 'friction', 'margin', 'gap'])
- update_default('equality', ['active'])
- # update_default('tendon', [''])
- update_default('general', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
- 'gear', 'cranklength', 'dyntype', 'gaintype', 'biastype', 'dynprm', 'gainprm',
- 'biasprm'])
- # name, class, joint, jointinparent, site, tendon, slidersite, cranksite
- update_default('motor', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
- 'cranklength'])
- update_default('position', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
- 'gear', 'cranklength', 'kp'])
- update_default('velocity', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
- 'gear', 'cranklength', 'kv'])
- update_default('cylinder', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
- 'gear', 'cranklength', 'timeconst', 'area', 'diameter', 'bias'])
- update_default('muscle', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
- 'cranklength', 'timeconst', 'range', 'force', 'scale', 'lmin', 'lmax', 'vmax',
- 'fpmax', 'fvmax'])
-
- # check options
- option_tag = root.find('option')
- if option_tag is not None:
- self.options = option_tag.attrib
- # TODO: check flag
-
- # check assets
- asset_tag = root.find('asset')
- if asset_tag is not None:
- # check texture, material, and mesh
- for tag in ['texture', 'material', 'mesh']:
- for i, inner_tag in asset_tag.findall(tag):
- attrib = inner_tag.attrib
- if len(attrib) > 0:
- self.assets.setdefault(tag, dict())[attrib.get('name')] = attrib
-
- # check world body
- worldbody_tag = root.find('worldbody')
- if worldbody_tag is not None:
-
- # light
- for i, light_tag in enumerate(worldbody_tag.findall('light')):
- attrib = light_tag.attrib
- light = Light(name=attrib.get('name', 'light_' + str(i)), cast_shadows=attrib.get('castshadow'),
- position=attrib.get('pos'), direction=attrib.get('dir'), ambient=attrib.get('ambient'),
- diffuse=attrib.get('diffuse'), specular=attrib.get('specular'))
-
- world.lights[light.name] = light
-
- # check each (multi-)body
- for i, body_tag in enumerate(worldbody_tag.findall('body')):
- # create tree
- tree = Tree(name=body_tag.attrib.get('name', 'prl_multibody_' + str(i)))
-
- # check recursively body
- self._check_body(tree, body_tag, body_idx=i)
- world.trees[tree.name] = tree
-
- # check contact
-
- # check equality constraint
-
- # check actuator
-
- # check sensor
-
- # set the world
- self.world = world
-
- def _check_orientation(self, attrib):
orientation = None
# quaternion
@@ -240,35 +171,403 @@ class MuJoCoParser(WorldParser):
return orientation
- def _check_body(self, tree, body_tag, body_idx, parent_body=None, joint_idx=0): # TODO: check with self.defaults
+ def parse(self, filename):
+ """
+ Load and parse the given MuJoCo XML file.
+
+ Args:
+ filename (str, ET.Element): path to the MuJoCo XML file, or XML root element.
+ """
+ if isinstance(filename, str):
+ # load and parse the XML file
+ tree_xml = ET.parse(filename)
+ # get the root
+ root = tree_xml.getroot()
+ elif isinstance(filename, ET.Element):
+ root = filename
+ else:
+ raise TypeError("Expecting the given 'filename' to be a string or an ET.Element, but got instead: "
+ "{}".format(type(filename)))
+
+ # 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 simulator data structure
+ self.simulator.name = root.attrib.get('model', 'simulator')
+
+ # parse compiler: This element is used to set options for the built-in parser and compiler. After parsing and
+ # compilation it no longer has any effect.
+ # compiler attributes: boundmass, boundinertia, settotalmass, balanceinertia, strippath, coordinate, angle,
+ # fitaabb, eulerseq, meshdir, texturedir, discardvisual, convexhull, userthread,
+ # fusestatic, inertiafromgeom, inertiagrouprange
+ self._parse_compiler(parent_tag=root)
+
+ # parse default: this is the default configuration when they are not specified
+ # default attribute: mesh, material, joint, geom, site, camera, light, pair, equality, tendon, general, motor,
+ # position, velocity, cylinder, muscle, custom
+ self._parse_default(parent_tag=root)
+
+ # parse options
+ self._parse_option(parent_tag=root)
+
+ # parse assets: This is a grouping element for defining assets. Assets are created in the model so that they
+ # can be referenced from other model elements
+ # asset attributes: texture, hfield, mesh, skin, material
+ self._parse_asset(parent_tag=root)
+
+ # parse (world) body: This element is used to construct the kinematic tree via nesting. The element worldbody
+ # is used for the top-level body, while the element body is used for all other bodies.
+ # body attributes: name, childclass, mocap, pos, quat, axisangle, xyaxes, zaxis, euler
+ self._parse_worldbody(parent_tag=root, update_world_attribute=True)
+
+ # parse contact
+ # contact attributes: pair,
+ self._parse_contact(parent_tag=root)
+
+ # parse equality constraint
+ # equality attributes: connect, weld, joint, tendon, distance
+ self._parse_equality_constraint(parent_tag=root)
+
+ # parse actuator
+ # actuator attributes: general, motor, position, velocity, cylinder, muscle
+ self._parse_actuator(parent_tag=root)
+
+ # parse sensor
+ # sensor attributes: touch, accelerometer, velocimeter, gyro, force, torque, magnetometer, rangefinder,
+ # jointpos, jointvel, tendonpos, tendonvel, actuatorpos, actuatorvel, actuatorfrce,
+ # ballquat, ballangvel, jointlimitpos, jointlimitvel, jointlimitfrc, tendonlimitpos,
+ # tendonlimitvel, tendonlimitfrc, framepos, framequat, framexaxis, frameyaxis, framezaxis,
+ # framelinvel, frameangvel, framelinacc, frameangacc, subtreecom, subtreelinvel,
+ # subtreeangmom
+ self._parse_sensor(parent_tag=root)
+
+ def _parse_compiler(self, parent_tag):
+ """
+ Parse the compiler tag if present, and update the `compiler` attribute of this class.
+
+ From the main documentation [2]: "This element is used to set options for the built-in parser and compiler.
+ After parsing and compilation it no longer has any effect. The settings here are global and apply to the
+ entire model.
+
+ Attributes:
+ - boundmass (real, "0"): This attribute imposes a lower bound on the mass of each body except for the
+ world body. It can be used as a quick fix for poorly designed models that contain massless moving
+ bodies, such as the dummy bodies often used in URDF models to attach sensors. Note that in MuJoCo
+ there is no need to create dummy bodies.
+ - boundinertia (real, "0"): This attribute imposes a lower bound on the diagonal inertia components of
+ each body except for the world body.
+ - settotalmass (real, "-1"): If this value is positive, the compiler will scale the masses and inertias of
+ all bodies in the model, so that the total mass equals the value specified here. The world body has mass
+ 0 and does not participate in any mass-related computations. This scaling is performed last, after all
+ other operations affecting the body mass and inertia.
+ - balanceinertia ([false, true], "false"): A valid diagonal inertia matrix must satisfy A+B>=C for all
+ permutations of the three diagonal elements. Some poorly designed models violate this constraint, which
+ will normally result in compile error. If this attribute is set to "true", the compiler will silently
+ set all three diagonal elements to their average value whenever the above condition is violated.
+ - etc
+ "
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ compiler_tag = parent_tag.find('compiler') # TODO: check other
+ if compiler_tag is not None:
+
+ def update_compiler(attributes):
+ """
+ Update the `compiler` attribute of this class.
+
+ Args:
+ attributes (list[str]): list of attributes to check.
+ """
+ for attribute in attributes:
+ attrib = compiler_tag.attrib.get(attribute)
+ if attrib is not None:
+ self.compiler[attribute] = attrib
+
+ coordinate = compiler_tag.attrib.get('coordinate')
+ if coordinate == 'global':
+ raise NotImplementedError("Currently, we only support local coordinate frames.")
+
+ update_compiler(['coordinate', 'angle', 'meshdir', 'texturedir', 'eulerseq', 'discardvisual',
+ 'convexhull', 'inertiafromgeom', 'fitaabb', 'fusestatic'])
+
+ def _parse_option(self, parent_tag):
+ """
+ Parse the option tag if present, and update the `options` attribute of this class.
+
+ From the main documentation [2]: "This element is is one-to-one correspondence with the low level structure
+ mjOption contained in the field mjModel.opt of mjModel. Options can be modified during runtime by the user."
+
+ Option attributes: timestep, apirate, impratio, gravity, wind, magnetic, density, viscosity, o_margin,
+ o_solref, o_solimp, integrator, collision, cone, jacobian, solver, iterations, tolerance,
+ noslip_iterations, noslip_tolerance, mpr_iterations, mpr_tolerance
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ option_tag = parent_tag.find('option')
+ if option_tag is not None:
+ self.options = option_tag.attrib
+ # TODO: check flag
+
+ def _parse_default(self, parent_tag):
+ """
+ Parse the default tag if present, and update the `defaults` attribute of this class.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ default_tag = parent_tag.find('default')
+ if default_tag is not None:
+
+ def update_default(tag, attributes):
+ """
+ Update the default dictionary which will be useful later.
+
+ Args:
+ tag (str): tag to check under the default XML element.
+ attributes (list[str]): list of attributes to check in the specified tag XML element (if found).
+ """
+ # find tag
+ tag = default_tag.find(tag)
+
+ # if tag was found
+ if tag is not None:
+ # go through each attribute and update the default dict
+ self.defaults[tag] = {}
+ for attribute in attributes:
+ item = tag.attrib.get(attribute)
+ if item is not None:
+ self.defaults['tag'][attribute] = item
+
+ update_default('mesh', ['scale'])
+ update_default('material', ['texture', 'emission', 'specular', 'shininess', 'reflectance', 'rgba'])
+ update_default('joint', ['type', 'pos', 'axis', 'limited', 'range', 'springdamper', 'stiffness',
+ 'damping', 'frictionloss', 'armature', 'margin', 'ref', 'springref'])
+ update_default('geom', ['type', 'contype', 'conaffinity', 'condim', 'size', 'material', 'rgba',
+ 'friction', 'mass', 'density', 'margin', 'fromto', 'pos', 'quat', 'axisangle',
+ 'xyaxes', 'zaxis', 'euler', 'hfield', 'mesh'])
+ update_default('site', ['type', 'material', 'rgba', 'size', 'fromto', 'pos', 'quat', 'axisangle',
+ 'xyaxes', 'zaxis', 'euler'])
+ update_default('camera', ['mode', 'target', 'fovy', 'ipd', 'pos', 'quat', 'axisangle', 'xyaxes', 'zaxis',
+ 'euler'])
+ update_default('light', ['mode', 'target', 'directional', 'castshadow', 'active', 'pos', 'dir',
+ 'attenuation', 'cutoff', 'exponent', 'ambient', 'diffuse', 'specular'])
+ update_default('pair', ['condim', 'friction', 'margin', 'gap'])
+ update_default('equality', ['active'])
+ # update_default('tendon', [''])
+ update_default('general', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+ 'gear', 'cranklength', 'dyntype', 'gaintype', 'biastype', 'dynprm', 'gainprm',
+ 'biasprm'])
+ # name, class, joint, jointinparent, site, tendon, slidersite, cranksite
+ update_default('motor', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
+ 'cranklength'])
+ update_default('position', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+ 'gear', 'cranklength', 'kp'])
+ update_default('velocity', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+ 'gear', 'cranklength', 'kv'])
+ update_default('cylinder', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+ 'gear', 'cranklength', 'timeconst', 'area', 'diameter', 'bias'])
+ update_default('muscle', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
+ 'cranklength', 'timeconst', 'range', 'force', 'scale', 'lmin', 'lmax', 'vmax',
+ 'fpmax', 'fvmax'])
+
+ def _parse_asset(self, parent_tag):
+ """
+ Parse the asset tag if present, and update the 'assets' attribute of this class.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ asset_tag = parent_tag.find('asset')
+ if asset_tag is not None:
+ # check texture, material, and mesh
+ for tag in ['texture', 'material', 'mesh']:
+ for i, inner_tag in asset_tag.findall(tag):
+ attrib = inner_tag.attrib
+ if len(attrib) > 0:
+ self.assets.setdefault(tag, dict())[attrib.get('name')] = attrib
+
+ def _parse_worldbody(self, parent_tag, update_world_attribute=False):
+ """
+ Parse the worldbody tag if present, and instantiate the `World` data structure, update the `world` attribute
+ of this class, and add the world to the simulator.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ update_world_attribute (bool): if we should
+ """
+ worldbody_tag = parent_tag.find('worldbody')
+ if worldbody_tag is not None:
+
+ # instantiate the world data structure
+ name = parent_tag.attrib.get('model')
+ if name is None:
+ name = 'prl_world_' + str(self._world_cnt)
+ self._world_cnt += 1
+ world = World(name=name)
+
+ # light: This element creates a light, which moves with the body where it is defined.
+ # light attributes: name, class, mode, target, directional, castshadow, active, pos, dir, attenuation,
+ # cutoff, exponent, ambient, diffuse, specular
+ for i, light_tag in enumerate(worldbody_tag.findall('light')):
+ attrib = light_tag.attrib
+ light = Light(name=attrib.get('name', 'light_' + str(i)), cast_shadows=attrib.get('castshadow'),
+ position=attrib.get('pos'), direction=attrib.get('dir'), ambient=attrib.get('ambient'),
+ diffuse=attrib.get('diffuse'), specular=attrib.get('specular'))
+
+ world.lights[light.name] = light
+
+ # check each (multi-)body
+ for i, body_tag in enumerate(worldbody_tag.findall('body')):
+ # create tree
+ name = body_tag.attrib.get('name')
+ if name is None:
+ name = 'prl_multibody_' + str(self._tree_cnt)
+ self._tree_cnt += 1
+ tree = Tree(name=name)
+
+ # check recursively body
+ self._parse_body(tree, body_tag=body_tag)
+
+ # save tree
+ world.trees[tree.name] = tree
+
+ # update the world attribute of this class if specified.
+ if update_world_attribute:
+ self.world = world
+
+ def _parse_body(self, tree, body_tag, parent_body=None): # TODO: check with self.defaults
"""
Construct recursively the given tree, and return Body instance from a .
Args:
tree (Tree): tree data structure containing the model.
body_tag (ET.Element): body XML element.
- body_idx (int): link index.
parent_body (Body, None): the parent body instance.
- joint_idx (int): joint index.
Returns:
Body: body data structure.
"""
# create body
- body = Body(body_id=body_idx, name=body_tag.attrib.get('name', 'prl_body_' + str(body_idx)))
+ name = body_tag.attrib.get('name')
+ if name is None:
+ name = 'prl_body_' + str(self._body_cnt)
+ self._body_cnt += 1
+ body = Body(body_id=self._body_cnt, name=name)
# add body to the tree
tree.bodies[body.name] = body
# check inertial
- inertial = Inertial()
inertial_tag = body_tag.find('inertial')
+ self._parse_inertial(body, inertial_tag)
+
+ # check geoms: geoms (short for geometric primitive) are used to specify appearance and collision geometry.
+ # geom attributes: name, class, type, contype, conaffinity, condim, group, priority, size, material, rgba,
+ # friction, mass, density, solmix, solref, solimp, margin, gap, fromto, pos, quat, axisangle,
+ # xyaxes, zaxis, euler, hfield, mesh, fitscale, user
+ for i, geom_tag in enumerate(body_tag.findall('geom')):
+ self._parse_geom(body=body, geom_tag=geom_tag, geom_idx=i)
+
+ # check sites: Sites are light geoms. They have the same appearance properties but cannot participate in
+ # collisions and cannot be used to infer body masses.
+ for i, site_tag in enumerate(body_tag.findall('site')):
+ self._parse_site(body=body, site_tag=site_tag, site_idx=i)
+
+ # check joints that connect the current body with its parent
+ joints = []
+ for i, joint_tag in enumerate(body_tag.findall('joint')):
+ # create joint with the corresponding attributes
+ joint = self._parse_joint(tree, body, joint_tag, parent_body=parent_body)
+ joints.append(joint)
+
+ # check bodies
+ for i, new_body_tag in enumerate(body_tag.findall('body')):
+ self._parse_body(tree, new_body_tag, parent_body=body)
+
+ # check include
+ for i, include_tag in enumerate(body_tag.findall('include')):
+ # create MuJoCoParser
+ parser = MuJoCoParser(filename=include_tag.attrib.get('include'))
+
+ # get the tree
+ raise NotImplementedError("We can not parse the tag yet...")
+
+ def _parse_joint(self, tree, body, joint_tag, parent_body=None):
+ """
+ Parse the joint tag if present and return the joint data structure.
+
+ Args:
+ tree (Tree): tree data structure containing the model.
+ body (Body): body data structure instance.
+ joint_tag (ET.Element): joint XML tag.
+ parent_body (Body, None): the parent body instance.
+
+ Returns:
+ Joint: joint data structure instance.
+ """
+ # create joint with the corresponding attributes
+ attrib = joint_tag.attrib
+
+ # get joint name
+ name = attrib.get('name')
+ if name is None:
+ name = 'prl_joint_' + str(self._joint_cnt)
+ self._joint_cnt += 1
+
+ # create joint data structure
+ joint = Joint(joint_id=self._joint_cnt, name=name, dtype=attrib.get('type'), position=attrib.get('pos'),
+ axis=attrib.get('axis'), friction=attrib.get('frictionloss'), damping=attrib.get('damping'),
+ parent=parent_body, child=body)
+
+ limited = attrib.get('limited')
+ if limited is not None:
+ limited = limited.lower().strip()
+ if limited == 'true':
+ joint.limits = attrib.get('range')
+
+ # add joint in tree and parent body
+ tree.joints[joint.name] = joint
+ if parent_body is not None:
+ parent_body.joints[joint.name] = joint
+
+ return joint
+
+ def _parse_inertial(self, body, inertial_tag): # DONE
+ """
+ Parse the inertial tag if present, and set the inertial data structure to the given body.
+
+ From the main documentation [2]: "This element specifies the mass and inertial properties of the body. If this
+ element is not included in a given body, the inertial properties are inferred from the geoms attached to the
+ body. When a compiled MJCF model is saved, the XML writer saves the inertial properties explicitly using this
+ element, even if they were inferred from geoms. The inertial frame is such that its center coincides with the
+ center of mass of the body, and its axes coincide with the principal axes of inertia of the body. Thus the
+ inertia matrix is diagonal in this frame.
+
+ Attributes:
+ - pos (real[3], required): position of the inertial frame.
+ - quat, axisangle, xyaxes, zaxis, euler: orientation of the inertial frame.
+ - mass (real, required): mass of the body.
+ - diaginertia (real[3], optional): diagonal inertia matrix, expressing the body inertia relative to the
+ inertial frame.
+ - fulldiagonal (real[6], optional): Full inertia matrix M (Ixx, Iyy, Izz, Ixy, Ixz, Iyz)."
+
+ Args:
+ body (Body): body data structure instance.
+ inertial_tag (ET.Element, None): XML element.
+ """
if inertial_tag is not None:
+ # instantiate inertial data structure
+ inertial = Inertial()
+
# position and orientation
position = inertial_tag.attrib.get('pos')
if position is not None:
inertial.position = position
- orientation = self._check_orientation(inertial_tag.attrib)
+ orientation = self._get_orientation(inertial_tag.attrib)
if orientation is not None:
inertial.orientation = orientation
@@ -280,218 +579,209 @@ class MuJoCoParser(WorldParser):
else:
inertial.inertia = inertial_tag.attrib.get('fullinertia')
- # check geoms
- for i, geom_tag in enumerate(body_tag.findall('geom')):
+ # add inertial element to body
+ body.add_inertial(inertial)
- ##########
- # visual #
- ##########
+ def _parse_geom(self, body, geom_tag, geom_idx):
+ """
+ Parse the geom tag if present, and set the visuals, and collisions to the given body. It can also set the
+ inertial elements if it was not defined previously.
- attrib = geom_tag.attrib
- dtype = attrib.get('type')
- visual = Visual(name=attrib.get('name'), dtype=dtype, color=attrib.get('rgba'))
+ From the main documentation [2]: "This element creates a geom, and attaches it rigidly to the body within
+ which the geom is defined. Multiple geoms can be attached to the same body. At runtime they determine the
+ appearance and collision properties of the body. At compile time they can also determine the inertial
+ properties of the body, depending on the presence of the inertial element and the setting of the
+ inertiafromgeom attribute of compiler. This is done by summing the masses and inertias of all geoms attached
+ to the body with geom group in the range specified by the inertiagrouprange attribute of compiler. The geom
+ masses and inertias are computed using the geom shape, a specified density or a geom mass which implies a
+ density, and the assumption of uniform density.
- # get position and orientation
- visual.pos = attrib.get('pos')
- visual.orientation = self._check_orientation(attrib)
+ Attributes:
+ - name (string, optional): Name of the geom.
+ - class (string, optional): Defaults class for setting unspecified attributes.
+ - type (string, [plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh], "sphere"): Type of
+ geometric shape
+ - contype (int, "1"): This attribute and the next specify 32-bit integer bitmasks used for contact
+ filtering of dynamically generated contact pairs. Two geoms can collide if the contype of one geom is
+ compatible with the conaffinity of the other geom or vice versa. Compatible means that the two bitmasks
+ have a common bit set to 1.
- # compute size (rescale them)
- if dtype == 'plane':
- size = attrib.get('size')
+ Args:
+ body (Body): body data structure instance.
+ geom_tag (ET.Element): geom XML field.
+ geom_idx (int): geom index.
+ """
+ ##########
+ # visual #
+ ##########
- if dtype in {'capsule', 'cylinder', 'ellipsoid', 'box'}:
- fromto = attrib.get('fromto')
- if fromto is not None:
- fromto = np.array([float(n) for n in fromto.split()])
- from_pos, to_pos = fromto[:3], fromto[3:]
- v = to_pos - from_pos
- pos = from_pos + v / 2.
- length = np.linalg.norm(v)
- z = v / length
- z_ = np.zeros([0., 0., 1.]) # old z axis
- x = np.cross(z, z_)
- y = np.cross(z, x)
- rot = np.array([x, y, z]).T
+ attrib = geom_tag.attrib
+ dtype = attrib.get('type')
+ visual = Visual(name=attrib.get('name'), dtype=dtype, color=attrib.get('rgba'))
- # set new position and orientation
- visual.pos = pos
- visual.orientation = rot
+ # get position and orientation
+ visual.pos = attrib.get('pos')
+ visual.orientation = self._get_orientation(attrib)
- # set size
- if dtype == 'capsule':
- size = None # TODO
+ # compute size (rescale them)
+ if dtype == 'plane':
+ size = attrib.get('size')
- # check texture
- material = attrib.get('material')
- if material in self.assets:
- pass
+ if dtype in {'capsule', 'cylinder', 'ellipsoid', 'box'}:
+ fromto = attrib.get('fromto')
+ if fromto is not None:
+ fromto = np.array([float(n) for n in fromto.split()])
+ from_pos, to_pos = fromto[:3], fromto[3:]
+ v = to_pos - from_pos
+ pos = from_pos + v / 2.
+ length = np.linalg.norm(v)
+ z = v / length
+ z_ = np.zeros([0., 0., 1.]) # old z axis
+ x = np.cross(z, z_)
+ y = np.cross(z, x)
+ rot = np.array([x, y, z]).T
- # check mesh
- mesh = attrib.get('mesh')
- if mesh is not None:
- # get the mesh from the assets
- mesh_dict = self.assets.get('mesh')
- if mesh_dict is not None:
- mesh = mesh_dict.get(mesh)
+ # set new position and orientation
+ visual.pos = pos
+ visual.orientation = rot
- # check mesh format
+ # set size
+ if dtype == 'capsule':
+ size = None # TODO
- # get the texture for the mesh
-
- # set visual to body
- body.visual = visual
-
- #############
- # collision #
- #############
-
- if not (attrib.get('contype') == "0" and attrib.get('conaffinity') == "0"):
- # copy collision shape information from visual shape
- collision = Collision()
- collision.name = visual.name
- collision.frame = visual.frame
- collision.geometry = visual.geometry
- body.collision = collision
-
- ############
- # inertial # just the mass
- ############
-
- # if the tag was not given, compute based on information in geom
- if inertial.mass is None or inertial.inertia is None:
-
- # if mesh, load it in memory
- if dtype == 'mesh':
- mesh = trimesh.load(mesh_path)
- if not mesh.is_watertight: # mesh.is_convex
- raise ValueError("Could not compute the volume because the mesh is not watertight...")
-
- # get mass
- mass = inertial.mass
- if mass is None:
- if attrib.get('mass') is not None:
- inertial.mass = attrib.get('mass')
- mass = inertial.mass # this makes the conversion to float
- else:
- # get density
- density = float(attrib.get('density', 1000.))
-
- # get mass by computing the volume
- volume = 1
- if dtype == 'box':
- w, h, d = visual.size # width, height, depth
- volume = w*h*d
- elif dtype == 'capsule':
- r, h = visual.size # radius, height
- sphere_volume = 4. / 3 * np.pi * r ** 3
- cylinder_volume = np.pi * r ** 2 * h
- volume = sphere_volume + cylinder_volume
- elif dtype == 'cylinder':
- r, h = visual.size # radius, height
- volume = np.pi * r ** 2 * h
- elif dtype == 'ellipsoid':
- a, b, c = visual.size
- volume = 4./3 * np.pi * a * b * c
- elif dtype == 'mesh':
- scale = float(attrib.get('fitscale', 1))
- volume = mesh.volume # in m^3 (in trimesh: mash.mass = mash.volume, i.e. density = 1)
- volume *= scale ** 3 # the scale is for each dimension
- elif dtype == 'sphere':
- r = visual.size # radius
- volume = 4. / 3 * np.pi * r ** 3
- mass = density * volume
- inertial.mass = mass
-
- # compute inertia if not given
- if inertial.inertia is None:
- inertia = None
- if dtype == 'box':
- w, h, d = visual.size # width, height, depth
- inertia = 1./12 * mass * np.array([h**2 + d**2, w**2 + d**2, w**2 + h**2])
- elif dtype == 'capsule':
- r, h = visual.size # radius, height
-
- # get mass of cylinder and hemisphere
- sphere_volume = 4./3 * np.pi * r**3
- cylinder_volume = np.pi * r**2 * h
- volume = sphere_volume + cylinder_volume
- density = mass / volume
- m_s = density * sphere_volume # sphere mass = 2 * hemisphere mass
- m_c = density * cylinder_volume # cylinder mass
-
- # from: https://www.gamedev.net/articles/programming/math-and-physics/capsule-inertia-\
- # tensor-r3856/
- ixx = m_c * (h**2/12. + r**2/4.) + m_s * (2*r**2/5. + h**2/2. + 3*h*r/8.)
- iyy = ixx
- izz = m_c * r**2/2. + m_s * 2 * r**2 / 5.
- inertia = np.array([ixx, iyy, izz])
- elif dtype == 'cylinder':
- r, h = visual.size # radius, height
- inertia = 1./12 * mass * np.array([3*r**2 + h**2, 3*r**2 + h**2, r**2])
- elif dtype == 'ellipsoid':
- a, b, c = visual.size
- inertia = 1./5 * mass * np.array([b**2 + c**2, a**2 + c**2, a**2 + b**2])
- elif dtype == 'mesh':
- scale = float(attrib.get('fitscale', 1))
-
- volume = mesh.volume # in m^3 (in trimesh: mash.mass = mash.volume, i.e. density = 1)
- volume *= scale**3 # the scale is for each dimension
- inertia = mesh.moment_inertia * scale**2 # I ~ mr^2
-
- # the previous inertia is based on the assumption that mesh.mass = mesh.volume
- density = mass / volume # density = new_mass / old_mass
- inertia *= density
-
- # com = mesh.center_mass * scale # uniform density assumption
- # (mesh.center_mass is a bit different from mesh.centroid)
- elif dtype == 'sphere':
- radius = visual.size
- inertia = 2./5 * mass * radius**2 * np.ones(3)
-
- inertial.inertia = inertia
-
- # check sites
- for i, site_tag in enumerate(body_tag.findall('site')):
+ # check texture
+ material = attrib.get('material')
+ if material in self.assets:
pass
- # check joints that connect the current body with its parent
- joints = []
- for i, joint_tag in enumerate(body_tag.findall('joint')):
+ # check mesh
+ mesh = attrib.get('mesh')
+ if mesh is not None:
+ # get the mesh from the assets
+ mesh_dict = self.assets.get('mesh')
+ if mesh_dict is not None:
+ mesh = mesh_dict.get(mesh)
- # create joint with the corresponding attributes
- attrib = joint_tag.attrib
- joint = Joint(joint_id=joint_idx, name=attrib.get('name', 'prl_joint_' + str(joint_idx)),
- dtype=attrib.get('type'), position=attrib.get('pos'), axis=attrib.get('axis'),
- friction=attrib.get('frictionloss'), damping=attrib.get('damping'),
- parent=parent_body, child=body)
+ # check mesh format
- limited = attrib.get('limited')
- if limited is not None:
- limited = limited.lower().strip()
- if limited == 'true':
- joint.limits = attrib.get('range')
+ # get the texture for the mesh
- # add joint in tree and parent body
- tree.joints[joint.name] = joint
- if parent_body is not None:
- parent_body.joints[joint.name] = joint
+ # set visual to body
+ body.visuals = visual
- # increment joint counter
- joint_idx += 1
- joints.append(joint)
+ #############
+ # collision #
+ #############
- # check bodies
- for i, new_body_tag in enumerate(body_tag.findall('body')):
- body_idx += 1
- self._check_body(tree, new_body_tag, body_idx=body_idx, parent_body=body, joint_idx=joint_idx)
+ if not (attrib.get('contype') == "0" and attrib.get('conaffinity') == "0"):
+ # copy collision shape information from visual shape
+ collision = Collision()
+ collision.name = visual.name
+ collision.frame = visual.frame
+ collision.geometry = visual.geometry
+ body.collisions = collision
- # check include
- for i, include_tag in enumerate(body_tag.findall('include')):
- # create MuJoCoParser
- parser = MuJoCoParser(filename=include_tag.attrib.get('include'))
+ ############
+ # inertial # just the mass
+ ############
- # get the tree
- raise NotImplementedError("We can not parse the tag yet...")
+ # if the tag was not given, compute based on information in geom
+ if body.inertial is None:
+ inertial = Inertial()
+
+ # if mesh, load it in memory
+ if dtype == 'mesh':
+ mesh = trimesh.load(mesh)
+ if not mesh.is_watertight: # mesh.is_convex
+ raise ValueError("Could not compute the volume because the mesh is not watertight...")
+
+ # get mass
+ mass = inertial.mass
+ if mass is None:
+
+ # if the mass is defined
+ if attrib.get('mass') is not None:
+ inertial.mass = attrib.get('mass')
+ mass = inertial.mass # this makes the conversion to float
+
+ # if the mass is not defined, compute it from the density
+ else:
+ density = float(attrib.get('density', 1000.))
+ dimensions = float(attrib.get('fitscale', 1)) if dtype == 'mesh' else visual.size
+ mass = Inertial.compute_mass_from_density(shape=dtype, dimensions=dimensions, density=density,
+ mesh=mesh)
+ inertial.mass = mass
+
+ # compute inertia if not given
+ if inertial.inertia is None:
+ dimensions = float(attrib.get('fitscale', 1)) if dtype == 'mesh' else visual.size
+ inertia = Inertial.compute_inertia(shape=dtype, dimensions=dimensions, mass=mass, mesh=mesh)
+ inertial.inertia = inertia
+
+ # add inertial in body
+ body.add_inertial(inertial)
+
+ def _parse_site(self, body, site_tag, site_idx):
+ """
+ Parse the site XML field.
+
+ From the main documentation [1]: "Sites are light geoms. They have the same appearance properties but cannot
+ participate in collisions and cannot be used to infer body masses. On the other hand sites can do things that
+ geoms cannot do: they can specify the volumes of touch sensors, the attachment of IMU sensors, the routing of
+ spatial tendons, the end-points of slider-crank actuators. These are all spatial quantities, and yet they do
+ not correspond to entities that should have mass or collide other entities - which is why the site element
+ was created. Sites can also be used to specify points (or rather frames) of interest to the user."
+
+
+ Args:
+ body (Body): body data structure instance.
+ site_tag (ET.Element): site XML field.
+ site_idx (int): site index.
+ """
+ ##########
+ # visual #
+ ##########
+ pass
+
+ def _parse_contact(self, parent_tag):
+ """
+ Parse contact XML field.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ pass
+
+ def _parse_equality_constraint(self, parent_tag):
+ """
+ Parse the equality XML field.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ pass
+
+ def _parse_actuator(self, parent_tag):
+ """
+ Parse the actuator XML field.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ pass
+
+ def _parse_sensor(self, parent_tag):
+ """
+ Parse the sensor XML field.
+
+ Args:
+ parent_tag (ET.Element): parent XML element to check if it has a 'compiler' tag.
+ """
+ pass
+
+ #############
+ # Generator #
+ #############
def generate(self, world=None): # TODO: check texture and mesh format
"""
@@ -556,3 +846,101 @@ class MuJoCoParser(WorldParser):
pyassimp.release(scene)
# create
+
+ def generate_body(self, parent_body, body):
+ r"""
+ Generate the body.
+
+ Args:
+ parent_body (ET.Element): parent body XML element to which the given Body data structure will be added.
+ body (Body): Body data structure.
+
+ Returns:
+ ET.Element: body XML element.
+ """
+ if not isinstance(parent_body, ET.Element):
+ raise TypeError("Expecting the given 'parent_body' to be an instance of `ET.Element`, but got instead: "
+ "{}".format(type(parent_body)))
+ if not isinstance(body, Body):
+ raise TypeError("Expecting the given 'body' to be an instance of `Body`, but got instead: "
+ "{}".format(type(body)))
+
+ body_tag = ET.SubElement(parent_body, "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"
+
+ def add_tree(self, tree):
+ r"""
+ Add the given tree / multi-body data structure.
+
+ Args:
+ tree (Tree, Body): multi-body data structure. If it is a body instance, it will automatically be
+ wrapped in a Tree instance.
+
+ Returns:
+ ET.Element: body XML element
+ """
+ if not isinstance(tree, Tree):
+ if isinstance(tree, Body):
+ tree = Tree(name=tree.name, root=tree, position=tree.position, orientation=tree.orientation)
+ else:
+ raise TypeError("Expecting the given 'tree' to be an instance of `Tree` or `Body` but got "
+ "instead: {}".format(type(tree)))
+
+ pass