update simulators and ROS middleware integration

pyrobolearn/robots/robot.py

@@ -1629,10 +1629,10 @@ class Robot(ControllableBody):
 
         Args:
             link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links
-                associated to actuated joints.
+              associated to actuated joints.
             compute_link_velocity (bool): if True, the Cartesian world velocity will be computed and returned.
             compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed
-                using forward kinematics.
+              using forward kinematics.
 
         Returns:
             if 1 link:
@@ -2696,9 +2696,9 @@ class Robot(ControllableBody):
         Args:
             link_id (int): link id.
             q (np.array[float[N]], None): joint positions of size N, where N is the number of DoFs. If None, it will
-                compute q based on the current joint positions.
+              compute q based on the current joint positions.
             local_position (None, np.array[float[3]]): the point on the specified link to compute the Jacobian (in link
-                local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
+              local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
             np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The
@@ -2737,9 +2737,9 @@ class Robot(ControllableBody):
         Args:
             link_id (int): link id
             q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. If None, it will compute
-                q based on the current joint positions.
+              q based on the current joint positions.
             local_position: the point on the specified link to compute the Jacobian (in link local coordinates around
-                its center of mass). If None, it will use the CoM position (in the link frame).
+              its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
             np.array[float[3,N]], np.array[float[3,6+N]]: full linear geometric Jacobian matrix. The number of
@@ -2761,9 +2761,9 @@ class Robot(ControllableBody):
         Args:
             link_id (int): link id
             q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. If None, it will compute
-                q based on the current joint positions.
+              q based on the current joint positions.
             local_position: the point on the specified link to compute the Jacobian (in link local coordinates around
-                its center of mass). If None, it will use the CoM position (in the link frame).
+              its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
             np.array[float[3,N]], np.array[float[3,6+N]]: full angular geometric Jacobian matrix. The number of

pyrobolearn/robots/urdfs/rrbot/rrbot.yaml

@@ -0,0 +1,15 @@
+rrbot:
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50  
+  
+  # Position Controllers ---------------------------------------
+  joint1_position_controller:
+    type: effort_controllers/JointPositionController
+    joint: joint1
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+  joint2_position_controller:
+    type: effort_controllers/JointPositionController
+    joint: joint2
+    pid: {p: 100.0, i: 0.01, d: 10.0}

pyrobolearn/simulators/__init__.py

@@ -2,6 +2,9 @@
 # load middlewares
 from . import middlewares
 
+# check Python version
+import sys
+python_version = sys.version_info[0]
 
 # load all simulators
 
@@ -14,14 +17,24 @@ from .bullet import Bullet
 # Bullet ros simulator
 from .bullet_ros import BulletROS
 
-# Dart simulator
-# from .dart import Dart
+if python_version >= 3:
+    # Dart simulator
+    try:
+        from .dart import Dart
+    except ImportError as e:
+        print("Dart not found.")
 
-# MuJoCo simulator
-# from .mujoco import Mujoco
+    # MuJoCo simulator
+    try:
+        from .mujoco import Mujoco
+    except ImportError as e:
+        print("MuJoCo not found.")
 
 # Raisim simulator
-# from .raisim import Raisim
+try:
+    from .raisim import Raisim
+except ImportError as e:
+    print("Raisim not found.")
 
 # Vrep simulator (note that there is a currently a problem when loading pybullet with pyrep)
 # from .vrep import VREP

pyrobolearn/simulators/bullet.py

@@ -1735,15 +1735,15 @@ class Bullet(Simulator):
             link_id (int): link index.
             compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned.
             compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed
-                using forward kinematics.
+              using forward kinematics.
 
         Returns:
             np.array[float[3]]: Cartesian world position of CoM
             np.array[float[4]]: Cartesian world orientation of CoM, in quaternion [x,y,z,w]
             np.array[float[3]]: local position offset of inertial frame (center of mass) expressed in the URDF
-                link frame
+              link frame
             np.array[float[4]]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF
-                link frame
+              link frame
             np.array[float[3]]: world position of the URDF link frame
             np.array[float[4]]: world orientation of the URDF link frame (expressed as a quaternion [x,y,z,w])
             np.array[float[3]]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
@@ -2214,7 +2214,7 @@ class Bullet(Simulator):
             return np.asarray(self.sim.getJointInfo(body_id, joint_ids)[-4])
         return np.asarray([self.sim.getJointInfo(body_id, joint_id)[-4] for joint_id in joint_ids])
 
-    def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
+    def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
         """
         Set the position of the given joint(s) (using position control).
 
@@ -2230,7 +2230,7 @@ class Bullet(Simulator):
         self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=positions,
                                      velocities=velocities, forces=forces, kp=kps, kd=kds)
 
-    def get_joint_positions(self, body_id, joint_ids):
+    def _get_joint_positions(self, body_id, joint_ids):
         """
         Get the position of the given joint(s).
 
@@ -2248,7 +2248,7 @@ class Bullet(Simulator):
             return self.sim.getJointState(body_id, joint_ids)[0]
         return np.asarray([state[0] for state in self.sim.getJointStates(body_id, joint_ids)])
 
-    def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
+    def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
         """
         Set the velocity of the given joint(s) (using velocity control).
 
@@ -2269,7 +2269,7 @@ class Bullet(Simulator):
         self.sim.setJointMotorControlArray(body_id, joint_ids, self.sim.VELOCITY_CONTROL,
                                            targetVelocities=velocities, forces=max_force)
 
-    def get_joint_velocities(self, body_id, joint_ids):
+    def _get_joint_velocities(self, body_id, joint_ids):
         """
         Get the velocity of the given joint(s).
 
@@ -2372,7 +2372,7 @@ class Bullet(Simulator):
     #     q_idx = self.get_q_indices(body_id, joint_ids)
     #     return accelerations[q_idx]
 
-    def set_joint_torques(self, body_id, joint_ids, torques):
+    def _set_joint_torques(self, body_id, joint_ids, torques):
         """
         Set the torque/force to the given joint(s) (using force/torque control).
 
@@ -2385,7 +2385,7 @@ class Bullet(Simulator):
             self.sim.setJointMotorControl2(body_id, joint_ids, self.sim.TORQUE_CONTROL, force=torques)
         self.sim.setJointMotorControlArray(body_id, joint_ids, self.sim.TORQUE_CONTROL, forces=torques)
 
-    def get_joint_torques(self, body_id, joint_ids):
+    def _get_joint_torques(self, body_id, joint_ids):
         """
         Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
         Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
@@ -3397,8 +3397,8 @@ class Bullet(Simulator):
         Args:
             body_id (int): unique body id.
             link_id (int): link id.
-            local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local
-                coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
+            local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link
+              local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
             q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs.
             dq (np.array[float[N]]): joint velocities of size N, where N is the number of DoFs.
             des_ddq (np.array[float[N]]): desired joint accelerations of size N.

pyrobolearn/simulators/bullet_ros.py

@@ -231,13 +231,13 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: joint position [rad]
             if multiple joints:
-                np.float[N]: joint positions [rad]
+                np.array[float[N]]: joint positions [rad]
         """
         if body_id in self.subscribers:
             q = self.subscribers[body_id].get_joint_positions(joint_ids)
@@ -258,12 +258,12 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            positions (float, np.float[N]): desired position, or list of desired positions [rad]
-            velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
-            kps (None, float, np.float[N]): position gain(s)
-            kds (None, float, np.float[N]): velocity gain(s)
-            forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
+            velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            kps (None, float, np.array[float[N]]): position gain(s)
+            kds (None, float, np.array[float[N]]): velocity gain(s)
+            forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
         """
         super(BulletROS, self).set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
         if body_id in self.publishers:
@@ -276,13 +276,13 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: joint velocity [rad/s]
             if multiple joints:
-                np.float[N]: joint velocities [rad/s]
+                np.array[float[N]]: joint velocities [rad/s]
         """
         if body_id in self.subscribers:
             dq = self.subscribers[body_id].get_joint_velocities(joint_ids)
@@ -303,9 +303,9 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
-            max_force (None, float, np.float[N]): maximum motor forces/torques
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            max_force (None, float, np.array[float[N]]): maximum motor forces/torques
         """
         super(BulletROS, self).set_joint_velocities(body_id, joint_ids, velocities, max_force)
         if body_id in self.publishers:
@@ -320,13 +320,13 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): a joint id, or list of joint ids.
+            joint_ids (int, list[int]): a joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: torque [Nm]
             if multiple joints:
-                np.float[N]: torques associated to the given joints [Nm]
+                np.array[float[N]]: torques associated to the given joints [Nm]
         """
         if body_id in self.subscribers:
             torques = self.subscribers[body_id].get_joint_torques(joint_ids)
@@ -347,8 +347,8 @@ class BulletROS(Bullet):  # , ROS):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
         """
         super(BulletROS, self).set_joint_torques(body_id, joint_ids, torques)
         if body_id in self.publishers:

