fix bugs + update sensors/actuators

pyrobolearn/__init__.py

@@ -3,6 +3,7 @@
 
 name = "pyrobolearn"
 
+import os
 import sys
 import signal
 from itertools import count
@@ -121,6 +122,11 @@ def module_imported(module_name):  # TODO: improve this method
     return False
 
 
+world_mesh_path = os.path.dirname(os.path.abspath(__file__)) + '/worlds/meshes/'
+
+__all__ = [simulators, robots, worlds, physics, states, actions, terminal_conditions, rewards, envs, models,
+           approximators, policies, values, actorcritics, dynamics, tools]
+
 # Define what submodules/classes/functions should be loaded when writing 'from pyrobolearn import *'
 # __all__ = [
 #     # Submodules

pyrobolearn/robots/actuators/actuator.py

@@ -7,7 +7,7 @@ other joint actuators. Additionally, this is important as more realistic motors
 simulation to reality.
 """
 
-# TODO: add latency + noise
+from pyrobolearn.robots.noise.noise import Noise, NoNoise
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -26,20 +26,42 @@ class Actuator(object):
     Other actuators such as speakers, leds, and others are attached to links.
     """
 
-    def __init__(self, latency=0):
+    def __init__(self, noise=None, latency=0):
         """
         Initialize the actuator.
 
         Args:
-            latency (int, float): latency.
+            noise (None, Noise): noise to be added.
+            latency (int, float, None): latency time / step.
         """
 
         # variable to check if the actuator is enabled
         self._enabled = True
+
+        # set the latency
+        if not isinstance(latency, (int, float)):
+            raise TypeError("Expecting the given 'latency' to be an int or float, instead got: "
+                            "{}".format(type(latency)))
+        if latency < 0:
+            raise ValueError("Expecting the given 'latency' to be a positive number, but got instead: "
+                             "{}".format(latency))
         self._latency = latency
+        self._latent_cnt = -1
+
+        # set the noise
+        if noise is None:
+            noise = NoNoise()
+        if not isinstance(noise, Noise):
+            raise TypeError("Expecting the given 'noise' to be an instance of Noise, instead got: "
+                            "{}".format(type(noise)))
+        self._noise = noise
 
     #     self.sim = simulator
-    #
+
+    ##############
+    # Properties #
+    ##############
+
     # @property
     # def simulator(self):
     #     return self.sim
@@ -48,6 +70,16 @@ class Actuator(object):
     # def simulator(self, simulator):
     #     self.sim = simulator
 
+    @property
+    def enabled(self):
+        """Return if the sensor is enabled or not."""
+        return self._enabled
+
+    @property
+    def disabled(self):
+        """Return if the sensor is disabled or not."""
+        return not self._enabled
+
     ###########
     # Methods #
     ###########

pyrobolearn/robots/actuators/joints.py

@@ -4,6 +4,7 @@
 
 import copy
 import numpy as np
+from abc import ABCMeta
 
 from pyrobolearn.robots.actuators.actuator import Actuator
 
@@ -18,16 +19,24 @@ __status__ = "Development"
 
 
 class JointActuator(Actuator):
-    r"""Joint Actuators
+    r"""Joint Actuators (abstract)
 
     This defined the joint actuator class; this is an actuator which is attached to a joint and outputs the torque
     to be applied on it using a specific control scheme (e.g. PD control).
     For instance, given a target joint position value, the actuator computes the necessary torque to be applied on
     the joint using a simple PD control (with certain gains).
     """
+    __metaclass__ = ABCMeta
 
-    def __init__(self, joint_id):
-        super(JointActuator, self).__init__()
+    def __init__(self, joint_id, latency=None):
+        """
+        Initialize the joint actuator.
+
+        Args:
+            joint_id (int): joint unique id.
+            latency (int, float, None): latency time / step.
+        """
+        super(JointActuator, self).__init__(latency=latency)
         self.joint_id = joint_id
 
     def __copy__(self):
@@ -55,12 +64,22 @@ class PDJointActuator(JointActuator):
     """
 
     def __init__(self, joint_id, kp=0, kd=0, min_torque=-np.infty, max_torque=np.infty, latency=0):
-        super(PDJointActuator, self).__init__(joint_id)
+        """
+        Initialize the PD joint actuator.
+
+        Args:
+            joint_id (int): joint id.
+            kp (float): position gain
+            kd (float): velocity gain
+            min_torque (float): minimum torque
+            max_torque (float): maximum torque
+            latency (int, float, None): latency time / step.
+        """
+        super(PDJointActuator, self).__init__(joint_id, latency=latency)
         self.kp = kp
         self.kd = kd
         self.min_torque = min_torque
         self.max_torque = max_torque
-        self.latency = latency
 
     def compute(self, qd, q, dq):
         """

pyrobolearn/robots/noise/gaussian_noise.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+"""Define the Gaussian noise class.
+
+This noise can be applied notably on sensors and actuators.
+"""
+
+import numpy as np
+
+from pyrobolearn.robots.noise.noise import Noise
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class GaussianNoise(Noise):
+    r"""Gaussian noise class
+
+    This computes:
+
+    .. math:: y = x + \epsilon
+
+    where :math:`x` is the original data, :math:`y` is the noisy data, and
+    :math:`\epsilon \sim \mathcal{N}(\mu, \Sigma)` is the added Gaussian noise.
+    """
+
+    def __init__(self, vars=1):
+        """Initialize the noise.
+
+        Args:
+            vars (float, int, np.array): variances or covariance matrix.
+        """
+        super(GaussianNoise, self).__init__()
+        self._multivariate = True if isinstance(vars, np.ndarray) and vars.ndim == 2 else False
+        if self._multivariate:
+            mean = np.zeros(vars.shape[0])
+        else:
+            if not isinstance(vars, (int, float, np.ndarray)):
+                raise TypeError("Expecting the given 'vars' to be an int, float, or np.array, instead got: "
+                                "{}".format(type(vars)))
+            if isinstance(vars, np.ndarray) and vars.ndim > 1:
+                raise ValueError("Expecting the given 'vars' to be a np.array of dim 1 or 2, instead got: "
+                                 "{}".format(vars.ndim))
+            vars = np.sqrt(vars)  # abuse of name; this is no more the variance but the standard deviation
+            mean = 0.
+        self.vars = vars
+        self.mean = mean
+
+    def apply_noise(self, data, inplace=False):
+        """
+        Apply the noise on the given data.
+
+        Args:
+            data (int, float, np.array): data to apply the noise on.
+            inplace (bool): if True, it will directly modify the given data, and won't return a copy of it.
+
+        Returns:
+            int, float, np.array: noisy data.
+        """
+        # compute noise
+        if self._multivariate:
+            if isinstance(data, np.ndarray) and data.shape != self.vars.shape:
+                noise = np.random.multivariate_normal(mean=self.mean, cov=self.vars, size=data.shape)
+            else:
+                noise = np.random.multivariate_normal(mean=self.mean, cov=self.vars)
+        else:
+            if isinstance(data, np.ndarray):
+                noise = np.random.normal(loc=self.mean, scale=self.vars, size=data.shape)
+            else:
+                noise = np.random.normal(loc=self.mean, scale=self.vars)
+
+        # check if inplace operation
+        if not inplace:
+            return data + noise
+        data += noise
+        return data
+
+    def __str__(self):
+        """Return a string describing the class."""
+        return self.__class__.__name__ + "(mean=0, vars=" + str(self.vars) + ")"

pyrobolearn/robots/noise/noise.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+"""Define the abstract noise class from which all noises inherit from.
+
+The noise can be applied notably on sensors and actuators.
+"""
+
+from abc import ABCMeta
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Noise(object):
+    r"""Noise (abstract) class
+
+    Abstract Noise class.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self):
+        """Initialize the noise.
+        """
+        pass
+
+    def apply_noise(self, data, inplace=False):
+        """
+        Apply the noise on the given data.
+
+        Args:
+            data (int, float, np.array): data to apply the noise on.
+            inplace (bool): if True, it will directly modify the given data, and won't return a copy of it.
+
+        Returns:
+            int, float, np.array: noisy data.
+        """
+        raise NotImplementedError
+
+    def __str__(self):
+        """Return a string describing the class."""
+        return self.__class__.__name__
+
+    def __call__(self, data, inplace=False):
+        """Apply the noise on the given data."""
+        return self.apply_noise(data, inplace=inplace)
+
+
+class NoNoise(Noise):
+    r"""No noise class.
+
+    This is a dummy class that don't apply any noises on the given data and just return it.
+    """
+
+    def apply_noise(self, data, inplace=False):
+        """
+        Apply the noise on the given data.
+
+        Args:
+            data (int, float, np.array): data to apply the noise on.
+            inplace (bool): if True, it will directly modify the given data, and won't return a copy of it.
+
+        Returns:
+            int, float, np.array: noisy data.
+        """
+        return data

pyrobolearn/robots/noise/uniform_noise.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+"""Define the Uniform noise class.
+
+This noise can be applied notably on sensors and actuators.
+"""
+
+import numpy as np
+
+from pyrobolearn.robots.noise.noise import Noise
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class UniformNoise(Noise):
+    r"""Uniform noise class
+
+    This computes:
+
+    .. math:: y = x + \epsilon
+
+    where :math:`x` is the original data, :math:`y` is the noisy data, and
+    :math:`\epsilon \sim \mathcal{U}(a=-\frac{w}{2}, b=\frac{w}{2})` is the added Uniform noise where :math:`w = b-a`
+    is the width of the distribution.
+    """
+
+    def __init__(self, width=1.):
+        """Initialize the noise.
+
+        Args:
+            width (float, int, np.array): variances or covariance matrix.
+        """
+        super(UniformNoise, self).__init__()
+        self.width = width
+        self.radius = self.width/2.
+
+    def apply_noise(self, data, inplace=False):
+        """
+        Apply the noise on the given data.
+
+        Args:
+            data (int, float, np.array): data to apply the noise on.
+            inplace (bool): if True, it will directly modify the given data, and won't return a copy of it.
+
+        Returns:
+            int, float, np.array: noisy data.
+        """
+        if isinstance(data, np.ndarray):
+            noise = np.random.uniform(low=-self.radius, high=self.radius, size=data.shape)
+        else:
+            noise = np.random.uniform(low=-self.radius, high=self.radius)
+
+        # check if inplace operation
+        if not inplace:
+            return data + noise
+        data += noise
+        return data
+
+    def __str__(self):
+        """Return a string describing the class."""
+        return self.__class__.__name__ + "(a=" + str(-self.radius) + "b=" + str(self.radius) + ")"

pyrobolearn/robots/pepper.py

@@ -64,16 +64,16 @@ class Pepper(WheeledRobot, BiManipulator):
 
         # Note that we divide width and height by 4 (otherwise the simulator is pretty slow)
         self.camera_top = CameraSensor(self.sim, self.id, 4, width=2560 / 4, height=1080 / 4, fovy=44.30,
-                                       near=0.3, far=100, rate=60)
+                                       near=0.3, far=100, ticks=60)
         self.camera_bottom = CameraSensor(self.sim, self.id, 9, width=2560 / 4, height=1080 / 4, fovy=44.30,
-                                          near=0.3, far=100, rate=60)
+                                          near=0.3, far=100, ticks=60)
 
         # 3D camera sensor
         # From [1]: "One 3D camera is located in the forehead. It provides image resolution up to 320x240 at
         # 20 frames per second. One ASUS Xtion 3D sensor is located behind the eyes. VFOV = 45 deg, HFOV = 58 deg,
         # focus = [80cm, 3.5m]."
         self.camera_depth = CameraSensor(self.sim, self.id, 6, width=320, height=240, fovy=45, near=0.3, far=3.5,
-                                         rate=120)
+                                         ticks=120)
 
         self.cameras = [self.camera_top, self.camera_bottom, self.camera_depth]
 

pyrobolearn/robots/robot.py

