Merge pull request #77 from petercorke/xyz-properties

Add properties for x,y,z for SE3 and SE2 classes.

Author: myeatman-bdai

spatialmath/pose2d.py

@@ -528,6 +528,46 @@ def t(self):
         else:
             return np.array([x[:2, 2] for x in self.A])
 
+    @property
+    def x(self):
+        """
+        First element of the translational component of SE(2)
+
+        :param self: SE(2)
+        :type self: SE2 instance
+        :return: translational component
+        :rtype: float
+
+        ``v.x`` is the first element of the translational vector component.  If ``len(x)`` is:
+
+        - 1, return an float
+        - N>1, return an ndarray with shape=(N,)
+        """
+        if len(self) == 1:
+            return self.A[0, 2]
+        else:
+            return np.array([v[0, 2] for v in self.A])
+
+    @property
+    def y(self):
+        """
+        Second element of the translational component of SE(2)
+
+        :param self: SE(2)
+        :type self: SE2 instance
+        :return: translational component
+        :rtype: float
+
+        ``v.y`` is the second element of the translational vector component.  If ``len(x)`` is:
+
+        - 1, return an float
+        - N>1, return an ndarray with shape=(N,)
+        """
+        if len(self) == 1:
+            return self.A[1, 2]
+        else:
+            return np.array([v[1, 2] for v in self.A])
+
     def xyt(self):
         r"""
         SE(2) as a configuration vector

spatialmath/pose3d.py

@@ -1036,6 +1036,105 @@ def t(self, v: ArrayLike3):
         v = smb.getvector(v, 3)
         self.A[:3, 3] = v
 
+    @property
+    def x(self) -> float:
+        """
+        First element of translational component of SE(3)
+
+        :return: first element of translational component of SE(3)
+        :rtype: float
+
+        If ``len(v) > 1``, return an array with shape=(N,).
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import SE3
+            >>> v = SE3(1,2,3)
+            >>> v.x
+            >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
+            >>> v.x
+
+        :SymPy: supported
+        """
+        if len(self) == 1:
+            return self.A[0, 3]
+        else:
+            return np.array([v[0, 3] for v in self.A])
+
+    @x.setter
+    def x(self, x: float):
+        if len(self) > 1:
+            raise ValueError("can only assign elements to length 1 object")
+        self.A[0, 3] = x
+
+    @property
+    def y(self) -> float:
+        """
+        Second element of translational component of SE(3)
+
+        :return: second element of translational component of SE(3)
+        :rtype: float
+
+        If ``len(v) > 1``, return an array with shape=(N,).
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import SE3
+            >>> v = SE3(1,2,3)
+            >>> v.y
+            >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
+            >>> v.y
+
+        :SymPy: supported
+        """
+        if len(self) == 1:
+            return self.A[1, 3]
+        else:
+            return np.array([v[1, 3] for v in self.A])
+
+    @y.setter
+    def y(self, y: float):
+        if len(self) > 1:
+            raise ValueError("can only assign elements to length 1 object")
+        self.A[1, 3] = y
+
+    @property
+    def z(self) -> float:
+        """
+        Third element of translational component of SE(3)
+
+        :return: third element of translational component of SE(3)
+        :rtype: float
+
+        If ``len(v) > 1``, return an array with shape=(N,).
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import SE3
+            >>> v = SE3(1,2,3)
+            >>> v.z
+            >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
+            >>> v.z
+
+        :SymPy: supported
+        """
+        if len(self) == 1:
+            return self.A[2, 3]
+        else:
+            return np.array([v[2, 3] for v in self.A])
+
+    @z.setter
+    def z(self, z: float):
+        if len(self) > 1:
+            raise ValueError("can only assign elements to length 1 object")
+        self.A[2, 3] = z
+
     # ------------------------------------------------------------------------ #
 
     def inv(self) -> SE3:

tests/test_pose2d.py

@@ -390,6 +390,8 @@ def test_Rt(self):
         array_compare(TT1.A, T1)
         array_compare(TT1.R, R1)
         array_compare(TT1.t, t1)
+        self.assertEqual(TT1.x, t1[0])
+        self.assertEqual(TT1.y, t1[1])
 
         TT = SE2([TT1, TT1, TT1])
         array_compare(TT.t, [t1, t1, t1])

tests/test_pose3d.py

@@ -789,6 +789,20 @@ def test_constructor(self):
         nt.assert_equal(T.R, R)
         nt.assert_equal(T.t, t)
 
+        nt.assert_equal(T.x, t[0])
+        nt.assert_equal(T.y, t[1])
+        nt.assert_equal(T.z, t[2])
+
+        TT = SE3([T, T, T])
+        desired_shape = (3,)
+        nt.assert_equal(TT.x.shape, desired_shape)
+        nt.assert_equal(TT.y.shape, desired_shape)
+        nt.assert_equal(TT.z.shape, desired_shape)
+
+        ones = np.ones(desired_shape)
+        nt.assert_equal(TT.x, ones*t[0])
+        nt.assert_equal(TT.y, ones*t[1])
+        nt.assert_equal(TT.z, ones*t[2])
 
         # copy constructor
         R = SE3.Rx(pi / 2)