pyrobolearn/simulators/middlewares/__init__.py

@@ -0,0 +1,14 @@
+
+# import middleware abstract class
+from .middleware import MiddleWare
+
+# import ROS
+from .ros import ROS
+
+
+# # define decorator
+# def middleware(function):
+#     def wrapper(self, *args, **kwargs):
+#         if self.middleware is None:
+#             return function(*args, **kwargs)
+#     return wrapper

pyrobolearn/simulators/middlewares/middleware.py

@@ -46,6 +46,10 @@ class MiddleWare(object):
         self.publish = publish
         self.teleoperate = teleoperate
 
+    ##############
+    # Properties #
+    ##############
+
     @property
     def subscribe(self):
         return self._subscribe
@@ -69,3 +73,178 @@ class MiddleWare(object):
     @teleoperate.setter
     def teleoperate(self, teleoperate):
         self._teleoperate = bool(teleoperate)
+
+    ###########
+    # Methods #
+    ###########
+
+    def has_sensor(self, body_id, name):
+        """
+        Check if the specified robot has the given sensor.
+
+        Args:
+            body_id (int): body unique id.
+            name (str): name of the sensor.
+
+        Returns:
+            bool: True if the specified robot has the given sensor.
+        """
+        pass
+
+    def get_sensor_values(self, body_id, name):
+        """
+        Return the sensor.
+
+        Args:
+            body_id (int): body unique id.
+            name (str): name of the sensor.
+
+        Returns:
+            np.array, dict, list, None: sensor values. None if it didn't have anything.
+        """
+        pass
+
+    def get_joint_positions(self, body_id, joint_ids):
+        """
+        Get the position of the given joint(s).
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint position [rad]
+            if multiple joints:
+                np.array[float[N]]: joint positions [rad]
+        """
+        pass
+
+    def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None,
+                            check_teleoperate=False):
+        """
+        Set the position of the given joint(s) (using position control).
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
+            velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            kps (None, float, np.array[float[N]]): position gain(s)
+            kds (None, float, np.array[float[N]]): velocity gain(s)
+            forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint positions. If the `teleoperate` argument has been set to False, it
+              won't set the joint positions.
+        """
+        pass
+
+    def get_joint_velocities(self, body_id, joint_ids):
+        """
+        Get the velocity of the given joint(s).
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint velocity [rad/s]
+            if multiple joints:
+                np.array[float[N]]: joint velocities [rad/s]
+        """
+        pass
+
+    def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None, check_teleoperate=False):
+        """
+        Set the velocity of the given joint(s) (using velocity control).
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint velocities. If the `teleoperate` argument has been set to False, it
+              won't set the joint velocities.
+        """
+        pass
+
+    def get_joint_torques(self, body_id, joint_ids):
+        """
+        Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
+        Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
+        applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1]
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): a joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: torque [Nm]
+            if multiple joints:
+                np.array[float[N]]: torques associated to the given joints [Nm]
+        """
+        pass
+
+    def set_joint_torques(self, body_id, joint_ids, torques, check_teleoperate=False):
+        """
+        Set the torque/force to the given joint(s) (using force/torque control).
+
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint torques. If the `teleoperate` argument has been set to False, it won't
+              set the joint torques.
+        """
+        pass
+
+    def get_jacobian(self, body_id, link_id, local_position=None, q=None):
+        r"""
+        Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that:
+
+        .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q}
+
+        where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity.
+
+        Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root
+            link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints.
+
+        Args:
+            body_id (int): unique body id.
+            link_id (int): link id.
+            local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local
+                coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
+            q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs.
+
+        Returns:
+            np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The
+                number of columns depends if the base is fixed or floating.
+        """
+        pass
+
+    def get_inertia_matrix(self, body_id, q):
+        r"""
+        Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint
+        space given by (see [1]):
+
+        .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q})
+
+        where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and
+        :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any
+        other forces acting on the system except the applied torques :math:`\tau`.
+
+        Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated
+            joints. If the base is fixed, it will return a [N,N] inertia matrix
+
+        Args:
+            body_id (int): body unique id.
+            q (np.array[float[N]]): joint positions of size N, where N is the total number of DoFs.
+
+        Returns:
+            np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix
+        """
+        pass

pyrobolearn/simulators/middlewares/ros.py

@@ -1,8 +1,35 @@
 #!/usr/bin/env python
 """Define the ROS middleware API.
 
+ROS (Robot Operating System) [1] is a robotics middleware which "provides libraries and tools to help software
+developers create robot applications. It provides hardware abstraction, device drivers, libraries, visualizers,
+message-passing, package management, and more."
+
+- rospy [2]: "rospy is a pure Python client library for ROS. The rospy client API enables Python programmers to
+  quickly interface with ROS Topics, Services, and Parameters."
+- ros_control [3]: "A set of packages that include controller interfaces, controller managers, transmissions and
+  hardware_interfaces."
+- robot_state_publisher [4]: "This package allows you to publish the state of a robot to tf. Once the state gets
+  published, it is available to all components in the system that also use tf. The package takes the joint angles of
+  the robot as input and publishes the 3D poses of the robot links, using a kinematic tree model of the robot. The
+  package can both be used as a library and as a ROS node."
+- joint_state_publisher [5]: "This package contains a tool for setting and publishing joint state values for a given
+  URDF."
+
+Note to compile ROS packages using `catkin_make`, you might have to specify the Python version used using:
+- catkin_make -DPYTHON_EXECUTABLE=path/to/bin/python3
+
 Dependencies in PRL:
 * `pyrobolearn.simulators.middlewares.middleware.MiddleWare`
+
+References:
+    - [1] ROS: http://www.ros.org/  and  http://wiki.ros.org
+    - [2] rospy: http://wiki.ros.org/rospy
+    - [3] ros_control: http://wiki.ros.org/ros_control
+        - ROS control an overview: https://roscon.ros.org/2014/wp-content/uploads/2014/07/ros_control_an_overview.pdf
+    - [4] robot_state_publisher: http://wiki.ros.org/robot_state_publisher
+    - [5] joint_state_publisher: http://wiki.ros.org/joint_state_publisher
+    - [6] roslaunch: http://wiki.ros.org/roslaunch/API%20Usage
 """
 
 # TODO
@@ -12,8 +39,21 @@ import psutil
 import signal
 import importlib
 import inspect
+import yaml
+
+import rospy
+import rostopic
+import controller_manager.controller_manager_interface as cm_interface
+import roslaunch
+import rosparam
+import std_msgs.msg as std_msg
+import sensor_msgs.msg as sensor_msg
+import gazebo_msgs.msg as gazebo_msg
+import geometry_msgs.msg as geometry_msg
 
 from pyrobolearn.simulators.middlewares.middleware import MiddleWare
+from pyrobolearn.simulators.middlewares.ros_publisher import Publisher, PublisherData, RobotPublisher
+from pyrobolearn.simulators.middlewares.ros_subscriber import Subscriber, SubscriberData, RobotSubscriber
 
 
 __author__ = "Brian Delhaisse"