@@ -22,6 +22,8 @@ from pyrobolearn.utils.transformation import *
 from pyrobolearn.utils.manifold_utils import tensor_matrix_product, symmetric_matrix_to_vector, logarithm_map, \
     distance_spd
 from pyrobolearn.robots.base import ControllableBody
+from pyrobolearn.robots.sensors.sensor import Sensor
+from pyrobolearn.robots.actuators.actuator import Actuator
 
 
 __author__ = "Brian Delhaisse"
@@ -167,8 +169,8 @@ class Robot(ControllableBody):
         self.kp, self.kd = None, None
 
         # sensors and actuators
-        self.sensors = []  # list of sensors
-        self.actuators = []  # list of actuators
+        self.sensors = {}  # dict of sensors {SensorClass: [sensorInstance]}
+        self.actuators = {}  # dict of actuators {ActuatorClass: [actuatorInstance]}
 
     #############
     # Operators #
@@ -227,6 +229,60 @@ class Robot(ControllableBody):
         """Return the number of joints that are not fixed."""
         return len(self.joints)
 
+    @property
+    def sensors(self):
+        """Return the dict of Sensor instances."""
+        return self._sensors
+
+    @sensors.setter
+    def sensors(self, sensors):
+        """Set the dict of Sensor instances."""
+        if sensors is None:
+            sensors = {}
+        elif isinstance(sensors, Sensor):
+            sensors = {sensors.__class__: [sensors]}
+        elif isinstance(sensors, (list, tuple, dict)):
+            if isinstance(sensors, dict):
+                sensors = sensors.values()
+            sensor_dict = {}
+            for i, sensor in enumerate(sensors):
+                if not isinstance(sensor, Sensor):
+                    raise TypeError("Expecting the {}th sensor to be an instance of `Sensor`, instead got: "
+                                    "{}".format(i, type(sensor)))
+                sensor_dict.setdefault(sensor.__class__, []).append(sensor)
+            sensors = sensor_dict
+        else:
+            raise TypeError("Expecting the given 'sensors' to be a `Sensor` or a list of `Sensor`, instead got: "
+                            "{}".format(type(sensors)))
+        self._sensors = sensors
+
+    @property
+    def actuators(self):
+        """Return the dict of Actuator instances."""
+        return self._actuators
+
+    @actuators.setter
+    def actuators(self, actuators):
+        """Set the dict of Actuator instances."""
+        if actuators is None:
+            actuators = {}
+        elif isinstance(actuators, Actuator):
+            actuators = {actuators.__class__: [actuators]}
+        elif isinstance(actuators, (list, tuple, dict)):
+            if isinstance(actuators, dict):
+                actuators = actuators.values()
+            actuator_dict = {}
+            for i, actuator in enumerate(actuators):
+                if not isinstance(actuator, Actuator):
+                    raise TypeError("Expecting the {}th actuator to be an instance of `Actuator`, instead got: "
+                                    "{}".format(i, type(actuator)))
+                actuator_dict.setdefault(actuator.__class__, []).append(actuator)
+            actuators = actuator_dict
+        else:
+            raise TypeError("Expecting the given 'actuators' to be a `Actuator` or a list of `Actuator`, instead got: "
+                            "{}".format(type(actuators)))
+        self._actuators = actuators
+
     ###########
     # Methods #
     ###########
@@ -244,13 +300,16 @@ class Robot(ControllableBody):
 
     def sense(self):
         """Run all the sensors."""
-        for sensor in self.sensors:
-            sensor()
+        for sensors in self.sensors.itervalues():
+            for sensor in sensors:
+                sensor.clean()
+                sensor.sense()
 
     def act(self):
         """Run all the actuators."""
-        for actuator in self.actuators:
-            actuator()
+        for actuators in self.actuators.itervalues():
+            for actuator in actuators:
+                actuator()
 
     ########
     # Base #
@@ -402,6 +461,28 @@ class Robot(ControllableBody):
             return np.concatenate((acc[0], acc[1]))
         return acc
 
+    def get_base_linear_acceleration(self):
+        """
+        Return the linear acceleration of the base. Some simulators does not provide the accelerations.
+        If that is the case, then we use finite difference to compute it (calling this the first time will return a
+        zero vector for the linear acceleration).
+
+        Returns:
+            np.array[3]: linear acceleration
+        """
+        return self.get_base_acceleration(concatenate=False)[0]
+
+    def get_base_angular_acceleration(self):
+        """
+        Return the angular acceleration of the base. Some simulators does not provide the accelerations.
+        If that is the case, then we use finite difference to compute it (calling this the first time will return a
+        zero vector for the angular acceleration).
+
+        Returns:
+            np.array[3]: angular acceleration
+        """
+        return self.get_base_acceleration(concatenate=False)[1]
+
     def get_base_spatial_acceleration(self):
         """
         Return the base spatial acceleration (which is the concatenation of the angular and linear acceleration).
@@ -1224,6 +1305,12 @@ class Robot(ControllableBody):
         r"""
         Reset the state of the robot.
 
+        Args:
+            q (int, float, np.array[N], None): joint position(s).
+            dq (int, float, np.array[N], None): joint velocity(ies).
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will disable the motors of
+                all actuated joints.
+
         Warnings: This is only valid in the simulator, and note that calling this method overrides all physics
         simulation.
         """
@@ -1610,10 +1697,10 @@ class Robot(ControllableBody):
         """
         # check if cached
         if 'link_pos' in self._state:
-            pos = self._state['link_pos'][0]  # (N,6)
+            pos = self._state['link_pos'][0]  # (N,3)
         else:
             links = list(range(self.num_links))
-            pos = self.sim.get_link_world_positions(body_id=self.id, link_ids=links)  # (N,6)
+            pos = self.sim.get_link_world_positions(body_id=self.id, link_ids=links)  # (N,3)
             self._state['link_pos'] = [pos, time.time()]
 
         # if one link
@@ -1651,6 +1738,10 @@ class Robot(ControllableBody):
         p1 = self.get_link_world_positions(link_ids, flatten=False)
         p0 = self.get_base_position() if wrt_link_id is None or wrt_link_id == -1 \
                 else self.get_link_world_positions(wrt_link_id, flatten=False)
+
+        # p^0 = o^0_1 + R^0_1 p^1
+        # Notation: ^i = expressed in frame i, _j = point j, o^i_j = origin position of point j in frame i
+        # TODO: should I express it in the world coordinate frame or in the wrt_link frame??
         p = (p1 - p0)
         if flatten:
             return p.reshape(-1)
@@ -1706,7 +1797,9 @@ class Robot(ControllableBody):
                 q0 = np.asarray([get_quaternion_inverse(self.get_link_world_orientations(link))
                                  for link in wrt_link_id])
 
+        # R^w_1 = R^w_0 R^0_1  <-->  R^0_1 = (R^w_0)^{-1} R^w_1 = R^0_w R^w_1 --> q = q_0^{-1} * q_1
         q = get_quaternion_product(q0, q1)
+
         if flatten:
             q.reshape(-1)
         return q
@@ -2065,7 +2158,57 @@ class Robot(ControllableBody):
             return acc.reshape(-1)  # (N*6,)
         return acc
 
-    def get_spatial_link_world_acceleration(self, link_ids=None, flatten=True):
+    def get_link_world_linear_accelerations(self, link_ids=None, flatten=True):
+        """
+        Return the linear accelerations (expressed in the Cartesian world space coordinates) for the given link(s).
+        See :func:`~get_link_world_accelerations` for more information.
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the linear
+                accelerations of all links associated to actuated joints.
+            flatten (bool): if True, it will return a 1D array instead of a 2D array
+
+        Returns:
+            if 1 link:
+                np.array[3]: linear acceleration of the link in the Cartesian world space
+            if multiple links:
+                np.array[N*3], np.array[N,3]: linear acceleration of each link
+        """
+        accelerations = self.get_link_world_accelerations(link_ids=link_ids, flatten=False)
+        if isinstance(link_ids, int):
+            return accelerations[:3]
+        accelerations = accelerations[:, :3]
+
+        if flatten:
+            return accelerations.reshape(-1)
+        return accelerations
+
+    def get_link_world_angular_accelerations(self, link_ids=None, flatten=True):
+        """
+        Return the angular accelerations (expressed in the Cartesian world space coordinates) for the given link(s).
+        See :func:`~get_link_world_accelerations` for more information.
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the angular
+                accelerations of all links associated to actuated joints.
+            flatten (bool): if True, it will return a 1D array instead of a 2D array
+
+        Returns:
+            if 1 link:
+                np.array[3]: angular acceleration of the link in the Cartesian world space
+            if multiple links:
+                np.array[N*3], np.array[N,3]: angular acceleration of each link
+        """
+        accelerations = self.get_link_world_accelerations(link_ids=link_ids, flatten=False)
+        if isinstance(link_ids, int):
+            return accelerations[3:]
+        accelerations = accelerations[:, 3:]
+
+        if flatten:
+            return accelerations.reshape(-1)
+        return accelerations
+
+    def get_spatial_link_world_accelerations(self, link_ids=None, flatten=True):
         r"""
         Return the spatial link world accelerations which is the concatenation of the angular and linear accelerations.
         The difference with :func:`~get_link_world_accelerations` is that this one returns the concatenation of the
@@ -4038,26 +4181,38 @@ class Robot(ControllableBody):
             return self.sensors
         return self.sensors[idx]
 
-    def get_imu(self, idx=0):
-        pass
+    def add_sensor(self, sensor):
+        """
+        Add a sensor to the list of sensors.
 
-    def get_force_torque_sensor(self, idx=0):
-        pass
+        Args:
+            sensor (Sensor): sensor instance.
+        """
+        if not isinstance(sensor, Sensor):
+            raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, instead got: "
+                            "{}".format(sensor))
+        self.sensors.setdefault(sensor.__class__, []).append(sensor)
 
-    def has_camera(self):
-        return False
-
-    def get_camera(self, idx=0):
-        pass
-
-    def get_camera_image(self, idx=0):
-        pass
-
-    def get_main_camera(self):
-        pass
-
-    def get_main_camera_image(self):
-        pass
+    # def get_imu(self, idx=0):
+    #     pass
+    #
+    # def get_force_torque_sensor(self, idx=0):
+    #     pass
+    #
+    # def has_camera(self):
+    #     return False
+    #
+    # def get_camera(self, idx=0):
+    #     pass
+    #
+    # def get_camera_image(self, idx=0):
+    #     pass
+    #
+    # def get_main_camera(self):
+    #     pass
+    #
+    # def get_main_camera_image(self):
+    #     pass
 
     #############
     # Actuators #
@@ -4087,6 +4242,18 @@ class Robot(ControllableBody):
             return self.actuators
         return self.actuators[idx]
 
+    def add_actuator(self, actuator):
+        """
+        Add an actuator to the list of actuators.
+
+        Args:
+            actuator (Actuator): actuator instance.
+        """
+        if not isinstance(actuator, Actuator):
+            raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, instead got: "
+                            "{}".format(actuator))
+        self.actuators.setdefault(actuator.__class__, []).append(actuator)
+
     #########
     # Debug #
     #########

pyrobolearn/robots/sensors/__init__.py

@@ -3,22 +3,28 @@
 from .sensor import Sensor
 
 # import joint + encoder sensors
-from .joints import *
+from .joints import JointSensor, JointEncoderSensor
 
 # import link sensors
-from .links import *
+from .links import LinkSensor
 
 # import ft sensors
-from .force_torque import *
+from .force_torque import JointTorqueSensor, JointForceTorqueSensor
 
 # import imu sensors
-from .imu import *
+from .imu import IMUSensor
+
+# import contact sensors
+from .contact import ContactSensor
 
 # import camera sensors
-from .camera import *
+from .camera import CameraSensor, DepthCameraSensor, RGBCameraSensor
+
+# import rays
+from .ray import RaySensor, RayBatchSensor, HeightmapSensor
 
 # import light / laser sensors
-from .light import *
+# from .light import *
 
 # import miscellaneous sensors
-from .misc import *
+# from .misc import *

pyrobolearn/robots/sensors/altimeter.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+"""Define the altimeter sensor which measures the altitude of an object.
+"""
+
+import numpy as np
+
+from pyrobolearn.robots.sensors.links import LinkSensor
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class AltimeterSensor(LinkSensor):
+    r"""Altimeter sensor
+
+    'An altimeter or an altitude meter is an instrument used to measure the altitude of an object above a fixed level.'
+    [1] Here the fixed level is set to be the ground.
+
+    References:
+        - [1] Altimeter (Wikipedia): https://en.wikipedia.org/wiki/Altimeter
+    """
+
+    def __init__(self, simulator, body_id, link_id=-1, noise=None, ticks=1, latency=None, position=None,
+                 orientation=None):
+        """
+        Initialize the altimeter sensor.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id.
+            link_id (int): unique id of the link.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+        """
+        super(AltimeterSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                              latency=latency, position=position, orientation=orientation)
+
+    def _sense(self, apply_noise=True):
+        """
+        Sense the altitude.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            float: altitude (in z-direction).
+        """
+        # check if the simulator supports that sensor
+        if self.sim.supports_sensors("altimeter"):
+            return self.sim.get_sensor("altimeter", self.body_id, self.link_id).sense()
+
+        z = self.get_link_world_position()[-1]  # get the z direction
+        if apply_noise:
+            z = self._noise(z)
+
+        return z

pyrobolearn/robots/sensors/camera.py

@@ -19,7 +19,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CameraSensor(LinkSensor):
+class CameraSensor(LinkSensor):  # TODO: double-check this class
     r"""Camera Sensor class.
 
     The following operations are carried out (in the given order) to display images seen by the camera:
@@ -44,7 +44,7 @@ class CameraSensor(LinkSensor):
 
     Examples:
         sim = BulletClient(connection_mode=p.GUI)
-        cam = Camera(sim, width=400, height=400, target_position=(0,0,0), eyePosition=(2,0,1))
+        cam = Camera(sim, width=400, height=400, target_position=(0,0,0), position=(2,0,1))
         img = cam.get_rgb_image()
         plt.imshow(img)
         plt.show()
@@ -56,7 +56,8 @@ class CameraSensor(LinkSensor):
         [4] http://learnwebgl.brown37.net/08_projections/projections_perspective.html
     """
 
-    def __init__(self, simulator, body_id, link_id, width, height, position=None, orientation=None, rate=50,
+    def __init__(self, simulator, body_id, link_id, width, height, noise=None, ticks=50, latency=None,
+                 position=None, orientation=None,
                  target_position=None, distance=10.,
                  fovy=60, aspect=None, near=0.01, far=100.,
                  left=None, right=None, bottom=None, top=None):
@@ -65,15 +66,22 @@ class CameraSensor(LinkSensor):
 
         Args:
             simulator: simulator to access to the sensor.
-            position (list/tuple/array of 3 floats): position of the sensor
-            orientation (quaternion): orientation of the sensor
+            body_id (int): unique id of the body.
+            link_id (int): unique id of the link.
             width (int): width of the returned pictures
             height (int): height of the returned pictures
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
 
             For a view matrix:
                 target_position (list/tuple of 3 floats): target focus point in cartesian world coordinates
-                eyePosition (list/tuple of 3 floats): eye position in cartesian world coordinates
-                UpVector (list/tuple of 3 floats): up vector of the camera in cartesian world coordinates
+                position (list/tuple of 3 floats): eye position in cartesian world coordinates
+                up_vector (list/tuple of 3 floats): up vector of the camera in cartesian world coordinates
 
             For a view matrix from RPY:
                 target_position (list/tuple of 3 floats): target focus point in cartesian world coordinates
@@ -81,7 +89,7 @@ class CameraSensor(LinkSensor):
                 yaw (float): yaw angle in degrees of the camera
                 pitch (float): pitch angle in degrees of the camera
                 roll (float): roll angle in degrees of the camera
-                upAxisIndex (int): either 1 for Y or 2 for Z axis up (default Z axis)
+                up_axis_index (int): either 1 for Y or 2 for Z axis up (default Z axis)
 
             For a perspective projection:
                 fovy (float): field of view in the y direction (height) in degrees
@@ -97,7 +105,8 @@ class CameraSensor(LinkSensor):
                 bottom (float): bottom screen (canvas) coordinate
                 top (float): top screen (canvas) coordinate
         """
-        super(CameraSensor, self).__init__(simulator, body_id, link_id, position, orientation, rate)
+        super(CameraSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                           latency=latency, position=position, orientation=orientation)
 
         self.width = int(width)
         self.height = int(height)
@@ -168,20 +177,18 @@ class CameraSensor(LinkSensor):
         """
         Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency)
         """
-        img = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
-                                        shadow=1,  # lightDirection=[1,1,1],
-                                        # renderer=self.sim.ER_TINY_RENDERER)[2])
-                                        renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[2]
-        img = img.reshape(self.width, self.height, 4)  # RGBA
+        img = self.sim.get_rgba_image(self.width, self.height, view_matrix=self.getV(), projection_matrix=self._P,
+                                      shadow=1,  # lightDirection=[1,1,1],
+                                      # renderer=self.sim.ER_TINY_RENDERER)[2])
+                                      renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)
         return img
 
     def get_depth_image(self):
         """
         Return the depth image.
         """
-        img = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
-                                        renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[3]
-        img = img.reshape(self.width, self.height)
+        img = self.sim.get_depth_image(self.width, self.height, view_matrix=self.getV(), projection_matrix=self._P,
+                                       renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)
         return img
 
     def get_rgbad_image(self, concatenate=True):
@@ -189,23 +196,82 @@ class CameraSensor(LinkSensor):
         Return the RGBA and depth images.
         """
         rgba, depth = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P)[2:4]
-        rgba = rgba.reshape(self.width, self.height, 4)
-        depth = depth.reshape(self.width, self.height)
         if concatenate:
             return np.dstack((rgba, depth))
         return rgba, depth
 
-    _sense = get_rgbad_image
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the camera sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[W,H,5]: RGBA and depth image concatenated together
+        """
+        data = self.get_rgbad_image()
+        if apply_noise:
+            self._noise(data, inplace=True)  # inplace otherwise can be expensive
+        return data
 
 
 class DepthCameraSensor(CameraSensor):
     r"""Depth Camera sensor.
     """
-    _sense = CameraSensor.get_depth_image
+
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the depth camera sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[W,H]: depth image
+        """
+        data = self.get_depth_image()
+        if apply_noise:
+            self._noise(data, inplace=True)  # inplace otherwise can be expensive
+        return data
 
 
-class Camera2DSensor(CameraSensor):
-    r"""2D camera sensor
+class RGBCameraSensor(CameraSensor):
+    r"""RGB camera sensor
     """
-    _sense = CameraSensor.get_rgb_image
 
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the camera RGB sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[W,H]: depth image
+        """
+        data = self.get_rgb_image()
+        if apply_noise:
+            self._noise(data, inplace=True)  # inplace otherwise can be expensive
+        return data
+
+
+# class SegmentationCameraSensor(CameraSensor):
+#     r"""Segmentation Camera sensor.
+#     """
+#
+#     def _sense(self, apply_noise=True):
+#         """
+#         Sense using the camera RGB sensor.
+#
+#         Args:
+#             apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+#
+#         Returns:
+#             np.array[W,H]: depth image
+#         """
+#         data = self.get_rgb_image()
+#         if apply_noise:
+#             self._noise(data, inplace=True)  # inplace otherwise can be expensive
+#         return data
+#

pyrobolearn/robots/sensors/contact.py

@@ -20,16 +20,29 @@ class ContactSensor(LinkSensor):
     This sensor return 1 if in contact with an object, and 0 otherwise.
     """
 
-    def __init__(self, simulator, body_id, link_id, position, orientation, rate=1):
-        super(ContactSensor, self).__init__(simulator, body_id, link_id, position, orientation, rate)
+    def __init__(self, simulator, body_id, link_id=-1, noise=None, ticks=1, latency=None):
+        """
+        Initialize the contact sensor.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id.
+            link_id (int): unique id of the link.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+        """
+        super(ContactSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                            latency=latency)
 
     def get_contact_points(self):
-        """Get the contact points.
+        """
+        Get the contact points.
 
         Returns:
             list: list of contacts
         """
-        contacts = self.sim.get_contact_points(bodyA=self.body_id, linkIndexA=self.link_id)
+        contacts = self.sim.get_contact_points(bodyA=self.body_id, link1_id=self.link_id)
         return contacts
 
     def is_in_contact(self):
@@ -38,27 +51,37 @@ class ContactSensor(LinkSensor):
         """
         return len(self.get_contact_points()) > 0
 
-    # alias
-    _sense = is_in_contact
-
-
-class PressureSensor(LinkSensor):
-    r"""Pressure sensor.
-
-    Compared to the binary contact sensor, this sensor returns a continuous pressure value.
-    """
-
-    def __init__(self, simulator, body_id, link_id):
-        super(PressureSensor, self).__init__(simulator, body_id, link_id)
-
-    def _sense(self):
-        raise NotImplementedError
-
-
-class TouchSensor(LinkSensor):
-    r"""Touch Sensor
-    """
-    pass
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the contact sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            bool: True if there is contact.
+        """
+        # there is already some noise from the simulator
+        return self.is_in_contact()
 
 
+# class PressureSensor(LinkSensor):
+#     r"""Pressure sensor.
+#
+#     Compared to the binary contact sensor, this sensor returns a continuous pressure value.
+#     """
+#
+#     def __init__(self, simulator, body_id, link_id):
+#         super(PressureSensor, self).__init__(simulator, body_id, link_id)
+#
+#     def _sense(self, apply_noise=True):
+#         raise NotImplementedError
+#
+#
+# class TouchSensor(LinkSensor):
+#     r"""Touch Sensor
+#     """
+#     pass
+#
+#
 # class SkinPressureSensor

pyrobolearn/robots/sensors/force_torque.py

@@ -16,15 +16,95 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ForceTorqueSensor(JointSensor):
-    r"""Force-Torque (FT or F/T) Sensor
+class JointTorqueSensor(JointSensor):
+    r"""Joint Torque sensor.
 
-    The F/T sensor allows to measure the forces and torques applied to it.
     """
 
-    def __init__(self, simulator, body_id, joint_id, position, orientation, rate=1):
-        super(ForceTorqueSensor, self).__init__(simulator, body_id, joint_id, position, orientation, rate)
-        self.sim.enable_joint_force_torque_sensor(body_id, joint_id, enableSensor=True)
+    def __init__(self, simulator, body_id, joint_ids, noise=None, ticks=1, latency=None):
+        """
+        Initialize the F/T sensor.
 
