Merge branch 'master' into fix-sphinx-automation

Author: myeatman-bdai

.github/CONTRIBUTORS.md

@@ -3,4 +3,4 @@ A number of people have contributed to this, and earlier, versions of this Toolb
 * Jesse Haviland, 2020 (part of the ropy project)
 * Luis Fernando Lara Tobar, 2008
 * Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (part of the robopy project)
-* Peter Corke
+* Peter Corke

.pre-commit-config.yaml

@@ -18,17 +18,16 @@ repos:
   rev: v4.5.0
   hooks:
   -   id: end-of-file-fixer
-  -   id: debug-statements # Ensure we don't commit `import pdb; pdb.set_trace()`
       exclude: |
-        (?x)^(
-          docker/ros/web/static/.*|
-        )$
+        (?x)(
+          ^docs/
+        )
+  -   id: debug-statements # Ensure we don't commit `import pdb; pdb.set_trace()`
   -   id: trailing-whitespace
       exclude: |
-        (?x)^(
-          docker/ros/web/static/.*|
-          (.*/).*\.patch|
-        )$
+        (?x)(
+          ^docs/
+        )
 # - repo: https://github.com/pre-commit/mirrors-mypy
 #   rev: v1.6.1
 #   hooks:

Makefile

@@ -39,4 +39,3 @@ clean: .FORCE
 	(cd docs; make clean)
 	-rm -r *.egg-info
 	-rm -r dist build
-

pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "spatialmath-python"
-version = "1.1.13"
+version = "1.1.14"
 authors = [
   { name="Peter Corke", email="[email protected]" },
 ]

spatialmath/base/README.md

@@ -21,9 +21,9 @@ import spatialmath as sm
 
 R = sm.SO3.Rx(30, 'deg')
 print(R)
-   1         0         0          
-   0         0.866025 -0.5        
-   0         0.5       0.866025   
+   1         0         0
+   0         0.866025 -0.5
+   0         0.5       0.866025
 ```
 which constructs a rotation about the x-axis by 30 degrees.
 
@@ -45,7 +45,7 @@ array([[ 1.        ,  0.        ,  0.        ],
        [ 0.        ,  0.29552021,  0.95533649]])
 
 >>> rotx(30, unit='deg')
-Out[438]: 
+Out[438]:
 array([[ 1.       ,  0.       ,  0.       ],
        [ 0.       ,  0.8660254, -0.5      ],
        [ 0.       ,  0.5      ,  0.8660254]])
@@ -64,7 +64,7 @@ We also support multiple ways of passing vector information to functions that re
 
 ```
 transl2(1, 2)
-Out[442]: 
+Out[442]:
 array([[1., 0., 1.],
        [0., 1., 2.],
        [0., 0., 1.]])