@@ -26,6 +66,123 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+class RobotMiddleWare(object):
+    r"""Robot middleware interface.
+
+    """
+
+    def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, control_file=None):
+        """
+        Initialize the robot middleware interface.
+
+        Args:
+            robot_id (int): robot unique id.
+            urdf (str): path to the URDF file.
+            subscribe (bool): if True, it will subscribe to the topics associated to the loaded robot, 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 robot.
+            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`.
+            control_file (str): path to the YAML control file.
+        """
+        self.id = robot_id
+        self.urdf = urdf
+        self.control_file = control_file
+        self.subscribers = {}
+        self.publishers = {}
+
+        # set variables
+        self.subscribe = subscribe
+        self.publish = publish
+        self.teleoperate = teleoperate
+
+    def get_joint_positions(self, joint_ids):
+        """
+        Get the position of the given joint(s).
+
+        Args:
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint position [rad]
+            if multiple joints:
+                np.array[float[N]]: joint positions [rad]
+        """
+        q = self.subscribers[body_id].get_joint_positions(joint_ids)
+        if self.teleoperate and body_id in self.publishers:
+            self.publishers[body_id].set_joint_positions(joint_ids, q)
+            self.publishers[body_id].publish('joint_states')
+        return q
+
+    def set_joint_positions(self, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
+        """
+        Set the position of the given joint(s) (using position control).
+
+        Args:
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
+            velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            kps (None, float, np.array[float[N]]): position gain(s)
+            kds (None, float, np.array[float[N]]): velocity gain(s)
+            forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
+        """
+        pass
+
+    def get_joint_velocities(self, joint_ids):
+        """
+        Get the velocity of the given joint(s).
+
+        Args:
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint velocity [rad/s]
+            if multiple joints:
+                np.array[float[N]]: joint velocities [rad/s]
+        """
+        pass
+
+    def set_joint_velocities(self, joint_ids, velocities, max_force=None):
+        """
+        Set the velocity of the given joint(s) (using velocity control).
+
+        Args:
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
+        """
+        pass
+
+    def get_joint_torques(self, joint_ids):
+        """
+        Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
+        Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
+        applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1]
+
+        Args:
+            joint_ids (int, list[int]): a joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: torque [Nm]
+            if multiple joints:
+                np.array[float[N]]: torques associated to the given joints [Nm]
+        """
+        pass
+
+    def set_joint_torques(self, joint_ids, torques):
+        """
+        Set the torque/force to the given joint(s) (using force/torque control).
+
+        Args:
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
+        """
+        pass
+
+
 class ROS(MiddleWare):
     r"""ROS Interface middleware
 
@@ -60,29 +217,24 @@ class ROS(MiddleWare):
             self.roscore = subprocess.Popen(["roscore", "-p", str(master_uri)], env=self.env,
                                             preexec_fn=os.setsid)  # , shell=True)
 
-        # set variables
-        self.subscribe = subscribe
-        self.publish = publish
-        self.teleoperate = teleoperate
-
         # remember each publisher/subscriber
         self.subscribers = {}
         self.publishers = {}
         self.models = []
 
+        self._robots = {}  # {body_id: RobotMiddleware}
+
         self.count_id = -1
 
-    def close(self):
-        """
-        Close everything
-        """
-        # delete each subscribers
+        # roslaunch
+        self.launch = roslaunch.scriptapi.ROSLaunch()
+        self.launch.start()
 
-        # delete each publishers
+        self.processes = {}  # {Node: process}
 
-        # delete ROS
-        if self.roscore is not None:
-            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+    ##############
+    # Properties #
+    ##############
 
     @property
     def is_subscribing(self):
@@ -94,6 +246,376 @@ class ROS(MiddleWare):
         """Return True if we are publishing to topics."""
         return self.publish
 
+    ###########
+    # Methods #
+    ###########
+
+    ##############
+    # ROS launch #
+    ##############
+
+    @staticmethod
+    def create_ros_node(package, node_type, name=None, namespace='/', machine_name=None, args='', respawn=False,
+                        respawn_delay=0.0, remap_args=None, env_args=None, output=None, cwd=None, launch_prefix=None,
+                        required=False, filename='<unknown>'):
+        """
+        Create a ROS node; data structure for storing information about a desired node in the ROS system Corresponds
+        to the 'node' tag in the launch specification.
+
+        This basically wraps the `roslaunch.core.Node(...)`.
+
+        Args:
+            package (str): node package name.
+            node_type (str): node type.
+            name (str): node name.
+            namespace (str): namespace for node.
+            machine_name (str): name of machine to run node on.
+            args (str): argument string to pass to node executable.
+            respawn (bool): if True, respawn node if it dies.
+            respawn_delay (float): if respawn is True, respawn node after delay.
+            remap_args (list[tuple[str,str]]): list of [(from, to)] remapping arguments.
+            env_args (list[tuple[str,str]]): list of [(key, value)] of additional environment vars to set for node.
+            output (str): where to log output to, either Node, 'screen' or 'log'.
+            cwd (str): current working directory of node, either 'node', 'ROS_HOME'. Default: ROS_HOME.
+            launch_prefix (str): launch command/arguments to prepend to node executable arguments.
+            required (bool): node is required to stay running (launch fails if node dies).
+            filename (str): name of file Node was parsed from.
+
+        Raises:
+            ValueError: if parameters do not validate.
+
+        Returns:
+            roslaunch.core.Node: ROS node data structure.
+        """
+        return roslaunch.core.Node(package=package, node_type=node_type, name=name, namespace=namespace,
+                                   machine_name=machine_name, args=args, respawn=respawn, respawn_delay=respawn_delay,
+                                   remap_args=remap_args, env_args=env_args, output=output, cwd=cwd,
+                                   launch_prefix=launch_prefix, required=required, filename=filename)
+
+    def launch(self, ros_node):
+        """
+        Launch a ROS node.
+
+        Args:
+            ros_node (roslaunch.core.Node): ROS node.
+
+        Returns:
+            roslaunch.nodeprocess.LocalProcess: the process.
+        """
+        process = self.launch.launch(ros_node)
+        self.processes[ros_node] = process
+        return process
+
+    @staticmethod
+    def get_namespace():
+        """
+        Get the namespace.
+
+        Returns:
+            str: namespace.
+        """
+        return rospy.get_namespace()
+
+    ################################################
+    # ROS Topics/Services + Publishers/Subscribers #
+    ################################################
+
+    def add_subscriber(self, body_id, topic_name, freq, fct, attribute):
+        pass
+
+    def add_publisher(self, body_id, topic_name, freq, fct, attribute):
+        pass
+
+    def remap(self, topic1, topic2, freq):
+        pass
+
+    def remove_publisher(self, publisher_id):
+        pass
+
+    def remove_subscriber(self, subscriber_id):
+        pass
+
+    @staticmethod
+    def get_topics(namespace='/'):
+        """
+        Get the published topics.
+
+        Returns:
+            list[list[str, str]]: list of tuples where the first is the topic and the second element is the message
+              type that topic accepts.
+        """
+        return rospy.get_published_topics(namespace=namespace)
+
+    @staticmethod
+    def has_topic(name, namespace='/'):
+        """
+        Check if the specified topic has been advertised.
+
+        Warnings: this has a O(N) time complexity.
+
+        Args:
+            name (str): name of the topic.
+            namespace (str): namespace.
+
+        Returns:
+            bool: True if the topic has been advertised.
+        """
+        topics = rospy.get_published_topics(namespace=namespace)
+        for topic_name, dtype in topics:
+            if topic_name == name:  # or topic_name.split('/')[-1] == name:
+                return True
+        return False
+
+    @staticmethod
+    def get_topic_type(name):
+        """
+        Get the topic type name of the given topic name.
+
+        This is the same as typing the following in the terminal:
+        - `rostopic type /topic_name`
+
+        Args:
+            name (str): name of the topic.
+
+        Returns:
+            str: type of the topic
+        """
+        return rostopic.get_topic_type(name)[0]
+
+    @staticmethod
+    def get_topic_class(name):
+        """
+        Get the topic class type of the given topic name.
+
+        Args:
+            name (str): name of the topic.
+
+        Returns:
+            type: topic class type. This can later be instantiated.
+        """
+        return rostopic.get_topic_class(name)
+
+    ###########################
+    # ROS parameters (+ YAML) #
+    ###########################
+
+    @staticmethod
+    def load_parameter_file(filename, namespace=None):
+        """
+        Load a parameter YAML file.
+
+        Args:
+            filename (str): path to the YAML file.
+            namespace (str): default namespace.
+
+        Returns:
+            list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace]
+        """
+        return rosparam.load_file(filename, default_namespace=namespace)
+
+    @staticmethod
+    def load_parameter_string(parameters, namespace=None):
+        """
+        Load a control configuration YAML string.
+
+        Args:
+            parameters (str): string in the YAML format specifying the control parameters.
+            namespace (str): default namespace.
+
+        Returns:
+            list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace]
+        """
+        return rosparam.load_str(parameters, default_namespace=namespace)
+
+    @staticmethod
+    def set_parameter(name, value):
+        """
+        Set a parameter on the param server.
+
+        Args:
+            name (str): name of the parameter.
+            value (str, dict): parameter value. "If param_value is a dictionary it will be treated as a parameter
+              tree, where param_name is the namespace. For example::: {'x':1,'y':2,'sub':{'z':3}} will set
+              `name/x=1`, `name/y=2`, and `name/sub/z=3`. Furthermore, it will replace all existing parameters in
+              the `name` namespace with the parameters in `value`. You must set parameters individually if you wish
+              to perform a union update.
+        """
+        rospy.set_param(name, value)
+
+    @staticmethod
+    def get_parameter(name, default=None):
+        r"""
+        Get the parameter associated with the given name from the param server.
+
+        Args:
+            name (str): name of the parameter.
+            default (object): default value to return if the parameter is not found.
+
+        Returns:
+            str: parameter value
+        """
+        return rospy.get_param(name, default=default)
+
+    @staticmethod
+    def get_parameter_names():
+        """
+        Return the parameter names that have been loaded.
+
+        Returns:
+            list[str]: list of parameter names.
+        """
+        return rospy.get_param_names()
+
+    @staticmethod
+    def upload_parameters(values, namespace='/'):
+        """
+        Upload parameters to the Parameter Server.
+
+        Args:
+            values (dict): dictionary where keys are parameter names and values are parameter values.
+            namespace (str): namespace to load parameters.
+        """
+        rosparam.upload_params(ns=namespace, values=values)
+
+    @staticmethod
+    def load_config_file(filename, namespace=None):
+        """
+        Load a control configuration YAML file.
+
+        Args:
+            filename (str): path to the YAML file.
+            namespace (str): default namespace.
+
+        Returns:
+            list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace]
+        """
+        params = rosparam.load_file(filename, default_namespace=namespace)
+        params = params[0]
+        namespace = params[1] + 'rrbot/'
+        params = params[0]['rrbot']
+        for key, value in params.items():
+            rosparam.upload_params(ns=namespace + key, values=value)
+
+    @staticmethod
+    def load_config_string(parameters, namespace=None):
+        """
+        Load a control configuration YAML string.
+
+        Args:
+            parameters (str): string in the YAML format specifying the control parameters.
+            namespace (str): default namespace.
+
+        Returns:
+            list[list[str,str]]:
+        """
+        params = rosparam.load_str(parameters, default_namespace=namespace)
+
+    ###############
+    # ROS CONTROL #
+    ###############
+
+    def get_pid(self, body_id):
+        pass
+
+    def set_pid(self, body_id, p=None, i=None, d=None):
+        pass
+
+    @staticmethod
+    def load_controller(name):
+        """
+        Load controller based on the given name.
+
+        Args:
+            name (str): name of the controller to load.
+
+        Returns:
+            bool: True if we could load the controller.
+        """
+        cm_interface.load_controller(name)
+
+    @staticmethod
+    def unload_controller(name):
+        """
+        Unload the controller based on the given name.
+
+        Args:
+            name (str): name of the controller to unload.
+
+        Returns:
+            bool: True if we could unload the controller.
+        """
+        cm_interface.unload_controller(name)
+
+    @staticmethod
+    def start_controller(name):
+        """
+        Start the controller based on the given name.
+
+        Args:
+            name (str): name of the controller to start.
+
+        Returns:
+            bool: True if we could start the controller.
+        """
+        cm_interface.start_controller(name)
+
+    @staticmethod
+    def stop_controller(name):
+        """
+        Stop the controller based on the given name.
+
+        Args:
+            name (str): name of the controller to stop.
+
+        Returns:
+            bool: True if we could stop the controller.
+        """
+        return cm_interface.stop_controller(name)
+
+    @staticmethod
+    def get_controllers():
+        """
+        Return the list of controllers.
+        """
+        rospy.wait_for_service('controller_manager/list_controllers')
+        s = rospy.ServiceProxy('controller_manager/list_controllers', cm_interface.ListControllers)
+        resp = s.call(cm_interface.ListControllersRequest())
+        return [(c.name, list(set(r.hardware_interface for r in c.claimed_resources)), c.state)
+                for c in resp.controller]
+
+    @staticmethod
+    def get_controller_types():
+        """
+        Return the list of controller types.
+        """
+        rospy.wait_for_service('controller_manager/list_controller_types')
+        s = rospy.ServiceProxy('controller_manager/list_controller_types', cm_interface.ListControllerTypes)
+        resp = s.call(cm_interface.ListControllerTypesRequest())
+        return [t for t in resp.types]
+
+    ##############
+    # Middleware #
+    ##############
+
+    def close(self):
+        """
+        Close everything
+        """
+        # delete each subscribers
+
+        # delete each publishers
+
+        for process in self.processes.values():
+            process.stop()
+
+        # stop launch
+        if self.launch:
+            self.launch.stop()
+
+        # delete ROS
+        if self.roscore is not None:
+            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+
     def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None,
                   use_fixed_base=None, flags=None, scale=None):
         """Load the given URDF file.
