update simulators, parsers and middlewares

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
 

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',

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()

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.
 

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)

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()

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)

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()

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():

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()

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

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