@@ -74,13 +74,13 @@ array([[1., 0., 1.],
 
 ```
 transl2( [1,2] )
-Out[443]: 
+Out[443]:
 array([[1., 0., 1.],
        [0., 1., 2.],
        [0., 0., 1.]])
 
 transl2( (1,2) )
-Out[444]: 
+Out[444]:
 array([[1., 0., 1.],
        [0., 1., 2.],
        [0., 0., 1.]])
@@ -90,7 +90,7 @@ array([[1., 0., 1.],
 
 ```
 transl2( np.array([1,2]) )
-Out[445]: 
+Out[445]:
 array([[1., 0., 1.],
        [0., 1., 2.],
        [0., 0., 1.]])
@@ -129,7 +129,7 @@ Using classes ensures type safety, for example it stops us mixing a 2D homogeneo
 These classes are all derived from two parent classes:
 
 * `RTBPose` which provides common functionality for all
-* `UserList` which provdides the ability to act like a list 
+* `UserList` which provdides the ability to act like a list
 
 The latter is important because frequnetly in robotics we want a sequence, a trajectory, of rotation matrices or poses.  However a list of these items has the type `list` and the elements are not enforced to be homogeneous, ie. a list could contain a mixture of classes.
 
@@ -178,7 +178,7 @@ Out[259]: int
 
 a = T[1,1]
 a
-Out[256]: 
+Out[256]:
 cos(theta)
 type(a)
 Out[255]: cos
@@ -226,10 +226,3 @@ TypeError: can't convert expression to float
 | t2r | yes |
 | rotx | yes |
 | rotx | yes |
-
-
-
-
-
-
-

spatialmath/base/argcheck.py

@@ -5,7 +5,7 @@
 
 """
 Utility functions for testing and converting passed arguments.  Used in all
-spatialmath functions and classes to provides for flexibility in argument types 
+spatialmath functions and classes to provides for flexibility in argument types
 that can be passed.
 """
 

spatialmath/base/symbolic.py

@@ -8,7 +8,7 @@
 Symbolic arguments.
 
 If SymPy is not installed then only the standard numeric operations are
-supported. 
+supported.
 """
 
 import math

spatialmath/base/transforms3d.py

@@ -43,7 +43,7 @@
     tr2rt,
     Ab2M,
 )
-from spatialmath.base.quaternions import r2q, q2r, qeye, qslerp
+from spatialmath.base.quaternions import r2q, q2r, qeye, qslerp, qunit
 from spatialmath.base.graphics import plotvol3, axes_logic
 from spatialmath.base.animate import Animate
 import spatialmath.base.symbolic as sym
@@ -1675,7 +1675,7 @@ def trinterp(start, end, s, shortest=True):
             q1 = r2q(end)
             qr = qslerp(q0, q1, s, shortest=shortest)
 
-        return q2r(qr)
+        return q2r(qunit(qr))
 
     elif ismatrix(end, (4, 4)):
         # SE(3) case
@@ -1697,7 +1697,7 @@ def trinterp(start, end, s, shortest=True):
             qr = qslerp(q0, q1, s, shortest=shortest)
             pr = p0 * (1 - s) + s * p1
 
-        return rt2tr(q2r(qr), pr)
+        return rt2tr(q2r(qunit(qr)), pr)
     else:
         return ValueError("Argument must be SO(3) or SE(3)")
 

spatialmath/base/transformsNd.py

@@ -514,7 +514,7 @@ def skew(v):
     if len(v) == 1:
         # fmt: off
         return np.array([
-            [0.0,  -v[0]], 
+            [0.0,  -v[0]],
             [v[0],  0.0]
         ])  # type: ignore
         # fmt: on

spatialmath/geom3d.py

@@ -507,7 +507,7 @@ def vec(self) -> R6:
     def skew(self) -> R4x4:
         r"""
         Line as a Plucker skew-symmetric matrix
-        
+
         :return: Skew-symmetric matrix form of Plucker coordinates
         :rtype: ndarray(4,4)
 
@@ -523,7 +523,7 @@ def skew(self) -> R4x4:
                 -\omega_x & -\omega_y & -\omega_z & 0 \end{bmatrix}
 
         .. note::
-            
+
             - For two homogeneous points P and Q on the line, :math:`PQ^T-QP^T` is
             also skew symmetric.
             - The projection of Plucker line by a perspective camera is a

spatialmath/spatialvector.py

@@ -10,7 +10,7 @@
    :top-classes: collections.UserList
    :parts: 1
 
-.. note:: Compared to Featherstone's papers these spatial vectors have the 
+.. note:: Compared to Featherstone's papers these spatial vectors have the
     translational components first, followed by rotational components.
 """
 

tests/test_pose3d.py

@@ -1389,6 +1389,46 @@ def test_rtvec(self):
         nt.assert_equal(rvec, [0, 1, 0])
         nt.assert_equal(tvec, [2, 3, 4])
 
+    def test_interp(self):
+        # This data is taken from https://github.com/bdaiinstitute/spatialmath-python/issues/165
+        se3_1 = SE3()
+        se3_1.t = np.array(
+            [0.5705748101710814, 0.29623210833184527, 0.10764106509086407]
+        )
+        se3_1.R = np.array(
+            [
+                [0.2852875203191073, 0.9581330588259315, -0.024332536551692617],
+                [0.9582072394229962, -0.28568756930438033, -0.014882844564011068],
+                [-0.021211248608609852, -0.019069722856395098, -0.9995931315303468],
+            ]
+        )
+        assert SE3.isvalid(se3_1.A)
+
+        se3_2 = SE3()
+        se3_2.t = np.array(
+            [0.5150284150005691, 0.25796537207802533, 0.1558725490743694]
+        )
+        se3_2.R = np.array(
+            [
+                [0.42058255728234184, 0.9064420651629983, -0.038380919906699236],
+                [0.9070822373513454, -0.4209501599465646, -0.0016665901233428627],
+                [-0.01766712176680449, -0.0341137119645545, -0.9992617912561634],
+            ]
+        )
+        assert SE3.isvalid(se3_2.A)
+
+        path_se3 = se3_1.interp(end=se3_2, s=15, shortest=False)
+
+        angle = None
+        for i in range(len(path_se3) - 1):
+            assert SE3.isvalid(path_se3[i].A)
+
+            if angle is None:
+                angle = path_se3[i].angdist(path_se3[i + 1])
+            else:
+                test_angle = path_se3[i].angdist(path_se3[i + 1])
+                assert abs(test_angle - angle) < 1e-6
+
 
 # ---------------------------------------------------------------------------------------#
 if __name__ == "__main__":