@@ -106,9 +628,10 @@ class ROS(MiddleWare):
 
         Args:
             filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
-            position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z]
-            orientation (quat): create the base of the object at the specified orientation as world space quaternion
-                [x,y,z,w]
+            position (np.array[float[3]]): create the base of the object at the specified position in world space
+              coordinates [x,y,z]
+            orientation (np.array[float[4]]): create the base of the object at the specified orientation as world
+              space quaternion [x,y,z,w]
             use_maximal_coordinates (int): Experimental. By default, the joints in the URDF file are created using the
                 reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm
                 (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid
@@ -171,6 +694,27 @@ class ROS(MiddleWare):
             if self.publish:
                 check_ros('publisher', self.publishers, id_)
 
+
+        # get path to the URDF folder
+        path = os.path.abspath(filename)  # /path/to/pyrobolearn/robots/urdfs/<robot>/robot.urdf
+        dirname = str(os.path.dirname(path))   # /path/to/pyrobolearn/robots/urdfs/<robot>/
+        basename = str(os.path.basename(path).split('.')[-2])  # robot name without extension
+        config_file = dirname + basename + ".yaml"
+
+        # check for YAML control configuration file
+        if os.path.isfile(config_file):
+            # if it exists, import it
+            data = yaml.load(open(config_file, 'r'), Loader=yaml.FullLoader)
+        else:
+            data = {basename: {'joint_state_controller': {'publish_rate': 50,
+                                                          'type': 'joint_state_controller/JointStateController'}}}
+
+        node = roslaunch.core.Node(package="controller_manager", name="controller_spawner", node_type="spawner",
+                                   respawn="false", output="screen", namespace="/rrbot", args=' '.join(
+                ["joint_state_controller", "joint1_position_controller", "joint2_position_controller"]))
+
+        # create subscribers
+
         return id_
 
     def get_joint_positions(self, body_id, joint_ids):
@@ -179,33 +723,41 @@ class ROS(MiddleWare):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: joint position [rad]
             if multiple joints:
-                np.float[N]: joint positions [rad]
+                np.array[float[N]]: joint positions [rad]
         """
-        if body_id in self.subscribers:
-            return self.subscribers[body_id].get_joint_positions(joint_ids)
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.get_joint_positions(joint_ids)
 
-    def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
+    def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None,
+                            check_teleoperate=False):
         """
         Set the position of the given joint(s) (using position control).
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            positions (float, np.float[N]): desired position, or list of desired positions [rad]
-            velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
-            kps (None, float, np.float[N]): position gain(s)
-            kds (None, float, np.float[N]): velocity gain(s)
-            forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
+            velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            kps (None, float, np.array[float[N]]): position gain(s)
+            kds (None, float, np.array[float[N]]): velocity gain(s)
+            forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint positions. If the `teleoperate` argument has been set to False, it
+              won't set the joint positions.
         """
-        if body_id in self.publishers:
-            self.publishers[body_id].set_joint_positions(joint_ids, positions)
-            self.publishers[body_id].publish('joint_states')
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            if check_teleoperate and not self.teleoperate:
+                return None
+            return robot.set_joint_positions(joint_ids, positions, velocities=velocities, kps=kps, kds=kds,
+                                             forces=forces)
 
     def get_joint_velocities(self, body_id, joint_ids):
         """
@@ -213,30 +765,36 @@ class ROS(MiddleWare):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: joint velocity [rad/s]
             if multiple joints:
-                np.float[N]: joint velocities [rad/s]
+                np.array[float[N]]: joint velocities [rad/s]
         """
-        if body_id in self.subscribers:
-            return self.subscribers[body_id].get_joint_velocities(joint_ids)
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.get_joint_velocities(joint_ids)
 
-    def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
+    def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None, check_teleoperate=False):
         """
         Set the velocity of the given joint(s) (using velocity control).
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
-            max_force (None, float, np.float[N]): maximum motor forces/torques
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint velocities. If the `teleoperate` argument has been set to False, it
+              won't set the joint velocities.
         """
-        if body_id in self.publishers:
-            self.publishers[body_id].set_joint_velocities(joint_ids, velocities)
-            self.publishers[body_id].publish('joint_states')
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            if check_teleoperate and not self.teleoperate:
+                return None
+            return robot.set_joint_velocities(joint_ids, velocities, max_force=max_force)
 
     def get_joint_torques(self, body_id, joint_ids):
         """
@@ -246,26 +804,128 @@ class ROS(MiddleWare):
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): a joint id, or list of joint ids.
+            joint_ids (int, list[int]): a joint id, or list of joint ids.
 
         Returns:
             if 1 joint:
                 float: torque [Nm]
             if multiple joints:
