Added gyroscope and accelerometer sensors (stepjam#86)

* Add distance acquisition method to ProximitySensor

* List of changes:
* add get_base_velocities method to NonHolomicBase
* change name of the measure method from ProximitySensor

* List of changes:
* created sensors package
* gyroscope sensor added
* accelerometer sensor added
* added functions to vrep.py

* Added unit tests for accelerometer, gyroscope and read method of proximity sensor

pyrep/backend/sim.py

@@ -1022,3 +1022,32 @@ def simUngroupShape(shapeHandle):
     handles = [shapes[i] for i in range(count[0])]
     # simReleaseBuffer(shapes)
     return handles
+
+
+def simInvertMatrix(matrix):
+    c_matrix = ffi.new('float []', matrix)
+    ret = lib.simInvertMatrix(c_matrix)
+    _check_return(ret)
+    return list(c_matrix)
+
+
+def simMultiplyMatrices(inMatrix1, inMatrix2):
+    outMatrix = ffi.new('float []', len(inMatrix1))
+    ret = lib.simMultiplyMatrices(inMatrix1, inMatrix2, outMatrix)
+    _check_return(ret)
+    _check_null_return(outMatrix)
+    return list(outMatrix)
+
+
+def simGetEulerAnglesFromMatrix(rotationMatrix):
+    eulerAngles = ffi.new('float [3]')
+    ret = lib.simGetEulerAnglesFromMatrix(rotationMatrix, eulerAngles)
+    _check_return(ret)
+    _check_null_return(eulerAngles)
+    return list(eulerAngles)
+
+
+def simGetSimulationTime():
+    time = lib.simGetSimulationTime()
+    _check_return(time)
+    return time

pyrep/objects/proximity_sensor.py

@@ -1,3 +1,5 @@
+from math import sqrt
+
 from pyrep.backend import sim
 from pyrep.objects.object import Object
 from pyrep.const import ObjectType
@@ -13,6 +15,18 @@ class ProximitySensor(Object):
     def _get_requested_type(self) -> ObjectType:
         return ObjectType.PROXIMITY_SENSOR
 
+    def read(self) -> float:
+        """Read the distance between sensor and first detected object. If
+        there is no detected object returns -1.0. It can be considered as
+        maximum measurable distance of the sensor.
+
+        :return: Float distance to the first detected object
+        """
+        state, _, points, _ = sim.simReadProximitySensor(self._handle)
+        if state:
+            return sqrt(points[0] ** 2 + points[1] ** 2 + points[2] ** 2)
+        return -1.0
+
     def is_detected(self, obj: Object) -> bool:
         """Checks whether the proximity sensor detects the indicated object.
 

pyrep/robots/mobiles/nonholonomic_base.py

@@ -1,10 +1,14 @@
-from pyrep.robots.mobiles.mobile_base import MobileBase
-from pyrep.robots.configuration_paths.nonholonomic_configuration_path import (
-    NonHolonomicConfigurationPath)
+from math import atan2
+from math import cos
+from math import sin
+from math import sqrt
+from typing import List
+
 from pyrep.const import ConfigurationPathAlgorithms as Algos
 from pyrep.errors import ConfigurationPathError
-from typing import List
-from math import sqrt, atan2, sin, cos
+from pyrep.robots.configuration_paths.nonholonomic_configuration_path import (
+    NonHolonomicConfigurationPath)
+from pyrep.robots.mobiles.mobile_base import MobileBase
 
 
 class NonHolonomicBase(MobileBase):
@@ -29,6 +33,21 @@ def __init__(self,
         self.Kd = 0.1
         self.desired_velocity = 0.05
 
+    def get_base_velocities(self) -> List[float]:
+        """Gets linear and angular velocities of the mobile robot base
+        calculated from kinematics equations. Left joint should have index 1,
+        right joint should have index.
+
+        :return: A list with linear and angular velocity of the robot base.
+        """
+        wv = self.get_joint_velocities()
+
+        lv = sum(wv) * self.wheel_radius / 2.0
+        v_diff = wv[1] - wv[0]
+        av = self.wheel_radius * v_diff / self.wheel_distance
+
+        return [lv, av]
+
     def get_linear_path(self, position: List[float],
                         angle=0) -> NonHolonomicConfigurationPath:
         """Initialize linear path and check for collision along it.

pyrep/sensors/accelerometer.py

@@ -0,0 +1,30 @@
+from typing import List
+
+from pyrep.backend import sim
+from pyrep.const import ObjectType
+from pyrep.objects.force_sensor import ForceSensor
+from pyrep.objects.object import Object
+from pyrep.objects.shape import Shape
+
+
+class Accelerometer(Object):
+    """An object able to measure accelerations that are applied to it.
+    """
+
+    def __init__(self, name):
+        super().__init__(name)
+        self._mass_object = Shape('%s_mass' % (self.get_name()))
+        self._sensor = ForceSensor('%s_force_sensor' % (self.get_name()))
+
+    def _get_requested_type(self) -> ObjectType:
+        return ObjectType(sim.simGetObjectType(self.get_handle()))
+
+    def read(self) -> List[float]:
+        """Reads the acceleration applied to accelerometer.
+
+        :return: A list containing applied accelerations along
+            the sensor's x, y and z-axes
+        """
+        forces, _ = self._sensor.read()
+        accel = [force / self._mass_object.get_mass() for force in forces]
+        return accel

pyrep/sensors/gyroscope.py

@@ -0,0 +1,38 @@
+from pyrep.backend import sim
+from pyrep.const import ObjectType
+from pyrep.objects.object import Object
+
+
+class Gyroscope(Object):
+    """An object able to measure angular velocities that are applied to it.
+    """
+    def __init__(self, name):
+        super().__init__(name)
+        self._ref = '%s_reference' % (self.get_name())
+
+        self._last_time = sim.simGetSimulationTime()
+        self._old_transformation_matrix = self.get_matrix()
+
+    def _get_requested_type(self) -> ObjectType:
+        return ObjectType(sim.simGetObjectType(self.get_handle()))
+
+    def read(self):
+        """Reads the angular velocities applied to gyroscope.
+
+        :return: A list containing applied angular velocities along
+            the sensor's x, y and z-axes.
+        """
+        current_time = sim.simGetSimulationTime()
+        dt = current_time - self._last_time
+
+        inv_old_matrix = sim.simInvertMatrix(self._old_transformation_matrix)
+        transformation_matrix = self.get_matrix()
+        mat = sim.simMultiplyMatrices(inv_old_matrix, transformation_matrix)
+        euler_angles = sim.simGetEulerAnglesFromMatrix(mat)
+
+        self._last_time = current_time
+        self._old_transformation_matrix = transformation_matrix
+
+        if dt != 0:
+            return [euler_angle / dt for euler_angle in euler_angles]
+        return [0.0, 0.0, 0.0]

setup.py

@@ -11,6 +11,7 @@
       packages=['pyrep',
                 'pyrep.backend',
                 'pyrep.objects',
+                'pyrep.sensors',
                 'pyrep.robots',
                 'pyrep.robots.arms',
                 'pyrep.robots.end_effectors',

tests/test_accelerometer.py

@@ -0,0 +1,18 @@
+import unittest
+from tests.core import TestCore
+from pyrep.sensors.accelerometer import Accelerometer
+
+
+class TestAccelerometer(TestCore):
+
+    def setUp(self):
+        super().setUp()
+        self.sensor = Accelerometer('accelerometer')
+
+    def test_read(self):
+        accelerations = self.sensor.read()
+        self.assertEqual(len(accelerations), 3)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/test_gyroscope.py

@@ -0,0 +1,18 @@
+import unittest
+from tests.core import TestCore
+from pyrep.sensors.gyroscope import Gyroscope
+
+
+class TestGyroscope(TestCore):
+
+    def setUp(self):
+        super().setUp()
+        self.sensor = Gyroscope('gyroscope')
+
+    def test_read(self):
+        angular_velocities = self.sensor.read()
+        self.assertEqual(len(angular_velocities), 3)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/test_proximity_sensors.py

@@ -10,6 +10,12 @@ def setUp(self):
         super().setUp()
         self.sensor = ProximitySensor('proximity_sensor')
 
+    def test_read(self):
+        self.pyrep.step()
+        distance = self.sensor.read()
+        self.pyrep.step()
+        self.assertAlmostEqual(distance, 0.1)
+
     def test_is_detected(self):
         ob1 = Shape('simple_model')
         ob2 = Shape('dynamic_cube')