Merge branch 'future' of github.com:petercorke/robotics-toolbox-python into future

# Conflicts:
#	roboticstoolbox/models/DH/AL5D.py

Author: petercorke

.github/workflows/future_testing.yml

@@ -11,7 +11,7 @@ jobs:
     strategy:
       matrix:
         os: [windows-latest, ubuntu-latest, macos-latest]
-        python-version: [3.7, 3.8, 3.9]
+        python-version: [3.7, 3.8, 3.9, '3.10']
 
     steps:
     - name: Set up Python ${{ matrix.python-version }}
@@ -28,18 +28,21 @@ jobs:
     - name: Checkout Swift
       uses: actions/checkout@v2
       with:
+        ref: future
         repository: jhavl/swift
         path: swift
 
     - name: Checkout Spatialmath
       uses: actions/checkout@v2
       with:
+        ref: future
         repository: petercorke/spatialmath-python
         path: sm
 
     - name: Checkout Spatialgeometry
       uses: actions/checkout@v2
       with:
+        ref: future
         repository: jhavl/spatialgeometry
         path: sg
 

roboticstoolbox/core/linalg.cpp

@@ -150,9 +150,9 @@ extern "C"
             else
             {
                 // a = np.pi / 2 * (np.diag(R) + 1);
-                e[3] = M_PI_2 * (R[0] + 1);
-                e[4] = M_PI_2 * (R[4] + 1);
-                e[5] = M_PI_2 * (R[8] + 1);
+                e[3] = PI_2 * (R[0] + 1);
+                e[4] = PI_2 * (R[4] + 1);
+                e[5] = PI_2 * (R[8] + 1);
             }
         }
         else

roboticstoolbox/core/linalg.h