-                np.float[N]: torques associated to the given joints [Nm]
+                np.array[float[N]]: torques associated to the given joints [Nm]
         """
-        if body_id in self.subscribers:
-            return self.subscribers[body_id].get_joint_torques(joint_ids)
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.get_joint_torques(joint_ids)
 
-    def set_joint_torques(self, body_id, joint_ids, torques):
+    def set_joint_torques(self, body_id, joint_ids, torques, check_teleoperate=False):
         """
         Set the torque/force to the given joint(s) (using force/torque control).
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list of int): joint id, or list of joint ids.
-            torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
+            check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True,
+              and if so, it will set the joint torques. If the `teleoperate` argument has been set to False, it won't
+              set the joint torques.
         """
-        if body_id in self.publishers:
-            self.publishers[body_id].set_joint_torques(joint_ids, torques)
-            self.publishers[body_id].publish('joint_states')
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            if check_teleoperate and not self.teleoperate:
+                return None
+            return robot.set_joint_torques(joint_ids, torques)
+
+    def get_jacobian(self, body_id, link_id, local_position=None, q=None):
+        r"""
+        Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that:
+
+        .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q}
+
+        where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity.
+
+        Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root
+            link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints.
+
+        Args:
+            body_id (int): unique body id.
+            link_id (int): link id.
+            local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local
+                coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
+            q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs.
+
+        Returns:
+            np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The
+                number of columns depends if the base is fixed or floating.
+        """
+        pass
+
+    def get_inertia_matrix(self, body_id, q):
+        r"""
+        Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint
+        space given by (see [1]):
+
+        .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q})
+
+        where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and
+        :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any
+        other forces acting on the system except the applied torques :math:`\tau`.
+
+        Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated
+            joints. If the base is fixed, it will return a [N,N] inertia matrix
+
+        Args:
+            body_id (int): body unique id.
+            q (np.array[float[N]]): joint positions of size N, where N is the total number of DoFs.
+
+        Returns:
+            np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix
+        """
+        pass
+
+    def has_sensor(self, body_id, name):
+        """
+        Check if the specified robot has the given sensor.
+
+        Args:
+            body_id (int): body unique id.
+            name (str): name of the sensor.
+
+        Returns:
+            bool: True if the specified robot has the given sensor.
+        """
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.has_sensor(name)
+        return False
+
+    def get_sensor_values(self, body_id, name):
+        """
+        Return the sensor.
+
+        Args:
+            body_id (int): body unique id.
+            name (str): name of the sensor.
+
+        Returns:
+            np.array, dict, list, None: sensor values. None if it didn't have anything.
+        """
+        robot = self._robots.get(body_id)
+        if robot is not None:
+            return robot.get_sensor_values(name)
+
+    def can_teleoperate(self, body_id):
+        robot = self._robots.get(body_id, None)
+        if robot is None:
+            return False
+        return robot.teleoperate
+
+    def can_subscribe(self, body_id):
+        robot = self._robots.get(body_id, None)
+        if robot is None:
+            return False
+        return robot.subscribe
+
+    def can_publish(self, body_id):
+        robot = self._robots.get(body_id, None)
+        if robot is None:
+            return False
+        return robot.publish

pyrobolearn/simulators/middlewares/ros_publisher.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python
+"""Define the abstract robot publisher.
+"""
+
+import rospy
+
+# import the messages
+from std_msgs import msg as std_msg
+from sensor_msgs import msg as sensor_msg
+from geometry_msgs import msg as geometry_msg
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class PublisherData(object):
+    r"""Publisher data holder
+    """
+
+    def __init__(self, topic, data_class, queue_size=10):
+        self.__dict__['publisher'] = rospy.Publisher(topic, data_class, queue_size=queue_size)
+        self.__dict__['attributes'] = [attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
+                                       if not callable(getattr(data_class, attr))]
+        self.__dict__['publisher_data'] = data_class()
+
+    def publish(self, data=None):
+        if data is None:
+            self.publisher.publish(self.publisher_data)
+        else:
+            self.publisher.publish(data)
+
+    def __setattr__(self, key, value):
+        if key in self.attributes:
+            setattr(self.publisher_data, key, value)
+
+    def __getattr__(self, key):
+        return getattr(self.publisher_data, key)
+
+
+class Publisher(object):
+    r"""Publisher class
+
+    This Publisher abstract class is the class from which all the other publishers inherit from. It provides the
+    common functionalities between the various publishers.
+    """
+
+    def __init__(self, publisher_id=None):
+        """
+        Initialize the publisher.
+
+        Args:
+            publisher_id (int, None): publisher id which is used when initializing the node. If None, a name will be
+                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+                parameter in `rospy.init_node`.
+        """
+
+        # initialize the node
+        if publisher_id is None:
+            rospy.init_node(self.__class__.__name__, anonymous=True)
+        else:
+            rospy.init_node(self.__class__.__name__ + str(publisher_id))
+
+        # all publishers
+        self.publishers = dict()
+
+    def create_publisher(self, name, topic, data_class):
+        """
+        Create a publisher to the specific topic.
+
+        Args:
+            name (str): unique name of the publisher. The name must be unique. You will be able to access to this
+            topic (str): name of the topic.
+            data_class (object): data type class to use for messages
+
+        Returns:
+            PublisherData: the publisher data holder.
+        """
+        publisher = PublisherData(topic, data_class)
+        self.publishers[name] = publisher
+        setattr(self, name, publisher)
+        return publisher
+
+    def publish(self, name=None, data=None):
+        if name is None and data is None:
+            for publisher in self.publishers.values():
+                publisher.publish()
+        elif name is not None:
+            self.__dict__[name].publish(data)
+
+    # def __getattr__(self, name):
+    #     return self.publishers[name]
+
+
+class RobotPublisher(Publisher):
+    r"""Robot Publisher class
+
+    This Robot Publisher class is the class from which all the robot publishers inherit from.
+    """
+
+    def __init__(self, name, id_=None):
+        r"""
+        Initialize the robot publisher.
+
+        Args:
+            name (str): name of the robot. This will be used to create the topics.
+            id_ (int, None): robot id which is used when initializing the node. If None, a name will be
+                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+                parameter in `rospy.init_node`.
+        """
+        super(RobotPublisher, self).__init__(publisher_id=id_)
+        self.name = name.lower()
+
+        # create Joint states
+        # self.create_publisher('joint_states', self.name + '/joint_states', sensor_msg.JointState)
+        self.joint_states = PublisherData(self.name + '/joint_states', sensor_msg.JointState)
+        self.publishers['joint_states'] = self.joint_states
+
+    def set_joint_positions(self, joint_ids, positions):
+        # self.joint_states.position[joint_ids] = positions
+        self.joint_states.position = positions
+
+    def set_joint_velocities(self, joint_ids, velocities):
+        self.joint_states.velocity[joint_ids] = velocities
+
+    def set_joint_torques(self, joint_ids, torques):
+        self.joint_states.effort[joint_ids] = torques
+
+
+# Tests
+if __name__ == '__main__':
+    # NOTE: run roscore before hand
+    import numpy as np
+    from itertools import count
+    import time
+
+    publisher = RobotPublisher('walter')
+    print("Published topics: {}".format(rospy.get_published_topics()))
+    print("Robot joint state attributes: {}".format(publisher.joint_states.attributes))
+
+    publisher.joint_states.position = np.array(range(3))
+
+    for t in count():
+        print(t)
+        publisher.publish()
+        time.sleep(0.1)

pyrobolearn/simulators/middlewares/ros_subscriber.py

@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+"""Define the abstract robot subscriber.
+"""
+
+import numpy as np
+import rospy
+
+# import the messages
+# from std_msgs import msg as std_msg
+from sensor_msgs import msg as sensor_msg
+# from geometry_msgs import msg as geometry_msg
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class SubscriberData(object):
+    r"""Subscriber data holder
+    """
+
+    def __init__(self, topic, data_class):
+        self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback)
+        self.attributes = set([attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
+                               if not callable(getattr(data_class, attr))])
+        self.subscriber_data = data_class()
+
+    def callback(self, data):
+        self.subscriber_data = data
+
+    def __getattr__(self, name):
+        return getattr(self.subscriber_data, name)
+
+    def unregister(self):
+        self.subscriber.unregister()
+
+
+class Subscriber(object):
+    r"""Subscriber class
+
+    This Subscriber abstract class is the class from which all the other subscribers inherit from. It provides the
+    common functionalities between the various subscribers.
+    """
+
+    def __init__(self, subscriber_id=None):
+        """
+        Initialize the subscriber.
+
+        Args:
+            subscriber_id (int, None): subscriber id which is used when initializing the node. If None, a name will be
+                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+                parameter in `rospy.init_node`.
+        """
+
+        # initialize the node
+        if subscriber_id is None:
+            rospy.init_node(self.__class__.__name__, anonymous=True)
+        else:
+            rospy.init_node(self.__class__.__name__ + str(subscriber_id))
+
+        # all subscribers
+        self.subscribers = dict()
+
+    def create_subscriber(self, name, topic, data_class):
+        """
+        Create a subscriber to the specific topic.
+
+        Args:
+            name (str): unique name of the subscriber. The name must be unique. You will be able to access to this
+            topic (str): name of the topic.
+            data_class (object): data type class to use for messages
+
+        Returns:
+            SubscriberData: the subscriber data holder.
+        """
+        subscriber = SubscriberData(topic, data_class)
+        self.subscribers[name] = subscriber
+        return subscriber
+
+    def __getattr__(self, name):
+        return self.subscribers[name]
+
+    def unregister(self, name=None):
+        if name is None:
+            for subscriber in self.subscribers.values():
+                subscriber.unregister()
+        else:
+            self.subscribers[name].unregister()
+
+    def close(self):
+        self.unregister()
+
+    def __del__(self):
+        self.unregister()
+
+
+class RobotSubscriber(Subscriber):
+    r"""Robot Subscriber class
+
+    This Robot Subscriber class is the class from which all the robot subscribers inherit from.
+    """
+
+    def __init__(self, name, id_=None):
+        r"""
+        Initialize the robot subscriber.
+
+        Args:
+            name (str): name of the robot. This will be used to create the topics.
+            id_ (int, None): robot id which is used when initializing the node. If None, a name will be
+                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+                parameter in `rospy.init_node`.
+        """
+        super(RobotSubscriber, self).__init__(subscriber_id=id_)
+        self.name = name.lower()
+
+        # create Joint states
+        self.create_subscriber('joint_states', self.name + '/joint_states', sensor_msg.JointState)
+
+    def get_joint_positions(self, joint_ids):
+        if len(self.joint_states.position) >= len(joint_ids):
+            return np.asarray(self.joint_states.position)  # [joint_ids]
+        return np.asarray(self.joint_states.position)
+
+    def get_joint_velocities(self, joint_ids):
+        if len(self.joint_states.velocity) >= len(joint_ids):
+            return np.asarray(self.joint_states.velocity)[joint_ids]
+        return np.asarray(self.joint_states.velocity)
+
+    def get_joint_torques(self, joint_ids):
+        if len(self.joint_states.effort) >= len(joint_ids):
+            return np.asarray(self.joint_states.effort)[joint_ids]
+        return np.asarray(self.joint_states.effort)
+
+
+# Tests
+if __name__ == '__main__':
+    # NOTE: run roscore before hand and don't forget to run the publisher code
+    import time
+    from itertools import count
+
+    subscriber = RobotSubscriber('walter')
+    print("Published topics: {}".format(rospy.get_published_topics()))
+    print("Robot joint state attributes: {}".format(subscriber.joint_states.attributes))
+
+    for t in count():
+        print(t)
+        print("Joint position data: {}".format(subscriber.joint_states.position))
+        time.sleep(0.1)

pyrobolearn/simulators/simulator.py

@@ -18,6 +18,7 @@ References:
 """
 
 from pyrobolearn.utils.data_structures.orderedset import OrderedSet