-    def _sense(self):
-        return np.array(self.sim.getJointState(self.body_id, self.joint_id)[2])
+        Args:
+            simulator (Simulator): simulator
+            body_id (int): unique body id.
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+        """
+        super(JointTorqueSensor, self).__init__(simulator, body_id=body_id, joint_ids=joint_ids, noise=noise,
+                                                ticks=ticks, latency=latency)
+
+        # enable the F/T sensors
+        self.sim.enable_joint_force_torque_sensor(body_id, joint_ids=self.joint_ids, enable=True)
+
+    def _sense(self, apply_noise=True):
+        """Sense the force/torques values.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[N]: torque values
+        """
+        # check if the simulator supports that sensor
+        if self.sim.supports_sensors("torque"):
+            return self.sim.get_sensor("torque", self.body_id, self.joint_ids).sense().reshape(-1)
+
+        torques = self.sim.get_joint_torques(self.body_id, self.joint_ids).reshape(-1)
+        if apply_noise:
+            torques = self._noise(torques)
+
+        return torques
+
+
+class JointForceTorqueSensor(JointSensor):
+    r"""Joint Force-Torque (FT or F/T) Sensor
+
+    The F/T sensor allows to measure the forces and torques applied to it; "it reports the joint reaction forces in
+    the fixed degrees of freedom: a fixed joint will measure all 6DOF joint forces/torques. A revolute joint
+    force/torque sensor will measure 5DOF reaction forces along all axis except the revolute axis." [1]
+
+    Note that this work with fixed joints as well.
+
+    References:
+        - [1] Pybullet
+    """
+
+    def __init__(self, simulator, body_id, joint_ids, noise=None, ticks=1, latency=None):
+        """
+        Initialize the joint F/T sensor.
+
+        Args:
+            simulator (Simulator): simulator
+            body_id (int): unique body id.
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+        """
+        super(JointForceTorqueSensor, self).__init__(simulator, body_id=body_id, joint_ids=joint_ids, noise=noise,
+                                                     ticks=ticks, latency=latency)
+
+        # enable the F/T sensors
+        self.sim.enable_joint_force_torque_sensor(body_id, joint_ids=self.joint_ids, enable=True)
+
+    def _sense(self, apply_noise=True):
+        """Sense the force/torques values.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[6*N]: F/T values
+        """
+        # check if the simulator supports that sensor
+        if self.sim.supports_sensors("force-torque"):
+            return self.sim.get_sensor("force-torque", self.body_id, self.joint_ids).sense().reshape(-1)
+
+        forces = self.sim.get_joint_reaction_forces(self.body_id, self.joint_ids).reshape(-1)
+        if apply_noise:
+            forces = self._noise(forces)
+
+        return forces

pyrobolearn/robots/sensors/imu.py

@@ -3,13 +3,19 @@
 
 'An inertial measurement unit (IMU) is an electronic device that measures and reports a body's specific force,
 angular rate, and sometimes the magnetic field surroundings the body, using a combination of accelerometers
-and gyroscopes, sometimes also magnetometers' (Wikipedia)
+and gyroscopes, sometimes also magnetometers' [1]
+
+References:
+    - [1] Inertial measurement unit: https://en.wikipedia.org/wiki/Inertial_measurement_unit
 """
 
+import numpy as np
+
+import pyrobolearn as prl
 from pyrobolearn.robots.sensors.links import LinkSensor
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -24,13 +30,64 @@ class IMUSensor(LinkSensor):
     The IMU sensor measures the linear and angular motion using accelerometers and gyroscopes. Specifically,
     3-axis accelerometers allow to measure the accelerations along its axes, and 3-axis gyroscopes measure the
     angular velocities around its axes. Sometimes, theses sensors also have a 3-axis magnetometer which measures
-    the magnetic field.
+    the magnetic field. [1]
 
     Note that real IMUs typically accumulate errors over time, and thus the integrated values drift over time.
+
+    A very wide variety of IMUs exists, depending on application types, with performance ranging [:
+
+    - from 0.1 deg/s to 0.001 deg/h for gyroscope
+    - from 100 mg to 10^{-6}g for accelerometers
+
+    References:
+        - [1] Inertial measurement unit: https://en.wikipedia.org/wiki/Inertial_measurement_unit
+        - [2] "IMU, what for: performance per application infographic":
+            https://www.thalesgroup.com/en/worldwide/aerospace/topaxyz-inertial-measurement-unit-imu/infographic
     """
 
-    def __init__(self, simulator, body_id, link_id, position, orientation):
-        super(IMUSensor, self).__init__(simulator, body_id, link_id, position, orientation)
+    def __init__(self, simulator, body_id, link_id=-1, noise=None, ticks=1, latency=None, position=None,
+                 orientation=None):
+        """
+        Initialize the IMU sensor.
 
-    def _sense(self):
-        pass
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id.
+            link_id (int): unique id of the link.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+        """
+        super(IMUSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                        latency=latency, position=position, orientation=orientation)
+
+    def _sense(self, apply_noise=True):
+        """Sense using the IMU sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[6]: concatenation of linear accelerations and angular velocities
+        """
+        # if the simulator supports IMU sensors, return the sensed data
+        if self.simulator.supports_sensors("imu"):
+            return self.simulator.get_sensor("imu", self.body_id, self.link_id).sense()
+
+        # linear acceleration
+        acceleration = self.get_link_world_acceleration()[:3]
+
+        # angular velocity
+        angular_velocity = self.get_link_world_velocity()[3:]
+
+        # concatenate the data and apply the noise
+        data = np.concatenate((acceleration, angular_velocity))
+        if apply_noise:
+            data = self._noise(data)
+
+        # return the noisy data
+        return data

pyrobolearn/robots/sensors/joints.py

@@ -5,7 +5,9 @@ This mainly include encoders.
 """
 
 import copy
-from abc import ABCMeta, abstractmethod
+import time
+from abc import ABCMeta
+import numpy as np
 
 from pyrobolearn.robots.sensors.sensor import Sensor
 
@@ -26,35 +28,167 @@ class JointSensor(Sensor):
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, simulator, body_id, joint_id, position=None, orientation=None, rate=1):
+    def __init__(self, simulator, body_id, joint_ids=None, noise=None, ticks=1, latency=None):
         """Initialize the sensor.
 
         Args:
             simulator (Simulator): simulator
-            body_id (int): unique id of the body
-            joint_id (int): unique id of the joint
-            position (vec3): local position of the sensor with respect to the given joint
-            orientation (vec4): local orientation of the sensor with respect to the given joint
-            rate (int): number of steps to wait before acquisition of the next sensor value.
+            body_id (int): unique body id.
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
         """
-        super(JointSensor, self).__init__(simulator, body_id, position, orientation, rate)
-        self.joint_id = joint_id
+        super(JointSensor, self).__init__(simulator, body_id=body_id, noise=noise, ticks=ticks, latency=latency)
 
-    @property
-    def position(self):
-        """
-        Return the joint position
-        """
-        return self.sim.get_joint_state(self.body_id, self.joint_id)[0]
+        # set the joint ids
+        if joint_ids is None:
+            # get actuated joints
+            joint_ids = []
+            for joint_id in range(self.sim.num_joints(self.body_id)):
+                joint_info = self.sim.get_joint_info(self.body_id, joint_id)
+                if joint_info[2] != self.sim.JOINT_FIXED:  # if not a fixed joint
+                    joint_ids.append(joint_info[0])
+        elif isinstance(joint_ids, int):
+            joint_ids = [joint_ids]
+        elif isinstance(joint_ids, (tuple, list)):
+            for i, joint in enumerate(joint_ids):
+                if not isinstance(joint, int):
+                    raise TypeError("Expecting the given {}th joint id to be an int, instead got: {}".format(i, joint))
+        else:
+            raise TypeError("Expecting the given 'joint_ids' to be an int or list of int, instead got: "
+                            "{}".format(joint_ids))
+        self.joint_ids = joint_ids
+        self.q_indices = self.sim.get_q_indices(self.body_id, self.joint_ids)
 
-    @abstractmethod
-    def _sense(self):
-        raise NotImplementedError
+        # joint state
+        self._state = {}
+        self._prev_state = {}
+
+    #############
+    # Operators #
+    #############
+
+    def clean(self):
+        """Clean sensor values."""
+        # update previous and current states
+        self._prev_state = self._state
+        self._state = {}
+
+    def get_joint_positions(self):
+        r"""
+        Get the position of the given joint(s).
+
+        Returns:
+            if 1 joint:
+                float: joint position [rad]
+            if multiple joints:
+                np.array[N]: joint positions [rad]
+        """
+        # check if cached
+        if 'q' in self._state:
+            # get cached joint positions
+            q = self._state['q'][0]
+        else:
+            # get joint positions and cache it
+            q = self.sim.get_joint_positions(self.body_id, self.joint_ids)
+            self._state['q'] = [q, time.time()]
+
+        # return joint positions
+        return q[self.q_indices]
+
+    def get_joint_velocities(self):
+        r"""
+        Get the velocity of the given joint(s).
+
+        Returns:
+            if 1 joint:
+                float: joint velocity [rad/s]
+            if multiple joints:
+                np.array[N]: joint velocities [rad/s]
+        """
+        # check if cached
+        if 'dq' in self._state:
+            # get cached joint velocities
+            dq = self._state['dq'][0]
+        else:
+            # get joint velocities and cache it
+            dq = self.sim.get_joint_velocities(self.body_id, self.joint_ids)
+            self._state['dq'] = [dq, time.time()]
+
+        # return joint velocities
+        return dq[self.q_indices]
+
+    def get_joint_accelerations(self):
+        r"""
+        Get the acceleration of the specified joint(s). If the simulator doesn't provide the joint accelerations, this
+        is computed using finite difference :math:`\ddot{q}(t) = \frac{\dot{q}(t) - \dot{q}(t-dt)}{dt}`.
+
+        Warnings: if we use finite difference, note that the first time this method is called, it will return a zero
+            vector because we do not have previous joint velocities (i.e. :math:`\dot{q}(t-dt)`) yet.
+
+        Returns:
+            if 1 joint:
+                float: joint acceleration [rad/s^2]
+            if multiple joints:
+                np.array[N]: joint accelerations [rad/s^2]
+        """
+        # check if cached
+        if 'ddq' in self._state:
+            ddq = self._state[0]
+            return ddq[self.q_indices]
+
+        # if simulator supports accelerations
+        if self.sim.supports_acceleration():
+            return self.sim.get_joint_accelerations(self.body_id, self.joint_ids)
+
+        # else, use finite difference
+
+        # get current joint velocities and time
+        if 'dq' in self._state:
+            dq, t = self._state['dq']
+        else:
+            dq, t = self.get_joint_velocities(), time.time()
+            self._state['dq'] = [dq, t]
+
+        # if we did not cache the previous joint velocities, return zero vector for accelerations
+        if 'dq' not in self._prev_state:
+            ddq = np.zeros(len(self.joint_ids))
+            self._state['ddq'] = [ddq, t]
+            return ddq[self.q_indices]
+
+        # retrieve previous joint velocities and time
+        dq_prev, t_prev = self._prev_state['dq']
+
+        # compute time difference
+        if self.sim.use_real_time():  # if the simulator is in real-time mode
+            dt = (t - t_prev)
+        else:  # if we are stepping in the simulator
+            dt = self.sim.timestep
+
+        # compute joint accelerations using finite difference, and cache it
+        ddq = (dq - dq_prev) / dt
+        self._state['ddq'] = [ddq, t]
+
+        # return joint accelerations
+        return ddq[self.q_indices]
+
+    def get_joint_torques(self):
+        r"""
+        Get the applied torque on the given joint(s).
+
+        Returns:
+            if 1 joint:
+                float: torque [Nm]
+            if multiple joints:
+                np.array[N]: torques associated to the given joints [Nm]
+        """
+        return self.sim.get_joint_torques(self.body_id, self.joint_ids)
 
     def __copy__(self):
         """Return a shallow copy of the sensor. This can be overridden in the child class."""
-        return self.__class__(simulator=self.simulator, body_id=self.body_id, joint_id=self.joint_id,
-                              position=self.local_position, orientation=self.local_orientation, rate=self.rate)
+        return self.__class__(simulator=self.simulator, body_id=self.body_id, joint_id=self.joint_ids,
+                              position=self.local_position, orientation=self.local_orientation, rate=self._ticks)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the sensor. This can be overridden in the child class.
@@ -64,21 +198,101 @@ class JointSensor(Sensor):
         """
         simulator = memo.get(self.simulator, self.simulator)  # copy.deepcopy(self.simulator, memo)
         body_id = copy.deepcopy(self.body_id)
-        joint_id = copy.deepcopy(self.joint_id)
+        joint_id = copy.deepcopy(self.joint_ids)
         position = copy.deepcopy(self.local_position)
         orientation = copy.deepcopy(self.local_orientation)
         sensor = self.__class__(simulator=simulator, body_id=body_id, joint_id=joint_id, position=position,
-                                orientation=orientation, rate=self.rate)
+                                orientation=orientation, rate=self._ticks)
         memo[self] = sensor
         return sensor
 
 
-class Encoder(JointSensor):
-    r"""Encoder joint sensor
+class JointEncoderSensor(JointSensor):
+    r"""Joint encoder sensor
 
     The encoder is a sensor that measures rotation allowing to determine the angle, displacement, velocity, or
     acceleration.
+
+    References:
+        - [1] Robot encoder: https://www.societyofrobots.com/sensors_encoder.shtml
     """
 