@@ -15,6 +15,8 @@ extern "C"
 {
 #endif /* __cplusplus */
 
+#define PI_2 1.57079632679489661923132169163975144
+
 #define Matrix4dc Eigen::Matrix4d
 #define Matrix4dr Eigen::Matrix<double, 4, 4, Eigen::RowMajor>
 

roboticstoolbox/models/DH/AL5D.py

@@ -6,6 +6,7 @@
 from roboticstoolbox import DHRobot, RevoluteMDH
 from spatialmath import SE3
 
+
 class AL5D(DHRobot):
     """
     Class that models a Lynxmotion AL5D manipulator
@@ -41,48 +42,50 @@ def __init__(self, symbolic=False):
 
         if symbolic:
             import spatialmath.base.symbolic as sym
+
             zero = sym.zero()
             pi = sym.pi()
         else:
             from math import pi
+
             zero = 0.0
 
         # robot length values (metres)
         a = [0, 0.002, 0.14679, 0.17751]
         d = [-0.06858, 0, 0, 0]
 
-        alpha = [pi, pi/2, pi, pi]
-        offset = [pi/2, pi, -0.0427, -0.0427-pi/2]
+        alpha = [pi, pi / 2, pi, pi]
+        offset = [pi / 2, pi, -0.0427, -0.0427 - pi / 2]
 
         # mass data as measured
         mass = [0.187, 0.044, 0.207, 0.081]
 
         # center of mass as calculated through CAD model
         center_of_mass = [
-                [0.01724, -0.00389,  0.00468],
-                [0.07084,  0.00000,  0.00190],
-                [0.05615, -0.00251, -0.00080],
-                [0.04318,  0.00735, -0.00523],
-            ]
+            [0.01724, -0.00389, 0.00468],
+            [0.07084, 0.00000, 0.00190],
+            [0.05615, -0.00251, -0.00080],
+            [0.04318, 0.00735, -0.00523],
+        ]
 
         # moments of inertia are practically zero
         moments_of_inertia = [
-                [0, 0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0, 0]
-            ]
+            [0, 0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0, 0],
+        ]
 
         joint_limits = [
-                [-pi/2, pi/2],
-                [-pi/2, pi/2],
-                [-pi/2, pi/2],
-                [-pi/2, pi/2],
-            ]
+            [-pi / 2, pi / 2],
+            [-pi / 2, pi / 2],
+            [-pi / 2, pi / 2],
+            [-pi / 2, pi / 2],
+        ]
 
         links = []
 
-        for j in range(4):
+        for j in range(3):
             link = RevoluteMDH(
                 d=d[j],
                 a=a[j],
@@ -92,27 +95,28 @@ def __init__(self, symbolic=False):
                 I=moments_of_inertia[j],
                 G=1,
                 B=0,
-                Tc=[0,0],
-                qlim=joint_limits[j]
+                Tc=[0, 0],
+                qlim=joint_limits[j],
             )
             links.append(link)
-            
-        tool=SE3(0.07719,0,0)
-    
+
+        tool = SE3(0.07719, 0, 0)
+
         super().__init__(
             links,
             name="AL5D",
             manufacturer="Lynxmotion",
-            keywords=('dynamics', 'symbolic'),
+            keywords=("dynamics", "symbolic"),
             symbolic=symbolic,
-            tool=tool
+            tool=tool,
         )
-    
+
         # zero angles
-        self.addconfiguration("home", np.array([pi/2, pi/2, pi/2, pi/2]))
+        self.addconfiguration("home", np.array([pi / 2, pi / 2, pi / 2, pi / 2]))
+
 
-if __name__ == '__main__':    # pragma nocover
+if __name__ == "__main__":  # pragma nocover
 
     al5d = AL5D(symbolic=False)
     print(al5d)
-    print(al5d.dyntable())
+    # print(al5d.dyntable())

roboticstoolbox/robot/DHRobot.py

@@ -1235,6 +1235,9 @@ def jacob0_analytical(self, q, representation=None, T=None):
         # compute Jacobian in world frame
         J0 = self.jacob0(q, T)
 
+        if representation is None:
+            return J0
+
         # compute rotational transform if analytical Jacobian required
 
         if representation == "rpy/xyz":
@@ -1385,10 +1388,10 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False):
         """
 
         if base_wrench:
-            return self.rne_python(q, qd, qdd, 
-                                gravity=gravity, fext=fext,
-                                base_wrench=base_wrench)
-        
+            return self.rne_python(
+                q, qd, qdd, gravity=gravity, fext=fext, base_wrench=base_wrench
+            )
+
         trajn = 1
 
         try:

roboticstoolbox/robot/ETS.py

@@ -6,7 +6,6 @@
 """
 
 from collections import UserList
-from functools import lru_cache, cached_property
 from numpy import (
     pi,
     where,
@@ -44,9 +43,19 @@
 from spatialmath.base import getvector
 from spatialmath import SE3
 from typing import Union, overload, List, Set
+from sys import version_info
 
 ArrayLike = Union[list, ndarray, tuple, set]
 
+py_ver = version_info
+
+if version_info >= (3, 9):
+    from functools import cached_property
+
+    c_property = cached_property
+else:
+    c_property = property
+
 
 class BaseETS(UserList):
     def __init__(self, *args):
@@ -238,7 +247,7 @@ def jindex_set(self) -> Set[int]:  #
         """
         return set([self[j].jindex for j in self.joint_idx()])  # type: ignore
 
-    @cached_property
+    @c_property
     def jindices(self) -> ndarray:
         """
         Get an array of joint indices
@@ -255,7 +264,7 @@ def jindices(self) -> ndarray:
         """
         return array([self[j].jindex for j in self.joints()])  # type: ignore
 
-    @cached_property
+    @c_property
     def qlim(self):
         r"""
         Joint limits

tests/test_DHRobot.py

@@ -390,7 +390,7 @@ def test_isspherical(self):
     def test_payload(self):
         panda = rp.models.DH.Panda()
         nt.assert_array_almost_equal(panda.r[:, 6], np.zeros(3))
-        nt.assert_array_almost_equal(panda.links[6].m, 0)
+        # nt.assert_array_almost_equal(panda.links[6].m, 0)
 
         m = 6
         p = [1, 2, 3]
@@ -1140,8 +1140,8 @@ def test_inertia_x(self):
             [0.0000, -0.9652, -0.0000, 0.1941, 0.0000, 0.5791],
         ]
 
-        M0 = puma.inertia_x(q, representation="rpy/xyz")
-        M1 = puma.inertia_x(np.c_[q, q].T, representation="rpy/xyz")
+        M0 = puma.inertia_x(q, representation=None)
+        M1 = puma.inertia_x(np.c_[q, q].T, representation=None)
 
         nt.assert_array_almost_equal(M0, Mr, decimal=4)
         nt.assert_array_almost_equal(M1[0, :, :], Mr, decimal=4)
@@ -1252,7 +1252,7 @@ def test_jacob_dot(self):
 
         from roboticstoolbox.tools import hessian_numerical
 
-        j0 = puma.jacob_dot(q, qd)
+        j0 = puma.jacob0_dot(q, qd)
 
         H = hessian_numerical(lambda q: puma.jacob0(q), q)
         Jd = np.zeros((6, puma.n))

tests/test_jacob.py

@@ -42,28 +42,36 @@ def test_jacob_analytical_eul(self):
         rep = "eul"
         q = self.q
         Ja = numjac(lambda q: tr2x(self.robot.fkine(q).A, representation=rep), q)
-        nt.assert_array_almost_equal(self.robot.jacob0_analytic(q, analytic=rep), Ja)
+        nt.assert_array_almost_equal(
+            self.robot.jacob0_analytical(q, representation=rep), Ja
+        )
 
     def test_jacob_analytical_rpy_xyz(self):
         rep = "rpy/xyz"
         q = self.q
         Ja = numjac(lambda q: tr2x(self.robot.fkine(q).A, representation=rep), q)
-        nt.assert_array_almost_equal(self.robot.jacob0_analytic(q, analytic=rep), Ja)
+        nt.assert_array_almost_equal(
+            self.robot.jacob0_analytical(q, representation=rep), Ja
+        )
 
     def test_jacob_analytical_rpy_zyx(self):
         rep = "rpy/zyx"
         q = self.q
         Ja = numjac(lambda q: tr2x(self.robot.fkine(q).A, representation=rep), q)
-        nt.assert_array_almost_equal(self.robot.jacob0_analytic(q, analytic=rep), Ja)
+        nt.assert_array_almost_equal(
+            self.robot.jacob0_analytical(q, representation=rep), Ja
+        )
 
     def test_jacob_analytical_exp(self):
         rep = "exp"
         q = self.q
         Ja = numjac(lambda q: tr2x(self.robot.fkine(q).A, representation=rep), q)
-        nt.assert_array_almost_equal(self.robot.jacob0_analytic(q, analytic=rep), Ja)
+        nt.assert_array_almost_equal(
+            self.robot.jacob0_analytical(q, representation=rep), Ja
+        )
 
     def test_jacob_dot(self):
-        j0 = self.robot.jacob_dot(self.q, self.qd)
+        j0 = self.robot.jacob0_dot(self.q, self.qd)
 
         H = numhess(lambda q: self.robot.jacob0(q), self.q)
         Jd = np.zeros((6, self.robot.n))
@@ -74,9 +82,11 @@ def test_jacob_dot(self):
 
     def test_jacob_dot_analytical_eul(self):
         rep = "eul"
-        j0 = self.robot.jacob_dot(self.q, self.qd, analytical=rep)
+        j0 = self.robot.jacob0_dot(self.q, self.qd, representation=rep)
 
-        H = numhess(lambda q: self.robot.jacob0_analytic(q, analytic=rep), self.q)
+        H = numhess(
+            lambda q: self.robot.jacob0_analytical(q, representation=rep), self.q
+        )
         Jd = np.zeros((6, self.robot.n))
         for i in range(self.robot.n):
             Jd += H[i, :, :] * self.qd[i]