+from pyrobolearn.simulators.middlewares.middleware import MiddleWare
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -199,7 +200,7 @@ class Simulator(object):
         self.real_time = False
         self.kwargs = kwargs
         self._num_instances = num_instances
-        self._middleware = middleware
+        self.middleware = middleware
 
         # main camera in the simulator
         self._camera = None
@@ -246,6 +247,19 @@ class Simulator(object):
         """Return the simulator time step."""
         return self.get_time_step()
 
+    @property
+    def middleware(self):
+        """Return the middleware."""
+        return self._middleware
+
+    @middleware.setter
+    def middleware(self, middleware):
+        """Set the middleware."""
+        if middleware is not None and not isinstance(middleware, MiddleWare):
+            raise TypeError("Expecting the given 'middleware' to be an instance of `MiddleWare`, but got instead: "
+                            "{}".format(middleware))
+        self._middleware = middleware
+
     #############
     # Operators #
     #############
@@ -1695,6 +1709,26 @@ class Simulator(object):
         """
         Set the position of the given joint(s) (using position control).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
+            velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            kps (None, float, np.array[float[N]]): position gain(s)
+            kds (None, float, np.array[float[N]]): velocity gain(s)
+            forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
+        """
+        # set joint positions in the simulator
+        self._set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
+
+        # publish the joint positions through the middleware
+        if self.middleware is not None:
+            self.middleware.set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
+
+    def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
+        """
+        Set the position of the given joint(s) (using position control).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
@@ -1710,6 +1744,39 @@ class Simulator(object):
         """
         Get the position of the given joint(s).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint position [rad]
+            if multiple joints:
+                np.array[float[N]]: joint positions [rad]
+        """
+        # if a middleware is defined
+        if self.middleware is not None:
+            # get joint positions from the middleware
+            q = self.middleware.get_joint_positions(body_id, joint_ids)
+            if q is None:  # if we didn't get the joint positions from the middleware, get them from the simulator
+                q = self._get_joint_positions(body_id, joint_ids)
+            else:  # if we got them from the middleware, set them in the simulator
+                self._set_joint_positions(body_id=body_id, joint_ids=joint_ids, positions=q)
+
+        else:
+            # get the joint positions from the simulator
+            q = self._get_joint_positions(body_id, joint_ids)
+
+            # if the middleware is set on the teleoperation mode, publish the joint positions through the middleware
+            if self.middleware is not None:
+                self.middleware.set_joint_positions(body_id, joint_ids, q, check_teleoperate=True)
+
+        return q
+
+    def _get_joint_positions(self, body_id, joint_ids):
+        """
+        Get the position of the given joint(s).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
@@ -1726,6 +1793,23 @@ class Simulator(object):
         """
         Set the velocity of the given joint(s) (using velocity control).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
+            max_force (None, float, np.array[float[N]]): maximum motor forces/torques
+        """
+        # set joint velocities in the simulator
+        self._set_joint_velocities(body_id, joint_ids, velocities, max_force)
+
+        # publish the joint velocities through the middleware
+        if self.middleware is not None:
+            self.middleware.set_joint_velocities(body_id, joint_ids, velocities, max_force)
+
+    def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
+        """
+        Set the velocity of the given joint(s) (using velocity control).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
@@ -1738,6 +1822,39 @@ class Simulator(object):
         """
         Get the velocity of the given joint(s).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: joint velocity [rad/s]
+            if multiple joints:
+                np.array[float[N]]: joint velocities [rad/s]
+        """
+        # if a middleware is defined
+        if self.middleware is not None:
+            # get joint velocities from the middleware
+            dq = self.middleware.get_joint_velocities(body_id, joint_ids)
+            if dq is None:  # if we didn't get the joint velocities from the middleware, get them from the simulator
+                dq = self._get_joint_velocities(body_id, joint_ids)
+            else:  # if we got them from the middleware, set them in the simulator
+                self._set_joint_velocities(body_id=body_id, joint_ids=joint_ids, velocities=dq)
+
+        else:
+            # get the joint velocities from the simulator
+            dq = self._get_joint_velocities(body_id, joint_ids)
+
+            # if the middleware is set on the teleoperation mode, publish the joint velocities through the middleware
+            if self.middleware is not None:
+                self.middleware.set_joint_velocities(body_id, joint_ids, dq, check_teleoperate=True)
+
+        return dq
+
+    def _get_joint_velocities(self, body_id, joint_ids):
+        """
+        Get the velocity of the given joint(s).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
@@ -1783,6 +1900,22 @@ class Simulator(object):
         """
         Set the torque/force to the given joint(s) (using force/torque control).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): joint id, or list of joint ids.
+            torques (float, list[float], np.array[float]): desired torque(s) to apply to the joint(s) [N].
+        """
+        # set joint torques in the simulator
+        self._set_joint_torques(body_id, joint_ids, torques)
+
+        # publish the joint torques through the middleware
+        if self.middleware is not None:
+            self.middleware.set_joint_torques(body_id, joint_ids, torques)
+
+    def _set_joint_torques(self, body_id, joint_ids, torques):
+        """
+        Set the torque/force to the given joint(s) (using force/torque control).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
@@ -1794,6 +1927,39 @@ class Simulator(object):
         """
         Get the applied torque(s) on the given joint(s).
 
+        Args:
+            body_id (int): unique body id.
+            joint_ids (int, list[int]): a joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                float: torque [Nm]
+            if multiple joints:
+                np.array[float[N]]: torques associated to the given joints [Nm]
+        """
+        # if a middleware is defined
+        if self.middleware is not None:
+            # get joint torques from the middleware
+            tau = self.middleware.get_joint_torques(body_id, joint_ids)
+            if tau is None:  # if we didn't get the joint torques from the middleware, get them from the simulator
+                tau = self._get_joint_torques(body_id, joint_ids)
+            else:  # if we got them from the middleware, set them in the simulator
+                self._set_joint_torques(body_id=body_id, joint_ids=joint_ids, torques=tau)
+
+        else:
+            # get the joint velocities from the simulator
+            tau = self._get_joint_torques(body_id, joint_ids)
+
+            # if the middleware is set on the teleoperation mode, publish the joint torques through the middleware
+            if self.middleware is not None:
+                self.middleware.set_joint_torques(body_id, joint_ids, tau, check_teleoperate=True)
+
+        return tau
+
+    def _get_joint_torques(self, body_id, joint_ids):
+        """
+        Get the applied torque(s) on the given joint(s).
+
         Args:
             body_id (int): unique body id.
             joint_ids (int, list[int]): a joint id, or list of joint ids.

pyrobolearn/utils/mesh.py

