API change. Get/set matrix now requires/expects 4x4 numpy array.

Author: stepjam

.github/workflows/build.yml

@@ -27,6 +27,7 @@ jobs:
       # Runs a set of commands using the runners shell
       - name: Run a multi-line script
         run: |
+          sudo apt-get update -qq
           sudo apt-get install -y xvfb qtbase5-dev qtdeclarative5-dev libqt5webkit5-dev libsqlite3-dev qt5-default qttools5-dev-tools
           # start xvfb in the background
           sudo /usr/bin/Xvfb $DISPLAY -screen 0 1280x1024x24 &

pyrep/objects/object.py

@@ -287,39 +287,34 @@ def set_parent(self, parent_object: Union['Object', None],
         parent = -1 if parent_object is None else parent_object.get_handle()
         sim.simSetObjectParent(self._handle, parent, keep_in_place)
 
-    def get_matrix(self, relative_to=None) -> List[float]:
+    def get_matrix(self, relative_to=None) -> np.ndarray:
         """Retrieves the transformation matrix of this object.
 
         :param relative_to: Indicates relative to which reference frame we want
             the matrix. Specify None to retrieve the absolute transformation
             matrix, or an Object relative to whose reference frame we want the
             transformation matrix.
-        :return: A list of 12 float values (the last row of the 4x4 matrix (
-            0,0,0,1) is not needed).
-                The x-axis of the orientation component is (m[0], m[4], m[8])
-                The y-axis of the orientation component is (m[1], m[5], m[9])
-                The z-axis of the orientation component is (m[2], m[6], m[10])
-                The translation component is (m[3], m[7], m[11])
+        :return: A 4x4 transformation matrix.
         """
         relto = -1 if relative_to is None else relative_to.get_handle()
-        return sim.simGetObjectMatrix(self._handle, relto)
+        m = sim.simGetObjectMatrix(self._handle, relto)
+        m_np = np.array(m).reshape((3, 4))
+        return np.concatenate([m_np, [np.array([0, 0, 0, 1])]])
 
-    def set_matrix(self, matrix: List[float], relative_to=None) -> None:
+    def set_matrix(self, matrix: np.ndarray, relative_to=None) -> None:
         """Sets the transformation matrix of this object.
 
         :param relative_to: Indicates relative to which reference frame the
             matrix is specified. Specify None to set the absolute transformation
             matrix, or an Object relative to whose reference frame the
             transformation matrix is specified.
-        :param matrix: A list of 12 float values (the last row of the 4x4 matrix
-            (0,0,0,1) is not needed).
-                The x-axis of the orientation component is (m[0], m[4], m[8])
-                The y-axis of the orientation component is (m[1], m[5], m[9])
-                The z-axis of the orientation component is (m[2], m[6], m[10])
-                The translation component is (m[3], m[7], m[11])
+        :param matrix: A 4x4 transformation matrix.
         """
+        if not isinstance(matrix, np.ndarray):
+            raise ValueError('Expected Numpy 4x4 array.')
         relto = -1 if relative_to is None else relative_to.get_handle()
-        sim.simSetObjectMatrix(self._handle, relto, matrix)
+        sim.simSetObjectMatrix(
+            self._handle, relto, matrix[:3, :4].reshape((12)).tolist())
 
     def is_collidable(self) -> bool:
         """Whether the object is collidable or not.

pyrep/sensors/gyroscope.py

@@ -11,7 +11,8 @@ def __init__(self, name):
         self._ref = '%s_reference' % (self.get_name())
 
         self._last_time = sim.simGetSimulationTime()
-        self._old_transformation_matrix = self.get_matrix()
+        self._old_transformation_matrix = self.get_matrix()[:3, :4].reshape(
+            (12, )).tolist()
 
     def _get_requested_type(self) -> ObjectType:
         return ObjectType(sim.simGetObjectType(self.get_handle()))
@@ -26,7 +27,8 @@ def read(self):
         dt = current_time - self._last_time
 
         inv_old_matrix = sim.simInvertMatrix(self._old_transformation_matrix)
-        transformation_matrix = self.get_matrix()
+        transformation_matrix = self.get_matrix()[:3, :4].reshape(
+            (12, )).tolist()
         mat = sim.simMultiplyMatrices(inv_old_matrix, transformation_matrix)
         euler_angles = sim.simGetEulerAnglesFromMatrix(mat)
 

tests/test_objects.py

@@ -131,8 +131,11 @@ def test_get_parent_when_orphan(self):
         parent = self.dummy.get_parent()
         self.assertIsNone(parent)
 
-    def test_get_matrix(self):
-        self.assertEqual(len(self.dynamic_cube.get_matrix()), 12)
+    def test_get_set_matrix(self):
+        m = self.dynamic_cube.get_matrix()
+        self.assertEqual(m.shape, (4, 4))
+        self.simple_model.set_matrix(m)
+        self.assertListEqual(self.simple_model.get_matrix().tolist(), m.tolist())
 
     def test_get_set_collidable(self):
         self.dynamic_cube.set_collidable(False)