-    def __init__(self, simulator, body_id, joint_id, noise=None):
-        super(Encoder, self).__init__(simulator, body_id, joint_id)
+    def __init__(self, simulator, body_id, joint_ids=None, noise=None, ticks=1, latency=None):
+        """
+        Initialize the joint encoder sensor.
+
+        Args:
+            simulator (Simulator): simulator
+            body_id (int): unique body id.
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+        """
+        super(JointEncoderSensor, self).__init__(simulator, body_id=body_id, joint_ids=joint_ids, noise=noise,
+                                                 ticks=ticks, latency=latency)
+
+        # sense once
+        self._sense()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def q(self):
+        """Return the sensed joint position values."""
+        return self._data[:len(self.joint_ids)]
+
+    @property
+    def dq(self):
+        """Return the sensed joint velocity values."""
+        return self._data[len(self.joint_ids):2*len(self.joint_ids)]
+
+    @property
+    def ddq(self):
+        """Return the sensed joint acceleration values."""
+        return self._data[2*len(self.joint_ids):]
+
+    ###########
+    # Methods #
+    ###########
+
+    def get_sensed_joint_positions(self):
+        """Return the sensed joint positions."""
+        return self.q
+
+    def get_sensed_joint_velocities(self):
+        """Return the sensed joint velocities."""
+        return self.dq
+
+    def get_sensed_joint_accelerations(self):
+        """Return the sensed joint accelerations."""
+        return self.ddq
+
+    def _sense(self, apply_noise=True):
+        """
+        Sense the joint (position, velocities, accelerations) values.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[3*N]: concatenation of joint positions, velocities and accelerations
+        """
+        # if the simulator supports encoder sensors, return the sensed data
+        # if self.simulator.supports_sensors("encoder"):
+        #     return self.simulator.get_sensor("encoder", self.body_id, self.joint_ids).sense()
+
+        # joint state #
+        positions = self.get_joint_positions()
+        velocities = self.get_joint_velocities()
+        accelerations = self.get_joint_accelerations()
+
+        # concatenate the data and apply the noise
+        data = np.concatenate((positions, velocities, accelerations))
+        if apply_noise:
+            data = self._noise(data)
+
+        # return the noisy data
+        return data

pyrobolearn/robots/sensors/light.py

@@ -7,7 +7,7 @@ import numpy as np
 from pyrobolearn.robots.sensors.links import LinkSensor
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -26,6 +26,7 @@ class LaserSensor(LinkSensor):
     r"""Laser Sensor
 
     """
+    pass
 
 
 class ProximityLaserSensor(LaserSensor):
@@ -35,6 +36,13 @@ class ProximityLaserSensor(LaserSensor):
     pass
 
 
+class RaySensor(LaserSensor):
+    r"""
+
+    """
+    pass
+
+
 class IRProximitySensor(ProximityLaserSensor):
     r"""Infrared proximity sensor
 

pyrobolearn/robots/sensors/links.py

@@ -5,9 +5,11 @@ These include IMU, contact, Camera, and other sensors.
 """
 
 import copy
-from abc import ABCMeta, abstractmethod
+import time
+import numpy as np
+from abc import ABCMeta
 
-from pyrobolearn.utils.transformation import get_quaternion_product
+from pyrobolearn.utils.transformation import get_quaternion_product, get_rotated_point_from_quaternion
 from pyrobolearn.robots.sensors.sensor import Sensor
 
 
@@ -29,27 +31,48 @@ class LinkSensor(Sensor):
 
     __metaclass__ = ABCMeta
 
-    def __init__(self, simulator, body_id, link_id=None, position=None, orientation=None, rate=1):
-        """Initialize the sensor.
+    def __init__(self, simulator, body_id, link_id=-1, position=None, orientation=None, noise=None, ticks=1,
+                 latency=None):
+        """Initialize the link sensor.
 
         Args:
-            simulator (Simulator): simulator
-            body_id (int): unique id of the body
-            link_id (int): unique id of the link
-            position (vec3): local position of the sensor with respect to the given link
-            orientation (vec4): local orientation of the sensor with respect to the given link
-            rate (int): number of steps to wait before acquisition of the next sensor value.
+            simulator (Simulator): simulator instance.
+            body_id (int): unique id of the body.
+            link_id (int): unique id of the link.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
         """
-        super(LinkSensor, self).__init__(simulator, body_id, position, orientation, rate)
+        super(LinkSensor, self).__init__(simulator, body_id=body_id, position=position, orientation=orientation,
+                                         noise=noise, ticks=ticks, latency=latency)
+        # set link id
+        if not isinstance(link_id, int):
+            raise TypeError("Expecting the given 'link_id' to be an int, instead got: {}".format(type(link_id)))
         self.link_id = link_id
 
+        # link state
+        self._state = {}
+        self._prev_state = {}
+
+    ##############
+    # Properties #
+    ##############
+
     @property
     def position(self):
         """
         Return the link position in the Cartesian world frame.
         """
-        position = self.sim.get_link_state(self.body_id, self.link_id)[0]
-        position += self.local_position
+        position = self.sim.get_link_state(self.body_id, self.link_id)[0]  # world position of CoM (o^w_1)
+        orientation = self.sim.get_link_state(self.body_id, self.link_id)[1]  # world orientation of CoM (R^w_1)
+        # p^w = o^w_1 + R^w_1 p^1
+        # TODO: check if we are giving p^1 or p^2  --> p^w = o^w_1 + R^w_1 R^1_2 p^2 ??
+        #  The local position that we are giving, is it in the new orientated frame or the local frame?
+        position += get_rotated_point_from_quaternion(orientation, self.local_position)
         return position
 
     @property
@@ -57,18 +80,124 @@ class LinkSensor(Sensor):
         """
         Return the link orientation in the Cartesian world frame.
         """
-        orientation = self.sim.get_link_state(self.body_id, self.link_id)[1]
-        orientation = get_quaternion_product(self.local_orientation, orientation)
+        orientation = self.sim.get_link_state(self.body_id, self.link_id)[1]  # world orientation of CoM (R^w_1)
+        # R^w_2 = R^w_1 R^1_2
+        orientation = get_quaternion_product(orientation, self.local_orientation)
         return orientation
 
-    @abstractmethod
-    def _sense(self):
-        raise NotImplementedError
+    ###########
+    # Methods #
+    ###########
+
+    def clean(self):
+        """Clean sensor values."""
+        # update previous and current states
+        self._prev_state = self._state
+        self._state = {}
+
+    def get_link_world_position(self):
+        r"""
+        Return the CoM position (in the Cartesian world space coordinates) of the link associated with the sensor.
+
+        Returns:
+            np.array[3]: the link CoM position in the world space
+        """
+        # check if cached
+        if 'pos' in self._state:
+            pos = self._state['pos'][0]  # (3,)
+        else:
+            if self.link_id == -1:  # base link
+                pos = self.sim.get_base_position(self.body_id)
+            else:  # other link
+                pos = self.sim.get_link_world_positions(body_id=self.body_id, link_ids=self.link_id)  # (3,)
+            self._state['pos'] = [pos, time.time()]
+
+        # return position
+        return pos
+
+    def get_link_world_velocity(self):
+        r"""
+        Return the linear and angular velocities (expressed in the Cartesian world space coordinates) of the link
+        associated with the sensor.
+
+        Returns:
+            np.array[6]: linear and angular velocity of the link in the Cartesian world space
+        """
+        # check if cached
+        if 'vel' in self._state:
+            vel = self._state['vel'][0]  # (6,)
+        else:
+            if self.link_id == -1:  # base link
+                vel = np.concatenate(self.sim.get_base_velocity(self.body_id))
+            else:  # other link
+                vel = self.sim.get_link_world_velocities(body_id=self.body_id, link_ids=self.link_id)
+            self._state['vel'] = [vel, time.time()]
+
+        # return velocity
+        return vel
+
+    def get_link_world_acceleration(self):
+        r"""
+        Return the linear and angular accelerations (expressed in the Cartesian world space coordinates) of the link
+        associated with the sensor.
+
+        Returns:
+            np.array[6]: linear and angular acceleration of the link in the Cartesian world space
+        """
+        # check if cached
+        if 'acc' in self._state:
+            acc = self._state['acc'][0]
+        else:
+
+            # if the simulator keep in memory the accelerations, return it
+            if self.sim.supports_acceleration():
+                acc = self.sim.get_link_world_accelerations(self.body_id, link_ids=self.link_id)  # (6,)
+            else:  # else, use finite difference
+
+                # get current link world velocities and time
+                if 'vel' not in self._state:
+                    self.get_link_world_velocity()
+                vel, t = self._state['vel']  # (6,)
+
+                # if we did not cache the previous base velocity
+                if 'vel' not in self._prev_state:
+                    acc = np.zeros(6)  # (6,)
+                else:
+                    # retrieve previous link world velocities and time
+                    vel_prev, t_prev = self._prev_state['vel']  # (6,)
+
+                    # compute time difference
+                    if self.sim.use_real_time():  # if the simulator is in real-time mode
+                        dt = (t - t_prev)
+                    else:  # if we are stepping in the simulator
+                        dt = self.sim.timestep
+
+                    # get current link positions
+                    pos = self.get_link_world_position()  # (3,)
+
+                    # separate linear and angular velocities
+                    lin_vel, ang_vel = vel[:3], vel[3:]  # (3,)
+                    lin_vel_prev, ang_vel_prev = vel_prev[:3], vel_prev[3:]  # (3,)
+
+                    # compute base acceleration
+                    ang_acc = (ang_vel - ang_vel_prev) / dt
+                    lin_acc = (lin_vel - lin_vel_prev) / dt
+                    lin_acc += np.cross(ang_acc, pos) + np.cross(ang_vel, np.cross(ang_vel, pos))
+                    acc = np.concatenate((lin_acc, ang_acc))  # (6,)
+
+            # cache the acceleration
+            self._state['acc'] = [acc, time.time()]
+
+        return acc
+
+    #############
+    # Operators #
+    #############
 
     def __copy__(self):
         """Return a shallow copy of the sensor. This can be overridden in the child class."""
         return self.__class__(simulator=self.simulator, body_id=self.body_id, link_id=self.link_id,
-                              position=self.local_position, orientation=self.local_orientation, rate=self.rate)
+                              position=self.local_position, orientation=self.local_orientation, rate=self._ticks)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the sensor. This can be overridden in the child class.
@@ -82,6 +211,6 @@ class LinkSensor(Sensor):
         position = copy.deepcopy(self.local_position)
         orientation = copy.deepcopy(self.local_orientation)
         sensor = self.__class__(simulator=simulator, body_id=body_id, link_id=link_id, position=position,
-                                orientation=orientation, rate=self.rate)
+                                orientation=orientation, rate=self._ticks)
         memo[self] = sensor
         return sensor

pyrobolearn/robots/sensors/misc.py

@@ -2,7 +2,7 @@
 """Define miscellaneous sensors.
 """
 
-from pyrobolearn.robots.sensors.links import Sensor, LinkSensor
+from pyrobolearn.robots.sensors.links import LinkSensor
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -14,76 +14,87 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class HeightmapSensor(Sensor):
-    r"""Heightmap Sensor
-
-    Sensor that detects the heights of its surrounding using a grid map.
-    """
-
-    def __init__(self, simulator, body_id, link_id, width, height, num_rays_width, num_rays_height,
-                 position=None, orientation=None):
-        """Initialize the heightmap sensor.
-
-        Note that `num_rays_width * num_rays_height` has to be smaller than `simulator.MAX_RAY_INTERSECTION_BATCH_SIZE`.
-        In pybullet, this is currently set to be 256.
-
-        Args:
-            simulator (Simulator): simulator
-            body_id (int): unique id of the body
-            link_id (int): unique id of the link
-            position: local position of the sensor with respect to the given link
-            orientation: local orientation of the sensor with respect to the given link
-            width (float): width of the map (along the left-right axis of the body, measured in meters)
-            height (float): height of the map (along the front-back axis of the body, measured in meters)
-            num_rays_width (int): number of rays along the width dimension (left-right axis). This will be the 'width'
-                of the returned heightmap.
-            num_rays_height (int): number of rays along the height dimension (front-back axis). This will be
-                the 'height' of the returned heightmap.
-        """
-        super(HeightmapSensor, self).__init__(simulator, body_id, link_id, position, orientation)
-
-        # Check arguments
-        if not isinstance(num_rays_width, int):
-            raise TypeError("num_rays_width needs to be an integer")
-        if not isinstance(num_rays_height, int):
-            raise TypeError("num_rays_height needs to be an integer")
-        if num_rays_width * num_rays_height > self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE:  # pybullet = 256
-            raise ValueError("num_rays_width * num_rays_height can not be bigger"
-                             " than {}".format(self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE))
-        if num_rays_width < 2:
-            raise ValueError("num_rays_width must be equal or bigger than 2")
-        if num_rays_height < 2:
-            raise ValueError("num_rays_height must be equal or bigger than 2")
-
-        # construct the grid
-        self.width_step = float(width) / num_rays_width
-        self.height_step = float(height) / num_rays_height
-
-    def get_ray_from_positions(self):
-        # using width and height
-        pass
-
-    def get_ray_to_positions(self):
-        pass
-
-    def _sense(self, normalize=False, display=False):
-        """Return the heightmap.
-
-        Returns:
-            np.array: Height map with shape [width, height] where the values are the heights (in meters).
-        """
-        # calculate width and height
-        collisions = self.sim.ray_test_batch(self.get_ray_from_positions(), self.get_ray_to_positions())
-        return collisions
-
-
 class TemperatureSensor(LinkSensor):
-    pass
+    r"""Temperature sensor
+
+    A temperature sensor measures the temperature and converts it into an electrical signal.
+    """
+    raise NotImplementedError
 
 
 class UltrasonicSensor(LinkSensor):
-    pass
+    r"""Ultrasonic Sensor
+
+    "Ultrasonic transducers or ultrasonic sensors are a type of acoustic sensor divided into three broad categories:
+    transmitters, receivers and transceivers. Transmitters convert electrical signals into ultrasound, receivers
+    convert ultrasound into electrical signals, and transceivers can both transmit and receive ultrasound." [1]
+
+    References:
+        - [1] Ultrasonic transducer (Wikipedia): https://en.wikipedia.org/wiki/Ultrasonic_transducer
+    """
+    raise NotImplementedError
+
+
+class TransmitterSensor(UltrasonicSensor):
+    r"""Transmitter sensor
+
+    "The transmitter sensor is an acoustic sensor which converts the electrical signals into ultrasounds." [1]
+
+    References:
+        - [1] Ultrasonic transducer (Wikipedia): https://en.wikipedia.org/wiki/Ultrasonic_transducer
+    """
+    raise NotImplementedError
+
+
+class ReceiverSensor(UltrasonicSensor):
+    r"""Receiver sensor
+
+    "The receiver sensor is an acoustic sensor which converts ultrasound into electrical signals." [1]
+
+    References:
+        - [1] Ultrasonic transducer (Wikipedia): https://en.wikipedia.org/wiki/Ultrasonic_transducer
+    """
+    raise NotImplementedError
+
+
+class TransceiverSensor(UltrasonicSensor):
+    r"""Receiver sensor
+
+    "The transceiver sensor can both transmit and receive ultrasound by converting into/from electrical signals." [1]
+
+    References:
+        - [1] Ultrasonic transducer (Wikipedia): https://en.wikipedia.org/wiki/Ultrasonic_transducer
+    """
+    raise NotImplementedError
 
 
 class HumiditySensor(LinkSensor):
-    pass
+    r"""Humidity sensor
+
+    A humidity sensor measures and reports the moisture and air temperature.
+    """
+    raise NotImplementedError
+
+
+class GPSSensor(LinkSensor):
+    r"""GPS sensor
+
+    "The GPS is a satellite-based radionavigation system. Autonomous robots use a GPS sensors to get the latitude,
+    longitude, time, speed, and heading." [1]
+
+    References:
+        - [1] Global Positioning System (Wikipedia): https://en.wikipedia.org/wiki/Global_Positioning_System
+    """
+    raise NotImplementedError
+
+
+class MagnetometerSensor(LinkSensor):
+    r"""Magnetometer Sensor
+
+    "A magnetometer is a device that measures magnetism - the direction, strength, or relative change of a magnetic
+    field at a particular location." [1]
+
+    References:
+        - [1] Magnetometer (Wikipedia): https://en.wikipedia.org/wiki/Magnetometer
+    """
+    raise NotImplementedError

pyrobolearn/robots/sensors/ray.py

@@ -0,0 +1,248 @@
+#!/usr/bin/env python
+"""Define ray sensors; sensors that cast rays into the world and return the range of the nearest object that were
+intersected with these ones.
+"""
+
+import numpy as np
+
+from pyrobolearn.robots.sensors.links import LinkSensor
+from pyrobolearn.utils.transformation import get_rotated_point_from_quaternion
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class RaySensor(LinkSensor):
+    r"""Ray sensor
+
+    This sensor casts a single ray into the world, check for intersection, and return the range of the nearest object.
+    """
+
+    def __init__(self, simulator, body_id, to_position, link_id=-1, noise=None, ticks=1, latency=None, position=None,
+                 orientation=None):
+        """
+        Initialize the Ray sensor.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id.
+            to_position (np.array[3]): position where the ray should stop with respect to the new local link frame
+                (specified by :attr:`position` and :attr:`orientation`).
+            link_id (int): unique id of the link.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+        """
+        super(RaySensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                        latency=latency, position=position, orientation=orientation)
+        if isinstance(to_position, (tuple, list)):
+            to_position = np.asarray(to_position)
+        if not isinstance(to_position, np.ndarray):
+            raise TypeError("Expecting the given 'to_position' to be a np.array, but got instead: "
+                            "{}".format(type(to_position)))
+        if to_position.shape != (3,):
+            raise ValueError("Expecting the shape of the given 'to_position' to be (3,), but got instead a shape of: "
+                             "{}".format(to_position.shape))
+        self.to_position = to_position
+
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the ray sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            float: hit fraction along the ray in range [0,1] along the ray.
+        """
+        if self.simulator.supports_sensors("ray"):
+            return self.simulator.get_sensor("ray", self.body_id, self.link_id).sense()
+        position = self.position + get_rotated_point_from_quaternion(self.orientation, self.to_position)
+        hit = self.sim.ray_test(from_position=self.position, to_position=position)[2]
+        if apply_noise:
+            hit = self._noise(hit)
+        return hit
+
+
+class RayBatchSensor(LinkSensor):
+    r"""Ray batch sensor.
+
+    This sensor casts a batch of rays into the world, check for intersections, and return the range of the nearest
+    objects. This can be used for sonars, laser scanning range sensors (such as LIDAR), and others.
+
+    Note that the number of rays must be smaller than `simulator.MAX_RAY_INTERSECTION_BATCH_SIZE`. In pybullet, this
+    is currently set to 16,384.
+    """
+
+    def __init__(self, simulator, body_id, to_positions, link_id=-1, noise=None, ticks=1, latency=None, position=None,
+                 orientation=None):
+        """
+        Initialize the Ray batch sensor.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique body id.
+            to_positions (np.array[N,3]): position where each ray should stop with respect to the new local link frame
+                (specified by :attr:`position` and :attr:`orientation`).
+            link_id (int): unique id of the link.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+        """
+        super(RayBatchSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, noise=noise, ticks=ticks,
+                                             latency=latency, position=position, orientation=orientation)
+        if isinstance(to_positions, (tuple, list)):
+            to_positions = np.asarray(to_positions)
+        if not isinstance(to_positions, np.ndarray):
+            raise TypeError("Expecting the given 'to_positions' to be a np.array, but got instead: "
+                            "{}".format(type(to_positions)))
+        if to_positions.ndim != 2:
+            raise ValueError("Expecting the given 'to_positions' to be 2D array, but got instead a {}D "
+                             "array".format(to_positions.ndim))
+        if to_positions.shape[1] != 3:
+            raise ValueError("Expecting the shape of the given 'to_positions' to be (N,3), but got instead a shape "
+                             "of: {}".format(to_positions.shape))
+        if len(to_positions) > self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE:
+            raise ValueError("The number of 'to_positions' (={}) is bigger than the maximum amount authorized "
+                             "(={})".format(len(to_positions), self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE))
+        self.to_positions = to_positions
+
+    def _sense(self, apply_noise=True):
+        """
+        Sense using the ray batch sensor.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array[N]: hit fractions along each ray in range [0,1] along the ray.
+        """
+        if self.simulator.supports_sensors("ray_batch"):
+            return self.simulator.get_sensor("ray_batch", self.body_id, self.link_id).sense()
+        position = self.position + get_rotated_point_from_quaternion(self.orientation, self.to_positions)
+        rays = self.sim.ray_test_batch(from_positions=self.position, to_positions=position)
+        hit = np.array([ray[2] for ray in rays])
+        if apply_noise:
+            hit = self._noise(hit)
+        return hit
+
+
+class HeightmapSensor(LinkSensor):
+    r"""Heightmap Sensor
+
+    Sensor that detects the heights of its surrounding using a grid map.
+
+    Warnings: this is only valid in the simulator.
+    """
+
+    def __init__(self, simulator, body_id, link_id, width, height, num_rays_width=2, num_rays_height=2,
+                 max_ray_length=100, position=None, orientation=None):  # TODO: use orientation initially
+        """
+        Initialize the heightmap sensor. This is only valid in the simulator.
+
+        Note that `num_rays_width * num_rays_height` has to be smaller than `simulator.MAX_RAY_INTERSECTION_BATCH_SIZE`.
+        In pybullet, this is currently set to 16,384.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            body_id (int): unique id of the body
+            link_id (int): unique id of the link
+            width (float): width of the map (along the left-right axis (i.e. y axis) of the body, measured in meters)
+            height (float): height of the map (along the front-back axis (i.e. x axis) of the body, measured in meters)
+            num_rays_width (int): number of rays along the width dimension (left-right axis). This will be the 'width'
+                of the returned heightmap. This must be bigger or equal to 2.
+            num_rays_height (int): number of rays along the height dimension (front-back axis). This will be
+                the 'height' of the returned heightmap. This must be bigger or equal to 2.
+            max_ray_length (float): maximum length of each ray.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector. This position represents the center of the map.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
+        """
+        super(HeightmapSensor, self).__init__(simulator, body_id=body_id, link_id=link_id, position=position,
+                                              orientation=orientation)
+
+        # Check arguments
+
+        # set num_rays_width and num_rays_height
+        num_rays_width, num_rays_height = int(num_rays_width), int(num_rays_height)
+        if num_rays_width * num_rays_height > self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE:  # pybullet = 16,384
+            raise ValueError("num_rays_width * num_rays_height can not be bigger"
+                             " than {}".format(self.sim.MAX_RAY_INTERSECTION_BATCH_SIZE))
+        if num_rays_width < 2:
+            raise ValueError("num_rays_width must be equal or bigger than 2, but got: {}".format(num_rays_width))
+        if num_rays_height < 2:
+            raise ValueError("num_rays_height must be equal or bigger than 2, but got: {}".format(num_rays_height))
+        self._num_rays_width = num_rays_width
+        self._num_rays_height = num_rays_height
+
+        # set max_ray_length
+        if not isinstance(max_ray_length, (float, int)):
+            raise TypeError("Expecting 'max_ray_length' to be an int or float, but got instead: "
+                            "{}".format(type(max_ray_length)))
+        max_ray_length = float(max_ray_length)
+        if max_ray_length <= 0.:
+            raise ValueError("Expecting 'max_ray_length' to be positive, but got instead: {}".format(max_ray_length))
+        self._max_ray_length = max_ray_length
+
+        # set width and height
+        width, height = float(width), float(height)
+        if width <= 0.:
+            raise ValueError("Expecting the 'width' to be bigger than 0, but got: {}".format(width))
+        if height <= 0.:
+            raise ValueError("Expecting the 'height' to be bigger than 0, but got: {}".format(height))
+        self._width = width
+        self._height = height
+
+        self._z_array = np.ones(self._num_rays_width * self._num_rays_height)
+
+    def get_ray_from_to_positions(self):
+        """
+        Return the world positions for the rays to start and end.
+
+        Returns:
+            np.array[N,3]: list of starting positions for the rays
+            np.array[N,3]: list of ending positions for the rays
+        """
+        pos = self.position
+        w2, h2 = self._width / 2., self._height / 2.
+
+        x, y = np.meshgrid(np.linspace(pos[1] - w2, pos[1] + w2, self._num_rays_width),
+                           np.linspace(pos[0] - h2, pos[0] + h2, self._num_rays_height))
+        x, y = x.ravel(), y.ravel()
+        from_z = pos[2] * self._z_array
+        to_z = from_z - self._max_ray_length
+
+        from_positions = np.vstack((x, y, from_z)).T  # (N, 3)
+        to_positions = np.vstack((x, y, to_z)).T  # (N, 3)
+
+        return from_positions, to_positions
+
+    def _sense(self, apply_noise=True):
+        """
+        Return the heightmap.
+
+        Returns:
+            np.array[width, height]: Height map with shape [width, height] where the values are the hit fractions [0,1],
+                you can multiply it by :attr:`max_ray_length` to get the depth in meters.
+        """
+        from_positions, to_positions = self.get_ray_from_to_positions()
+        rays = self.sim.ray_test_batch(from_positions=from_positions, to_positions=to_positions)
+        hit = np.array([ray[2] for ray in rays]).reshape(self._num_rays_width, self._num_rays_height)
+        if apply_noise:
+            hit = self._noise(hit)
+        return hit

pyrobolearn/robots/sensors/sensor.py

@@ -8,14 +8,24 @@ simulation to reality. Also, note that some simulators are deterministic and thu
 add some noise to the returned sense value. The type of noise can also be selected at runtime.
 """
 
-# TODO: add latency + noise
+# TODO: convert ticks to rate when setting real-time
 
+import sys
 import copy
 from abc import ABCMeta, abstractmethod
 import numpy as np
 
+from pyrobolearn.simulators.simulator import Simulator
+from pyrobolearn.robots.base import Body
+from pyrobolearn.robots.noise.noise import Noise, NoNoise
 from pyrobolearn.utils.transformation import get_quaternion_product
 
+
+# define long for Python 3.x
+if int(sys.version[0]) == 3:
+    long = int
+
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -36,37 +46,80 @@ class Sensor(object):  # sensor attached to a link or joint
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, simulator, body_id, position=None, orientation=None, rate=1, latency=0):
-        """Initialize the sensor.
+    def __init__(self, simulator, body_id, noise=None, ticks=1, latency=None, position=None, orientation=None):
+        """
+        Initialize the sensor.
 
         Args:
-            simulator (Simulator): simulator
-            body_id (int): unique id of the body
-            position (vec3): local position of the sensor with respect to the given link
-            orientation (vec4): local orientation of the sensor with respect to the given link
-            rate (int): number of steps to wait before acquisition of the next sensor value.
-            latency (int, float): latency.
+            simulator (Simulator): simulator instance.
+            body_id (int, Body): unique id of the body, or body instance.
+            noise (None, Noise): noise to be added.
+            ticks (int): number of steps to wait/sleep before acquisition of the next sensor value.
+            latency (int, float, None): latency time / step.
+            position (np.array[3], None): local position of the sensor with respect to the given link. If None, it will
+                be the zero vector.
+            orientation (np.array[4], None): local orientation of the sensor with respect to the given link (expressed
+                as a quaternion [x,y,z,w]). If None, it will be the unit quaternion [0,0,0,1].
         """
+        # setting simulator
+        if not isinstance(simulator, Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
         self.sim = simulator
+
+        # set the body id
+        if isinstance(body_id, Body):
+            body_id = body_id.id
+        elif not isinstance(body_id, (int, long)):
+            raise TypeError("Expecting the given 'body_id' to be an int or an instance of `Body`, but got instead: "
+                            "{}".format(type(body_id)))
+        if body_id < 0:
+            raise ValueError("Expecting the given 'body_id' to be a positive integer, but got instead: "
+                             "{}".format(body_id))
         self.body_id = body_id
 
+        # set the local position of the sensor
         if position is None:
             position = [0., 0., 0.]
-        self.local_position = np.array(position)
+        self.local_position = np.asarray(position)
 
+        # set the local orientation of the sensor
         if orientation is None:
             orientation = [0., 0., 0., 1.]
-        self.local_orientation = np.array(orientation)
+        self.local_orientation = np.asarray(orientation)
 
-        self.rate = rate
-        self.cnt = -1
+        # set the ticks / rate
+        if ticks < 1:
+            raise ValueError("Expecting the given 'ticks' to be a positive number, but got instead: {}".format(ticks))
+        self._ticks = ticks
+        self._cnt = -1
 
-        # data from last acquisition
-        self.data = None
+        # set the noise
+        if noise is None:
+            noise = NoNoise()
+        if not isinstance(noise, Noise):
+            raise TypeError("Expecting the given 'noise' to be an instance of Noise, instead got: "
+                            "{}".format(type(noise)))
+        self._noise = noise
 
         # variable to check if the sensor is enabled
         self._enabled = True
+
+        # set the latency
+        if latency is None:
+            latency = 0
+        if not isinstance(latency, (int, float)):
+            raise TypeError("Expecting the given 'latency' to be an int or float, instead got: "
+                            "{}".format(type(latency)))
+        if latency < 0:
+            raise ValueError("Expecting the given 'latency' to be a positive number, but got instead: "
+                             "{}".format(latency))
         self._latency = latency
+        self._latent_cnt = -1
+
+        # data from last acquisition
+        self._data = None
+        self._latent_data = None  # self._sense()
 
     ##############
     # Properties #
@@ -77,28 +130,42 @@ class Sensor(object):  # sensor attached to a link or joint
         """Return the simulator instance."""
         return self.sim
 
-    @property
-    def position(self):
-        """
-        Return the body's CoM position in the Cartesian world frame.
-        """
-        position = self.sim.get_base_position(self.body_id)
-        position += self.local_position
-        return position
+    # @property
+    # def position(self):
+    #     """Return the body's base position in the Cartesian world frame."""
+    #     position = self.sim.get_base_position(self.body_id)
+    #     position += self.local_position
+    #     return position
+    #
+    # @property
+    # def orientation(self):
+    #     """Return the body's base orientation in the Cartesian world frame."""
+    #     orientation = self.sim.get_base_orientation(self.body_id)
+    #     orientation = get_quaternion_product(orientation, self.local_orientation)
+    #     return orientation
 
     @property
-    def orientation(self):
-        """
-        Return the body's CoM orientation in the Cartesian world frame.
-        """
-        orientation = self.sim.get_base_orientation(self.body_id)
-        orientation = get_quaternion_product(self.local_orientation, orientation)
-        return orientation
+    def enabled(self):
+        """Return if the sensor is enabled or not."""
+        return self._enabled
+
+    @property
+    def disabled(self):
+        """Return if the sensor is disabled or not."""
+        return not self._enabled
 
     ###########
     # Methods #
     ###########
 
+    def reset(self):
+        """Reset sensor."""
+        self._latent_data = self._sense()
+
+    def clean(self):
+        """clean sensor values."""
+        pass
+
     def enable(self):
         """Enable the sensor."""
         self._enabled = True
@@ -108,18 +175,38 @@ class Sensor(object):  # sensor attached to a link or joint
         self._enabled = False
 
     @abstractmethod
-    def _sense(self):
-        """Sense method to be implemented in the child class."""
+    def _sense(self, apply_noise=True):
+        """Sense method to be implemented in the child class. This has to apply the noise if you wish to have
+        noisy data.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+
+        Returns:
+            np.array: sensed data.
+        """
         raise NotImplementedError
 
-    def sense(self):
-        """Get the next sensor value."""
-        self.cnt += 1
-        if self.cnt % self.rate == 0:
-            self.data = self._sense()
-            self.cnt = 0
-            return self.data
-        return self.data
+    def sense(self, apply_noise=True):
+        """Get the next sensor value.
+
+        Args:
+            apply_noise (bool): if we should apply the noise or not. Note that the sensor might already have some noise.
+        """
+        if self._enabled:
+            self._cnt += 1
+            if (self._cnt % self._ticks) == 0:
+                if self._latency == 0:  # if no latency
+                    self._data = self._sense()
+                    self._latent_data = self._data
+                else:  # if latency
+                    self._latent_cnt += 1
+                    if (self._latent_cnt % self._latency) == 0:
+                        self._data = self._latent_data
+                        self._latent_data = self._sense(apply_noise=apply_noise)
+                        self._latent_cnt = 0
+                self._cnt = 0
+        return self._data
 
     #############
     # Operators #
@@ -141,7 +228,8 @@ class Sensor(object):  # sensor attached to a link or joint
     def __copy__(self):
         """Return a shallow copy of the sensor. This can be overridden in the child class."""
         return self.__class__(simulator=self.simulator, body_id=self.body_id, position=self.local_position,
-                              orientation=self.local_orientation, rate=self.rate)
+                              orientation=self.local_orientation, noise=self._noise, ticks=self._ticks,
+                              latency=self._latency)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the sensor. This can be overridden in the child class.
@@ -155,7 +243,8 @@ class Sensor(object):  # sensor attached to a link or joint
         body_id = copy.deepcopy(self.body_id)
         position = copy.deepcopy(self.local_position)
         orientation = copy.deepcopy(self.local_orientation)
+        noise = copy.deepcopy(self._noise)
         sensor = self.__class__(simulator=simulator, body_id=body_id, position=position, orientation=orientation,
-                                rate=self.rate)
+                                noise=noise, ticks=self._ticks, latency=self._latency)
         memo[self] = sensor
         return sensor

pyrobolearn/robots/walkman.py

@@ -64,16 +64,16 @@ class Walkman(BipedRobot, BiManipulator):
         # fovx = 1.3962634rad = 80 degrees, Gaussian noise = N(0, 0.007)
         # "left_camera_frame",
         self.left_camera = CameraSensor(self.sim, self.id, 11, width=800, height=800, fovy=80, near=0.02, far=300,
-                                        rate=30)  # 11
+                                        ticks=30)  # 11
         self.right_camera = CameraSensor(self.sim, self.id, 13, width=800, height=800, fovy=80, near=0.02, far=300,
-                                         rate=30)  # 13
+                                         ticks=30)  # 13
 
         # Laser (depth) sensor: Hokuyo sensor
         # link: "head_hokuyo_frame"
         # freq=40, samples = 720, angle = [-1.570796, 1.570796] rad, range = [0.10, 30.0] m
         # Gaussian noise: N(0.0, 0.01)
         self.depth_camera = CameraSensor(self.sim, self.id, 10, width=200, height=200, fovy=80, near=0.1, far=30.0,
-                                         rate=40)
+                                         ticks=40)
 
         self.cameras = [self.left_camera, self.right_camera, self.depth_camera]
 

pyrobolearn/simulators/bullet.py

@@ -1705,8 +1705,8 @@ class Bullet(Simulator):
                 using forward kinematics.
 
         Returns:
-            np.array[3]: Cartesian position of CoM
-            np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
+            np.array[3]: Cartesian world position of CoM
+            np.array[4]: Cartesian world orientation of CoM, in quaternion [x,y,z,w]
             np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
             np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
                 frame

pyrobolearn/simulators/middlewares/README.rst

@@ -0,0 +1,7 @@
+Middlewares
+===========
+
+This folder provides interfaces to the middlewares that are used in robotics (such as ROS, YARP, etc). All these
+classes inherit from the ``Middleware`` abstract class. Middlewares can be provided to simulators which can then use
+them to send/receive messages.
+

pyrobolearn/simulators/middlewares/middleware.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Define the abstract middleware API.
+
+Dependencies in PRL:
+* NONE
+"""
+
+# TODO
+import os
+import subprocess
+import psutil
+import signal
+import importlib
+import inspect
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class MiddleWare(object):
+    r"""Middleware (abstract) class
+
+    Middlewares can be provided to simulators which can then use them to send/receive messages.
+    """
+    pass

pyrobolearn/simulators/middlewares/ros.py

@@ -0,0 +1,262 @@
+#!/usr/bin/env python
+"""Define the ROS middleware API.
+
+Dependencies in PRL:
+* `pyrobolearn.simulators.simulator.Simulator`
+"""
+
+
+# TODO
+import os
+import subprocess
+import psutil
+import signal
+import importlib
+import inspect
+
+from pyrobolearn.simulators.simulator import Simulator
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["ROS (Willow Garage)", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+# TODO: maybe I should inherit from MiddleWare instead of Simulator... Then we can give these MiddleWare to different
+#  simulators. Other communication middleware layer includes YARP, etc.
+
+class ROS(Simulator):
+    r"""ROS Interface
+    """
+
+    def __init__(self, subscribe=False, publish=False, teleoperate=False, master_uri=11311, **kwargs):
+        super(ROS, self).__init__(render=False)
+
+        # Environment variable
+        self.env = os.environ.copy()
+        self.env["ROS_MASTER_URI"] = "http://localhost:" + str(master_uri)
+
+        # this is for the rospy methods such as: wait_for_service(), init_node(), ...
+        os.environ['ROS_MASTER_URI'] = self.env['ROS_MASTER_URI']
+
+        # run ROS core if not already running
+        self.roscore = None
+        if "roscore" not in [p.name() for p in psutil.process_iter()]:
+            # subprocess.Popen("roscore", env=self.env)
+            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.count_id = -1
+
+    def close(self):
+        """
+        Close everything
+        """
+        # delete each subscribers
+
+        # delete each publishers
+
+        # delete ROS
+        if self.roscore is not None:
+            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)
+
+    @property
+    def is_subscribing(self):
+        """Return True if we are subscribing to topics."""
+        return self.subscribe
+
+    @property
+    def is_publishing(self):
+        """Return True if we are publishing to topics."""
+        return self.publish
+
+    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.
+
+        The load_urdf will send a command to the physics server to load a physics model from a Universal Robot
+        Description File (URDF). The URDF file is used by the ROS project (Robot Operating System) to describe robots
+        and other objects, it was created by the WillowGarage and the Open Source Robotics Foundation (OSRF).
+        Many robots have public URDF files, you can find a description and tutorial here:
+        http://wiki.ros.org/urdf/Tutorials
+
+        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]
+            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
+                body for each link, and constraints between those rigid bodies are used to model joints.
+            use_fixed_base (bool): force the base of the loaded object to be static
+            flags (int): URDF_USE_INERTIA_FROM_FILE (val=2): by default, Bullet recomputed the inertia tensor based on
+                mass and volume of the collision shape. If you can provide more accurate inertia tensor, use this flag.
+                URDF_USE_SELF_COLLISION (val=8): by default, Bullet disables self-collision. This flag let's you
+                enable it.
+                You can customize the self-collision behavior using the following flags:
+                    * URDF_USE_SELF_COLLISION_EXCLUDE_PARENT (val=16) will discard self-collision between links that
+                        are directly connected (parent and child).
+                    * URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS (val=32) will discard self-collisions between a
+                        child link and any of its ancestors (parents, parents of parents, up to the base).
+                    * URDF_USE_IMPLICIT_CYLINDER (val=128), will use a smooth implicit cylinder. By default, Bullet
+                        will tessellate the cylinder into a convex hull.
+            scale (float): scale factor to the URDF model.
+
+        Returns:
+            int (non-negative): unique id associated to the load model.
+        """
+        id_ = self.count_id
+        self.count_id += 1
+
+        # get path to directory of urdf
+        path = os.path.dirname(os.path.abspath(filename))           # /path/to/pyrobolearn/robots/urdfs/<robot>/
+        robot_directory_name = path.split('/')[-1]                  # <robot>
+        path = path + '/../../ros/' + robot_directory_name + '/'    # /path/to/pyrobolearn/robots/ros/<robot>/
+
+        # check if valid robot directory
+        # if os.path.isdir(path):
+        robot_path = '/'.join(path.split('/')[-5:-2])
+        if robot_path == 'pyrobolearn/robots/urdfs':
+
+            # get corresponding subscriber/publisher
+            def check_ros(name, dictionary, id_):
+                # TODO: do I really need a class for each robot? Can I not just use RobotPublisher?
+                # if os.path.isfile(path + name + '.py'):
+                #     module = importlib.import_module('pyrobolearn.robots.ros.' + robot_directory_name + '.' + name)
+                #     classes = inspect.getmembers(module, inspect.isclass)  # list of (name, class)
+                #     length = len(name)
+                #     robot_name = ''.join(robot_directory_name.split('_'))
+                #
+                #     # go through each class and get the :attr:`name` corresponding to the robot and add it to the
+                #     # given :attr:`dictionary`
+                #     for name, cls in classes:
+                #         if name[:-length].lower() == robot_name:
+                #             dictionary[id_] = cls(id_=id_)
+                #             break
+                module = importlib.import_module('pyrobolearn.robots.ros.' + name)
+                classes = dict(inspect.getmembers(module, inspect.isclass))
+                cls = classes['Robot' + name.capitalize()]
+                dictionary[id_] = cls(name=robot_directory_name, id_=id_)
+
+            # load subscriber in simulator
+            if self.subscribe:
+                check_ros('subscriber', self.subscribers, id_)
+
+            # load publisher in simulator
+            if self.publish:
+                check_ros('publisher', self.publishers, id_)
+
+        return id_
+
+    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 of 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]
+        """
+        if body_id in self.subscribers:
+            return self.subscribers[body_id].get_joint_positions(joint_ids)
+
+    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 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.
+        """
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_positions(joint_ids, positions)
+            self.publishers[body_id].publish('joint_states')
+
+    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 of 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]
+        """
+        if body_id in self.subscribers:
+            return self.subscribers[body_id].get_joint_velocities(joint_ids)
+
+    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 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
+        """
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_velocities(joint_ids, velocities)
+            self.publishers[body_id].publish('joint_states')
+
+    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 of 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]
+        """
+        if body_id in self.subscribers:
+            return self.subscribers[body_id].get_joint_torques(joint_ids)
+
+    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 of int): joint id, or list of joint ids.
+            torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
+        """
+        if body_id in self.publishers:
+            self.publishers[body_id].set_joint_torques(joint_ids, torques)
+            self.publishers[body_id].publish('joint_states')

pyrobolearn/simulators/ros.py

@@ -1,20 +1,8 @@
 #!/usr/bin/env python
-"""Define the Bullet Simulator API.
-
-This is the main interface that communicates with the PyBullet simulator [1]. By defining this interface, it allows to
-decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required by
-PyBullet. For instance, some methods in PyBullet do not accepts numpy arrays but only lists. The interface provided
-here makes the necessary conversions.
-
-The signature of each method defined here are inspired by [1,2] but in accordance with the PEP8 style guide [3].
+"""Define the ROS API.
 
 Dependencies in PRL:
 * `pyrobolearn.simulators.simulator.Simulator`
-
-References:
-    [1] PyBullet: https://pybullet.org
-    [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
-    [3] PEP8: https://www.python.org/dev/peps/pep-0008/
 """
 
 # TODO

pyrobolearn/simulators/simulator.py

@@ -1142,8 +1142,8 @@ class Simulator(object):
                 using forward kinematics.
 
         Returns:
-            np.array[3]: Cartesian position of CoM
-            np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
+            np.array[3]: Cartesian world position of CoM
+            np.array[4]: Cartesian world orientation of CoM, in quaternion [x,y,z,w]
             np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
             np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
                 frame

pyrobolearn/utils/transformation.py

@@ -753,9 +753,38 @@ def get_spatial_transformation_matrix(rotation, position):
 quat_converter = QuaternionNumpyConverter(convention=1)
 
 
+def get_rotated_point_from_quaternion(q, p, convention='xyzw'):
+    """
+    Return the rotated point due to the provided quaternion.
+
+    .. math:: P_{\text{rotated}} = q * P * q^{-1} = q * P * q'
+
+    where :math:`q` is the unit quaternion (to represents a proper rotation and thus the inverse :math:`q^{-1}` is
+    equal to the conjugate :math:`q' = \bar{q}`), and :math:`P` is the quaternion where its vector part is equal to
+    the 3D position of the point :math:`p` and its scalar part is 0.
+
+    Args:
+        q (np.array[4], quaternion.quaternion): quaternion (it doesn't have to be a unit quaternion)
+        p (np.array[3]): 3d point in space.
+        convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
+            'wxyz'.
+
+    Returns:
+        np.array[3]: rotated 3 point.
+    """
+    # TODO: fix this!
+    p_quat = np.array([p[0], p[1], p[2], 0])
+    q_inv = get_quaternion_inverse(q, convention=convention)
+    p_rot = get_quaternion_product(q, get_quaternion_product(p_quat, q_inv, convention=convention),
+                                   convention=convention)
+    return p_rot[:3]
+
+
 def get_quaternion_conjugate(q, convention='xyzw'):
     r"""Return the conjugate of the given quaternion; i.e. if the quaternion is given by q = [x,y,z,w] where [x,y,z]
-    is the vector part and
+    is the vector part and w is the scalar part, the conjugate is q'=[-x,-y,-z,w].
+
+    If the quaternion is a unit quaternion, the conjugate is equal to the inverse of that quaternion.
 
     Args:
         q (np.array[4], quaternion.quaternion): quaternion (it doesn't have to be a unit quaternion)
@@ -813,6 +842,13 @@ def get_quaternion_inverse(q, convention='xyzw'):
     """Return the inverse of the given quaternion.
 
     Note: the inverse of a quaternion is the conjugate of the quaternion divided by the square norm of that quaternion.
+    Thus for a unit quaternion, the inverse of a quaternion is equal to its conjugate.
+
+    This is given by:
+
+    .. math:: q^{-1} = \frac{\bar{q}}{||q||}
+
+    where :math:`\bar{q}` is the conjugate of the quaternion :math:`q`.
 
     Args:
         q (np.array[4], quaternion.quaternion): quaternion.
@@ -824,13 +860,13 @@ def get_quaternion_inverse(q, convention='xyzw'):
     """
     if isinstance(q, quaternion.quaternion):
         return q.inverse()
-    elif isinstance(q, Iterable):
+    elif isinstance(q, (np.ndarray, tuple, list)):
         if convention == 'xyzw':
             x, y, z, w = q
-            return np.array([-x, -y, -z, w])
+            return np.array([-x, -y, -z, w]) / np.linalg.norm(q)
         elif convention == 'wxyz':
             w, x, y, z = q
-            return np.array([w, -x, -y, -z])
+            return np.array([w, -x, -y, -z]) / np.linalg.norm(q)
         else:
             raise NotImplementedError("Asking for a convention that has not been implemented")
     else:
@@ -840,6 +876,9 @@ def get_quaternion_inverse(q, convention='xyzw'):
 def get_quaternion_product(q1, q2, convention='xyzw'):
     """Return the quaternion product between two quaternions.
 
+    The quaternion corresponding to the product :math:`R_1 R_2` where :math:`R_i` are rotation matrices is given by
+    :math:`q_1 * q_2`.
+
     Args:
         q1 (np.array[4], quaternion.quaternion): first quaternion
         q2 (np.array[4], quaternion.quaternion): second quaternion