@@ -39,7 +39,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-def convert_mesh(from_filename, to_filename, library='pyassimp'):
+def convert_mesh(from_filename, to_filename, library='pyassimp', binary=False):
     """
     Convert the given file containing the original mesh to the other specified format using the `pyassimp` library.
 
@@ -47,11 +47,16 @@ def convert_mesh(from_filename, to_filename, library='pyassimp'):
         from_filename (str): filename of the mesh to convert.
         to_filename (str): filename of the converted mesh.
         library (str): library to use to convert the meshes. Select between 'pyassimp' and 'trimesh'.
+        binary (bool): if True, it will be in a binary format. This is only valid for some formats such as STL where
+          you have the ASCII version 'stl' and the binary version 'stlb'.
     """
     if library == 'pyassimp':
         scene = pyassimp.load(from_filename)
-        extension = to_filename.split('.')[-1]
-        pyassimp.export(scene, to_filename, file_type=extension)
+        extension = to_filename.split('.')[-1].lower()
+        if binary:  # for binary add 'b' as a suffix. Ex: '<file>.stlb'
+            pyassimp.export(scene, to_filename, file_type=extension + 'b')
+        else:
+            pyassimp.export(scene, to_filename, file_type=extension)
         pyassimp.release(scene)
     elif library == 'trimesh':
         export_mesh(trimesh.load(from_filename), to_filename)

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -190,7 +190,7 @@ class Frame(object):
               using the right hand.
         """
         # forward_axis=(1., 0., 0.), up_axis=(0., 0., 1.)):
-
+        self.name = None
         self.position = position
         self.orientation = orientation
         self.dtype = dtype  # world frame, body frame, joint frame, inertial frame, etc.
@@ -707,7 +707,7 @@ class Floor(object):
         """
         self.name = name
         self.dimensions = dimensions
-        self.frame = Frame(position, orientation, dtype='world')
+        self.frame = Frame(position=position, orientation=orientation, dtype='world')
         if name is not None:
             name = name + '_material'
         self.material = Material(name=name, color=color, texture=texture)
@@ -867,7 +867,10 @@ class MultiBody(object):
         self.joints = OrderedDict()  # {name: Joint}
         self.root = root
         self.materials = {}
-        self.frame = Frame(position, orientation, dtype='world')
+        self.frame = Frame(position=position, orientation=orientation, dtype='world')
+
+        self.sensors = {}
+        self.actuators = {}
 
     @property
     def name(self):
@@ -1052,6 +1055,42 @@ class MultiBody(object):
                 # replace old joint dictionary
                 self.joints = joints
 
+    def has_sensors(self):
+        """
+        Return True if the multi-body data structure has some sensors.
+        """
+        return len(self.sensors) > 0
+
+    def has_actuators(self):
+        """
+        Return True if the multi-body data structure has some actuators.
+        """
+        return len(self.actuators) > 0
+
+    def add_sensor(self, sensor):
+        """
+        Add a sensor to the multi-body data structure.
+
+        Args:
+            sensor (Sensor): sensor data structure.
+        """
+        if not isinstance(sensor, Sensor):
+            raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, but got instead: "
+                            "{}".format(type(sensor)))
+        self.sensors[sensor.id] = sensor
+
+    def add_actuator(self, actuator):
+        """
+        Add an actuator to the multi-body data structure.
+
+        Args:
+            actuator (Actuator): actuator data structure.
+        """
+        if not isinstance(actuator, Actuator):
+            raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, but got instead: "
+                            "{}".format(type(actuator)))
+        self.actuators[actuator.id] = actuator
+
 
 # alias
 Tree = MultiBody
@@ -2831,20 +2870,39 @@ class Material(object):
         self._emissive = self._check_color(emissive)
 
 
+class Noise(object):
+    """Noise distribution used in sensors and actuators."""
+    pass
+
+
+class GaussianNoise(Noise):
+    """Gaussian noise distribution used in sensors and actuators."""
+
+    def __init__(self, mean, stddev):
+        self.mean = mean
+        self.stddev = stddev
+
+
 class Sensor(object):
     r"""Sensor (abstract) class.
 
     """
 
-    def __init__(self, name=None, sensors=[]):
+    def __init__(self, sensor_id, name=None, update_rate=None, noise=None, sensors=[]):
         """
         Initialize the sensor.
 
         Args:
+            sensor_id (int): sensor unique id.
             name (str): name of the sensor.
+            update_rate (float): update rate.
+            noise (Noise): noise that is applied on the sensor.
             sensors (list[Sensor]): inner list of sensors.
         """
+        self.id = sensor_id
         self.name = name
+        self.update_rate = update_rate
+        self.noise = noise
         self.sensors = sensors
 
     @property
@@ -2859,25 +2917,255 @@ class Sensor(object):
             raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name)))
         self._name = name
 
+    @property
+    def noise(self):
+        """Return the sensor noise."""
+        return self._noise
+
+    @noise.setter
+    def noise(self, noise):
+        """Set the sensor noise."""
+        if noise is not None and not isinstance(noise, Noise):
+            raise TypeError("Expecting the given 'noise' to be an instance of `Noise` but got instead: "
+                            "{}".format(type(noise)))
+        self._noise = noise
+
     @property
     def num_sensors(self):
         """Return the number of inner sensors."""
         return len(self.sensors)
 
 
-class Actuator(object):  # Motor
-    r"""Actuator/Motor (abstract) class.
+class JointSensor(Sensor):
+    """Joint sensor
 
+    Sensor attached to a joint.
     """
 
-    def __init__(self, name=None, actuators=[]):
+    def __init__(self, sensor_id, joint, name=None, update_rate=None, noise=None):
+        """
+        Initialize the joint sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            joint (Joint): joint to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(JointSensor, self).__init__(sensor_id, name, update_rate, noise)
+        self.joint = joint
+
+    @property
+    def joint(self):
+        """Return the joint data structure."""
+        return self._joint
+
+    @joint.setter
+    def joint(self, joint):
+        """Set the joint."""
+        if joint is not None and not isinstance(joint, Joint):
+            raise TypeError("Expecting the given 'joint' to be an instance of `Joint` but got instead: "
+                            "{}".format(type(joint)))
+        self._joint = joint
+
+
+class LinkSensor(Sensor):
+    """Link Sensor
+
+    Sensor attached to a link.
+    """
+
+    def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None):
+        """
+        Initialize the link sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            link (Body): link/body to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(LinkSensor, self).__init__(sensor_id, name, update_rate, noise)
+        self.link = link
+
+    @property
+    def link(self):
+        """Return the link data structure."""
+        return self._link
+
+    @link.setter
+    def link(self, link):
+        """Set the link."""
+        if link is not None and not isinstance(link, Body):
+            raise TypeError("Expecting the given 'link' to be an instance of `Body` but got instead: "
+                            "{}".format(type(link)))
+        self._link = link
+
+
+class CameraSensor(LinkSensor):
+    """Camera sensor
+
+    References:
+        - http://gazebosim.org/tutorials?tut=ros_gzplugins#Camera
+    """
+
+    def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None):
+        """
+        Initialize the camera sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            link (Body): link/body to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(CameraSensor, self).__init__(sensor_id, link, name, update_rate, noise)
+        self.frame = Frame()
+        self.visualize = False
+
+        # intrinsic properties of camera
+        self.horizontal_fov = None
+        self.width = None
+        self.height = None
+        self.format = None  # R8G8B8
+        self.near = None
+        self.far = None
+
+        self.plugin_filename = None
+        self.plugin_name = None
+        self.camera_base_topic = None  # camera_base_topic
+        self.image_topic = None  # added to the camera_base_topic
+        self.camera_info_topic = None  # added to the camera_base_topic
+        self.frame_name = None  # check 'name' attribute in Frame class
+        self.hack_baseline = None
+        self.distortion_k1 = None
+        self.distortion_k2 = None
+        self.distortion_k3 = None
+        self.distortion_t1 = None
+        self.distortion_t2 = None
+
+
+class DepthCameraSensor(LinkSensor):
+    """Depth camera sensor
+
+    References:
+        - http://gazebosim.org/tutorials?tut=ros_gzplugins#Camera
+    """
+
+    def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None):
+        """
+        Initialize the depth camera sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            link (Body): link/body to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(DepthCameraSensor, self).__init__(sensor_id, link, name, update_rate, noise)
+
+
+class GPURay(LinkSensor):
+    """GPU Ray sensor
+
+    References:
+        - http://gazebosim.org/tutorials?tut=ros_gzplugins#GPULaser
+    """
+
+    def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None):
+        """
+        Initialize the GPU ray sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            link (Body): link/body to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(GPURay, self).__init__(sensor_id, link, name, update_rate, noise)
+
+        self.frame = Frame()
+        self.visualize = False
+
+        # <scan>
+        self.horizontal = None
+        self.samples = None
+        self.scan_resolution = None
+        self.range_angle = None  # <min_angle> and <max_angle>
+
+        # <range>
+        self.range = None  # <min> and <max>
+        self.range_resolution = 0.01
+
+        # plugin
+        self.plugin_filename = None
+        self.plugin_name = None
+        self.topic = None
+        self.frame_name = None  # Check 'name' attribute in Frame class
+
+
+class IMUSensor(LinkSensor):
+    """IMU sensor.
+
+    References:
+        - http://gazebosim.org/tutorials?tut=ros_gzplugins#IMUsensor(GazeboRosImuSensor)
+    """
+
+    def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None):
+        """
+        Initialize the IMU sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            link (Body): link/body to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(IMUSensor, self).__init__(sensor_id, link, name, update_rate, noise)
+
+
+class ForceTorqueSensor(JointSensor):
+    """Force torque sensor.
+
+    References:
+        - http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors
+        - http://docs.ros.org/jade/api/gazebo_plugins/html/group__GazeboRosFTSensor.html
+    """
+
+    def __init__(self, sensor_id, joint, name=None, update_rate=None, noise=None):
+        """
+        Initialize the F/T sensor.
+
+        Args:
+            sensor_id (int): unique sensor id.
+            joint (Joint): joint to which the sensor is attached.
+            name (str): name of the sensor.
+            update_rate (float): update rate of the sensor.
+            noise (Noise): noise distribution that is used on the sensor.
+        """
+        super(ForceTorqueSensor, self).__init__(sensor_id, joint, name, update_rate, noise)
+
+
+class Actuator(object):  # Motor
+    r"""Actuator/Motor (abstract) class.
+    """
+
+    def __init__(self, actuator_id, name=None, actuators=[]):
         """
         Initialize the actuator/motor.
 
         Args:
+            actuator_id (int): actuator unique id.
             name (str): name of the actuator/motor.
             actuators (list[Sensor]): inner list of actuators.
         """
+        self.id = actuator_id
         self.name = name
         self.actuators = actuators
 
@@ -2899,6 +3187,73 @@ class Actuator(object):  # Motor
         return len(self.actuators)
 
 
+class JointActuator(Actuator):
+    """Joint Actuator."""
+
+    def __init__(self, actuator_id, joint, name=None):
+        """
+        Initialize the joint actuator.
+
+        Args:
+            actuator_id (int): unique actuator id.
+            joint (Joint): joint to which the actuator is attached.
+            name (str): name of the actuator.
+        """
+        super(JointActuator, self).__init__(actuator_id, name)
+        self.joint = joint
+
+    @property
+    def joint(self):
+        """Return the joint data structure."""
+        return self._joint
+
+    @joint.setter
+    def joint(self, joint):
+        """Set the joint."""
+        if joint is not None and not isinstance(joint, Joint):
+            raise TypeError("Expecting the given 'joint' to be an instance of `Joint` but got instead: "
+                            "{}".format(type(joint)))
+        self._joint = joint
+
+
+class MotorJointActuator(JointActuator):
+    """Motor joint actuator."""
+
+    def __init__(self, actuator_id, joint, name=None):
+        """
+        Initialize the joint actuator.
+
+        Args:
+            actuator_id (int): unique actuator id.
+            joint (Joint): joint to which the actuator is attached.
+            name (str): name of the actuator.
+        """
+        super(MotorJointActuator, self).__init__(actuator_id, joint, name)
+
+        self.transmission_type = None
+        self.hardware_interface = None  # EffortJointInterface
+        self.mechanical_reduction = None
+
+
+class PositionJointActuator(MotorJointActuator):
+    """Position joint actuator."""
+
+    def __init__(self, actuator_id, joint, name=None):
+        """
+        Initialize the position joint actuator.
+
+        Args:
+            actuator_id (int): unique actuator id.
+            joint (Joint): joint to which the actuator is attached.
+            name (str): name of the actuator.
+        """
+        super(MotorJointActuator, self).__init__(actuator_id, joint, name)
+
+        self.p = None
+        self.i = None
+        self.d = None
+
+
 class Heightmap(object):
     r"""Heightmap (abstract) class.
 
@@ -2912,3 +3267,46 @@ class Constraint(object):
     This allows to define a constraint.
     """
     pass
+
+
+class Transmission(object):
+    r"""Transmission interface.
+
+    The transmission interface is used in the control loop to "describe the relationship between an actuator and a
+    joint. This allows one to model concepts such as gear ratios and parallel linkages. A transmission transforms
+    efforts/flow variables such that their product - power - remains constant. Multiple actuators may be linked to
+    multiple joints through complex transmission." [3]
+
+    The control loop consists of 6 stages:
+    - read state from robotic hardware
+    - transmission: actuator to joint state
+    - controller manager update
+
+    Available transmission type:
+    - Simple reducer (type = transmission_interface/SimpleTransmission)
+    - Four-bar linkage
+    - Differential
+
+    References:
+        - [1] ROS Control: https://roscon.ros.org/2014/wp-content/uploads/2014/07/ros_control_an_overview.pdf
+        - [2] ros_control: http://wiki.ros.org/ros_control
+        - [3] URDF Transmissions: https://wiki.ros.org/urdf/XML/Transmission
+    """
+
+    def __init__(self, name, joint, transmission_type=None, actuator_name=None, hardware_interface=None):
+        """
+        Initialize the transmission.
+
+        Args:
+            name (str): name of the transmission.
+            joint (Joint): joint to which is attached the transmission.
+            transmission_type (str): transmission type; select between {'simple', 'four-bar linkage', 'differential'}.
+            actuator_name (str): name of the actuator.
+            hardware_interface (str): hardware interface.
+        """
+        self.name = name
+        self.type = transmission_type
+        self.actuator = actuator_name
+        self.mechanical_reduction = None
+        self.joint = joint
+        self.hardware_interface = hardware_interface

pyrobolearn/utils/parsers/robots/mujoco_parser.py

@@ -1286,12 +1286,13 @@ class MuJoCoParser(WorldParser):
             # if file does not already exists, convert it
             if not os.path.isfile(new_filename):
 
-                # # Arf, pyassimp export an ASCII STL, but Mujoco requires a binary STL --> use trimesh
-                # scene = pyassimp.load(filename)
-                # pyassimp.export(scene, new_filename, file_type='stl')
-                # pyassimp.release(scene)
+                # use pyassimp to
+                scene = pyassimp.load(filename)
+                pyassimp.export(scene, new_filename, file_type='stlb')
+                pyassimp.release(scene)
 
-                export_mesh(trimesh.load(filename), new_filename)
+                #
+                # export_mesh(trimesh.load(filename), new_filename)
 
             return new_filename
         return filename

pyrobolearn/utils/parsers/robots/robot_parser.py

@@ -143,5 +143,6 @@ class RobotParser(object):
             root (ET.Element): root element in the XML file.
         """
         xml_str = self.get_string(root)
+        print(xml_str)
         with open(filename, "w") as f:
             f.write(xml_str)  # .encode('utf-8'))

pyrobolearn/utils/parsers/robots/urdf_parser.py

@@ -308,10 +308,13 @@ class URDFParser(RobotParser):
             tree = self.tree
 
         # create root element
-        root = ET.Element('robot')
+        attrib = {}
+        if tree.name is not None:
+            attrib['name'] = tree.name
+        root = ET.Element('robot', attrib=attrib)
 
         # generate material tags
-        for material in tree.materials:
+        for material in tree.materials.values():
             material_tag = ET.SubElement(root, 'material', attrib={'name': material.name})
             if material.color is not None:
                 ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
@@ -352,12 +355,15 @@ class URDFParser(RobotParser):
                         attrib['length'] = str(geometry.size[1])
                     else:  # mesh
                         attrib['filename'] = geometry.filename
-                        attrib['scale'] = str(np.asarray(geometry.size))[1:-1]
+                        if geometry.size is not None:
+                            size = geometry.size
+                            size = np.array([size] * 3) if isinstance(size, (float, int)) else np.asarray(size)
+                            attrib['scale'] = str(size)[1:-1]
 
                     ET.SubElement(geometry_tag, dtype, attrib=attrib)
 
         # generate <link>
-        for link in tree.bodies:
+        for link in tree.bodies.values():
             link_tag = ET.SubElement(root, 'link', attrib={'name': link.name})
 
             # create <inertial> tag
@@ -434,7 +440,7 @@ class URDFParser(RobotParser):
             return ET.SubElement(parent_tag, tag, attrib=kwargs)
 
         # generate <joint>
-        for joint in tree.joints:
+        for joint in tree.joints.values():
             # set joint name and type
             joint_tag = set_name_and_type(root, 'joint', joint)
 
@@ -443,11 +449,11 @@ class URDFParser(RobotParser):
 
             # <parent>
             if joint.parent is not None:
-                ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent})
+                ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent.name})
 
             # <child>
             if joint.child is not None:
-                ET.SubElement(joint_tag, 'child', attrib={'link': joint.child})
+                ET.SubElement(joint_tag, 'child', attrib={'link': joint.child.name})
 
             # <axis>
             if joint.axis is not None: