From 1f28014fc3c5b37aeea3fd697161e457ba039e51 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 12:54:13 -0400
Subject: [PATCH 01/11] Add BSpline_SE3 class

---
 spatialmath/base/animate.py |  8 ++--
 spatialmath/spline.py       | 75 +++++++++++++++++++++++++++++++++++++
 2 files changed, 80 insertions(+), 3 deletions(-)
 create mode 100644 spatialmath/spline.py

diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py
index 8efa3a4e..3876a2ea 100755
--- a/spatialmath/base/animate.py
+++ b/spatialmath/base/animate.py
@@ -212,6 +212,7 @@ def update(frame, animation):
                 # assume it is an SO(3) or SE(3)
                 T = frame
             # ensure result is SE(3)
+            
             if T.shape == (3, 3):
                 T = smb.r2t(T)
 
@@ -308,7 +309,7 @@ def __init__(self, anim: Animate, h, xs, ys, zs):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T.A @ self.p
             self.h.set_data(p[0, :], p[1, :])
             self.h.set_3d_properties(p[2, :])
 
@@ -365,7 +366,8 @@ def __init__(self, anim, h):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            # import ipdb; ipdb.set_trace()
+            p = T.A @ self.p
 
             # reshape it
             p = p[0:3, :].T.reshape(3, 2, 3)
@@ -419,7 +421,7 @@ def __init__(self, anim, h, x, y, z):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T.A @ self.p
             # x2, y2, _ = proj3d.proj_transform(
             #   p[0], p[1], p[2], self.anim.ax.get_proj())
             # self.h.set_position((x2, y2))
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
new file mode 100644
index 00000000..562fd2a0
--- /dev/null
+++ b/spatialmath/spline.py
@@ -0,0 +1,75 @@
+# Copyright (c) 2024 Boston Dynamics AI Institute LLC.
+# MIT Licence, see details in top-level file: LICENCE
+
+"""
+Classes for parameterizing a trajectory in SE3 with B-Splines. 
+
+Copies parts of the API from scipy's B-Spline class.
+"""
+
+from typing import Any
+from scipy.interpolate import BSpline
+from spatialmath import SE3
+import numpy as np
+import matplotlib.pyplot as plt
+from spatialmath.base.transforms3d import tranimate, trplot
+
+class BSpline_SE3:
+    """ 
+    """
+    def __init__(self, knots, control_poses: list[SE3], degree) -> None:
+        self.t = knots # knots 
+
+        # a matrix where each row is a control pose as a twist
+        # (so each column is a vector of control points for that dim of the twist)
+        self.control_pose_matrix = np.vstack([np.array(element.twist()) for element in control_poses]) 
+        
+        self.k = degree # degree of spline
+
+        self.splines = [ BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) ]
+
+    def __call__(self, x: float) -> Any:
+        """
+        x: Normalized time value to evaluate at.
+        """
+        twist = np.hstack([spline(x) for spline in self.splines])
+        return SE3.Exp(twist)
+    
+    def eval(self, x):
+        self.__call__(x)
+
+
+def main():
+    degree = 3
+    control_poses = [
+        SE3.Trans(
+            [e, 2*np.cos(e/2 * np.pi), 2*np.sin(e/2 * np.pi)]
+        )
+        *SE3.Ry(e/8 * np.pi) for e in range(1,9)
+    ]
+    # t =  np.linspace(0, 1, len(control_poses) + degree + 1)   
+    knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
+    knots=np.append([0,0,0],knots)
+    knots=np.append(knots,[1,1,1]) 
+    trajectory = BSpline_SE3(knots, control_poses, degree)
+    
+    out_poses = [trajectory(i) for i in np.linspace(0,1,100)]
+    x = [pose.x for pose in out_poses]
+    y = [pose.y for pose in out_poses]
+    z = [pose.z for pose in out_poses]
+    
+    fig = plt.figure(figsize=(10,10))
+    ax = fig.add_subplot(projection='3d')
+    trplot([np.array(control_poses)],ax=ax, length= 1.0, color="green")
+    ax.plot(x,y,z)
+
+    tranimate(
+        out_poses,
+        repeat=True, 
+        wait=True,
+        length = 1.0
+    )
+    
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file

From 4c21989b634fda19fb5c88d23ca5140ec7317852 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 12:54:56 -0400
Subject: [PATCH 02/11] formatting

---
 spatialmath/spline.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 562fd2a0..142c5658 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -14,7 +14,7 @@
 import matplotlib.pyplot as plt
 from spatialmath.base.transforms3d import tranimate, trplot
 
-class BSpline_SE3:
+class BSplineSE3:
     """ 
     """
     def __init__(self, knots, control_poses: list[SE3], degree) -> None:
@@ -51,7 +51,7 @@ def main():
     knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
     knots=np.append([0,0,0],knots)
     knots=np.append(knots,[1,1,1]) 
-    trajectory = BSpline_SE3(knots, control_poses, degree)
+    trajectory = BSplineSE3(knots, control_poses, degree)
     
     out_poses = [trajectory(i) for i in np.linspace(0,1,100)]
     x = [pose.x for pose in out_poses]

From 043b217e8885ba3976170df397eeb5348ed5f761 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 13:22:27 -0400
Subject: [PATCH 03/11] Clean up. Doc strings. Etc..

---
 spatialmath/spline.py | 81 ++++++++++++++++++++++++++-----------------
 1 file changed, 49 insertions(+), 32 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 142c5658..121a8221 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -7,7 +7,7 @@
 Copies parts of the API from scipy's B-Spline class.
 """
 
-from typing import Any
+from typing import Any, Optional
 from scipy.interpolate import BSpline
 from spatialmath import SE3
 import numpy as np
@@ -15,29 +15,64 @@
 from spatialmath.base.transforms3d import tranimate, trplot
 
 class BSplineSE3:
-    """ 
+    """ A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
+
+    The SE3 control poses are converted to se3 twists (the lie algebra) and a B-ppline 
+    is created for each dimension of the twist, using the corresponding element of the twists 
+    as the control point for the spline.  
+
+    For detailed information about B-Splines, please see this wikipedia article. 
+    https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline
     """
-    def __init__(self, knots, control_poses: list[SE3], degree) -> None:
-        self.t = knots # knots 
 
+    def __init__(self, control_poses: list[SE3], degree: int = 3, knots: Optional[list[float]] = None) -> None:
+        """ Construct BSplineSE3 object. The default arguments generate a cubic B-spline
+        with uniformly spaced knots.
+
+        - control_poses: list of SE3 objects that govern the shape of the spline. 
+        - degree: int that controls degree of the polynomial that governs any given point on the spline.
+        - knots: list of floats that govern which control points are active during evaluating the spline 
+        at a given t input.
+        """
+        
+        self.control_poses = control_poses
+        
         # a matrix where each row is a control pose as a twist
         # (so each column is a vector of control points for that dim of the twist)
         self.control_pose_matrix = np.vstack([np.array(element.twist()) for element in control_poses]) 
         
-        self.k = degree # degree of spline
+        self.degree = degree 
+        
+        if knots is None:
+            knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
+            knots=np.append([0,0,0],knots) # ensures the curve starts on the first control pose
+            knots=np.append(knots,[1,1,1])  # ensures the curve ends on the last control pose
+        self.knots = knots  
 
         self.splines = [ BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) ]
 
-    def __call__(self, x: float) -> Any:
-        """
-        x: Normalized time value to evaluate at.
+    def __call__(self, t: float) -> SE3:
+        """ Returns pose of spline at t.
+
+        t: Normalized time value [0,1] to evaluate the spline at.
         """
-        twist = np.hstack([spline(x) for spline in self.splines])
+        twist = np.hstack([spline(t) for spline in self.splines])
         return SE3.Exp(twist)
     
-    def eval(self, x):
-        self.__call__(x)
+    def visualize(self, num_samples: int, repeat: bool = False) -> None:
+        """ Displays an animation of the trajectory with the control points.
+        """
+        out_poses = [self(i) for i in np.linspace(0,1,num_samples)]
+        x = [pose.x for pose in out_poses]
+        y = [pose.y for pose in out_poses]
+        z = [pose.z for pose in out_poses]
+        
+        fig = plt.figure(figsize=(10,10))
+        ax = fig.add_subplot(projection='3d')
+        trplot([np.array(self.control_poses)],ax=ax, length= 1.0, color="green") # plot control points
+        ax.plot(x,y,z) # plot x,y,z trajectory
 
+        tranimate(out_poses, repeat=repeat, wait=True, length = 1.0) # animate pose along trajectory
 
 def main():
     degree = 3
@@ -45,30 +80,12 @@ def main():
         SE3.Trans(
             [e, 2*np.cos(e/2 * np.pi), 2*np.sin(e/2 * np.pi)]
         )
-        *SE3.Ry(e/8 * np.pi) for e in range(1,9)
+        *SE3.Ry(e/8 * np.pi) for e in range(0,8)
     ]
-    # t =  np.linspace(0, 1, len(control_poses) + degree + 1)   
-    knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
-    knots=np.append([0,0,0],knots)
-    knots=np.append(knots,[1,1,1]) 
-    trajectory = BSplineSE3(knots, control_poses, degree)
     
-    out_poses = [trajectory(i) for i in np.linspace(0,1,100)]
-    x = [pose.x for pose in out_poses]
-    y = [pose.y for pose in out_poses]
-    z = [pose.z for pose in out_poses]
+    spline = BSplineSE3(control_poses, degree)
+    spline.visualize(num_samples=100, repeat=True)
     
-    fig = plt.figure(figsize=(10,10))
-    ax = fig.add_subplot(projection='3d')
-    trplot([np.array(control_poses)],ax=ax, length= 1.0, color="green")
-    ax.plot(x,y,z)
-
-    tranimate(
-        out_poses,
-        repeat=True, 
-        wait=True,
-        length = 1.0
-    )
     
 
 if __name__ == "__main__":

From 74fd97a6b976b0fd8ff62247d746a2406b8eb097 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 13:26:43 -0400
Subject: [PATCH 04/11] nit

---
 spatialmath/spline.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 121a8221..a02b9f10 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -2,9 +2,9 @@
 # MIT Licence, see details in top-level file: LICENCE
 
 """
-Classes for parameterizing a trajectory in SE3 with B-Splines. 
+Classes for parameterizing a trajectory in SE3 with B-splines. 
 
-Copies parts of the API from scipy's B-Spline class.
+Copies parts of the API from scipy's B-spline class.
 """
 
 from typing import Any, Optional
@@ -21,7 +21,7 @@ class BSplineSE3:
     is created for each dimension of the twist, using the corresponding element of the twists 
     as the control point for the spline.  
 
-    For detailed information about B-Splines, please see this wikipedia article. 
+    For detailed information about B-splines, please see this wikipedia article. 
     https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline
     """
 
@@ -60,7 +60,7 @@ def __call__(self, t: float) -> SE3:
         return SE3.Exp(twist)
     
     def visualize(self, num_samples: int, repeat: bool = False) -> None:
-        """ Displays an animation of the trajectory with the control points.
+        """ Displays an animation of the trajectory with the control poses.
         """
         out_poses = [self(i) for i in np.linspace(0,1,num_samples)]
         x = [pose.x for pose in out_poses]

From e065e5e580b9d51186636d8b20b77a89a50f4afc Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 13:33:52 -0400
Subject: [PATCH 05/11] nit

---
 spatialmath/spline.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index a02b9f10..ebc7ebe5 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -17,7 +17,7 @@
 class BSplineSE3:
     """ A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
 
-    The SE3 control poses are converted to se3 twists (the lie algebra) and a B-ppline 
+    The SE3 control poses are converted to se3 twists (the lie algebra) and a B-spline 
     is created for each dimension of the twist, using the corresponding element of the twists 
     as the control point for the spline.  
 

From 20d491e0089da7de4c271d5baf044770497f61c8 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:06:16 -0400
Subject: [PATCH 06/11] Add unit test.

---
 spatialmath/__init__.py |  2 ++
 spatialmath/spline.py   | 43 ++++++++++++++++++-----------------------
 tests/test_spline.py    | 34 ++++++++++++++++++++++++++++++++
 3 files changed, 55 insertions(+), 24 deletions(-)
 create mode 100644 tests/test_spline.py

diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py
index 63287dcf..59f3929d 100644
--- a/spatialmath/__init__.py
+++ b/spatialmath/__init__.py
@@ -15,6 +15,7 @@
 )
 from spatialmath.quaternion import Quaternion, UnitQuaternion
 from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion
+from spatialmath.spline import BSplineSE3
 
 # from spatialmath.Plucker import *
 # from spatialmath import base as smb
@@ -43,6 +44,7 @@
     "LineSegment2",
     "Polygon2",
     "Ellipse",
+    "BSplineSE3"
 ]
 
 try:
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index ebc7ebe5..29bbbb57 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -7,7 +7,7 @@
 Copies parts of the API from scipy's B-spline class.
 """
 
-from typing import Any, Optional
+from typing import Any, Dict, List, Optional
 from scipy.interpolate import BSpline
 from spatialmath import SE3
 import numpy as np
@@ -25,7 +25,7 @@ class BSplineSE3:
     https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline
     """
 
-    def __init__(self, control_poses: list[SE3], degree: int = 3, knots: Optional[list[float]] = None) -> None:
+    def __init__(self, control_poses: List[SE3], degree: int = 3, knots: Optional[List[float]] = None) -> None:
         """ Construct BSplineSE3 object. The default arguments generate a cubic B-spline
         with uniformly spaced knots.
 
@@ -59,7 +59,16 @@ def __call__(self, t: float) -> SE3:
         twist = np.hstack([spline(t) for spline in self.splines])
         return SE3.Exp(twist)
     
-    def visualize(self, num_samples: int, repeat: bool = False) -> None:
+    def visualize(
+            self, 
+            num_samples: int, 
+            length:float = 1.0,
+            repeat: bool = False, 
+            ax: Optional[plt.Axes] = None,
+            kwargs_trplot: Dict[str, Any] = {"color": "green"},
+            kwargs_tranimate:  Dict[str, Any] = {"wait": True},
+            kwargs_plot: Dict[str, Any] = {}
+        ) -> None:
         """ Displays an animation of the trajectory with the control poses.
         """
         out_poses = [self(i) for i in np.linspace(0,1,num_samples)]
@@ -67,26 +76,12 @@ def visualize(self, num_samples: int, repeat: bool = False) -> None:
         y = [pose.y for pose in out_poses]
         z = [pose.z for pose in out_poses]
         
-        fig = plt.figure(figsize=(10,10))
-        ax = fig.add_subplot(projection='3d')
-        trplot([np.array(self.control_poses)],ax=ax, length= 1.0, color="green") # plot control points
-        ax.plot(x,y,z) # plot x,y,z trajectory
+        if ax is None:
+            fig = plt.figure(figsize=(10,10))
+            ax = fig.add_subplot(projection='3d')
 
-        tranimate(out_poses, repeat=repeat, wait=True, length = 1.0) # animate pose along trajectory
+        trplot([np.array(self.control_poses)], ax=ax, length= length, **kwargs_trplot) # plot control points
+        ax.plot(x,y,z, **kwargs_plot) # plot x,y,z trajectory
 
-def main():
-    degree = 3
-    control_poses = [
-        SE3.Trans(
-            [e, 2*np.cos(e/2 * np.pi), 2*np.sin(e/2 * np.pi)]
-        )
-        *SE3.Ry(e/8 * np.pi) for e in range(0,8)
-    ]
-    
-    spline = BSplineSE3(control_poses, degree)
-    spline.visualize(num_samples=100, repeat=True)
-    
-    
-
-if __name__ == "__main__":
-    main()
\ No newline at end of file
+        tranimate(out_poses, repeat=repeat, length = length, **kwargs_tranimate) # animate pose along trajectory
+        
\ No newline at end of file
diff --git a/tests/test_spline.py b/tests/test_spline.py
new file mode 100644
index 00000000..c9c4ec4f
--- /dev/null
+++ b/tests/test_spline.py
@@ -0,0 +1,34 @@
+import numpy.testing as nt
+import numpy as np
+import matplotlib.pyplot as plt
+import unittest
+import sys
+import pytest
+
+from spatialmath import BSplineSE3, SE3
+
+class TestBSplineSE3(unittest.TestCase):
+
+    control_poses = [
+        SE3.Trans(
+            [e, 2*np.cos(e/2 * np.pi), 2*np.sin(e/2 * np.pi)]
+        )
+        *SE3.Ry(e/8 * np.pi) for e in range(0,8)
+    ]
+    
+    @classmethod
+    def tearDownClass(cls):
+        plt.close("all")
+
+    def test_constructor(self):
+        BSplineSE3(self.control_poses)
+
+    def test_evaluation(self):
+        spline = BSplineSE3(self.control_poses)
+        nt.assert_almost_equal(spline(0).A, self.control_poses[0].A)
+        nt.assert_almost_equal(spline(1).A, self.control_poses[-1].A)
+
+    def test_visualize(self):
+        spline = BSplineSE3(self.control_poses)
+        spline.visualize(num_samples=100, repeat=False)
+    

From f04e0ddb7c46d83031c89ea42c159e8f30ef9e69 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:09:04 -0400
Subject: [PATCH 07/11] Fix jien's review comments.

---
 spatialmath/spline.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 29bbbb57..580b03db 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -45,8 +45,8 @@ def __init__(self, control_poses: List[SE3], degree: int = 3, knots: Optional[Li
         
         if knots is None:
             knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
-            knots=np.append([0,0,0],knots) # ensures the curve starts on the first control pose
-            knots=np.append(knots,[1,1,1])  # ensures the curve ends on the last control pose
+            knots=np.append([0.0]*degree, knots) # ensures the curve starts on the first control pose
+            knots=np.append(knots, [1]*degree)  # ensures the curve ends on the last control pose
         self.knots = knots  
 
         self.splines = [ BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) ]
@@ -71,7 +71,7 @@ def visualize(
         ) -> None:
         """ Displays an animation of the trajectory with the control poses.
         """
-        out_poses = [self(i) for i in np.linspace(0,1,num_samples)]
+        out_poses = [self(t) for t in np.linspace(0,1,num_samples)]
         x = [pose.x for pose in out_poses]
         y = [pose.y for pose in out_poses]
         z = [pose.z for pose in out_poses]
@@ -84,4 +84,3 @@ def visualize(
         ax.plot(x,y,z, **kwargs_plot) # plot x,y,z trajectory
 
         tranimate(out_poses, repeat=repeat, length = length, **kwargs_tranimate) # animate pose along trajectory
-        
\ No newline at end of file

From 6ca54b4f8342a5af2da5b9746baa551f2b547568 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:12:58 -0400
Subject: [PATCH 08/11] Run black formatter.

---
 spatialmath/__init__.py |  2 +-
 spatialmath/spline.py   | 97 ++++++++++++++++++++++++-----------------
 tests/test_spline.py    | 12 +++--
 3 files changed, 63 insertions(+), 48 deletions(-)

diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py
index 59f3929d..e6ef1f77 100644
--- a/spatialmath/__init__.py
+++ b/spatialmath/__init__.py
@@ -44,7 +44,7 @@
     "LineSegment2",
     "Polygon2",
     "Ellipse",
-    "BSplineSE3"
+    "BSplineSE3",
 ]
 
 try:
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 580b03db..a5a56fe9 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -14,73 +14,90 @@
 import matplotlib.pyplot as plt
 from spatialmath.base.transforms3d import tranimate, trplot
 
+
 class BSplineSE3:
-    """ A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
+    """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
 
-    The SE3 control poses are converted to se3 twists (the lie algebra) and a B-spline 
-    is created for each dimension of the twist, using the corresponding element of the twists 
-    as the control point for the spline.  
+    The SE3 control poses are converted to se3 twists (the lie algebra) and a B-spline
+    is created for each dimension of the twist, using the corresponding element of the twists
+    as the control point for the spline.
 
-    For detailed information about B-splines, please see this wikipedia article. 
+    For detailed information about B-splines, please see this wikipedia article.
     https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline
     """
 
-    def __init__(self, control_poses: List[SE3], degree: int = 3, knots: Optional[List[float]] = None) -> None:
-        """ Construct BSplineSE3 object. The default arguments generate a cubic B-spline
+    def __init__(
+        self,
+        control_poses: List[SE3],
+        degree: int = 3,
+        knots: Optional[List[float]] = None,
+    ) -> None:
+        """Construct BSplineSE3 object. The default arguments generate a cubic B-spline
         with uniformly spaced knots.
 
-        - control_poses: list of SE3 objects that govern the shape of the spline. 
+        - control_poses: list of SE3 objects that govern the shape of the spline.
         - degree: int that controls degree of the polynomial that governs any given point on the spline.
-        - knots: list of floats that govern which control points are active during evaluating the spline 
+        - knots: list of floats that govern which control points are active during evaluating the spline
         at a given t input.
         """
-        
+
         self.control_poses = control_poses
-        
+
         # a matrix where each row is a control pose as a twist
         # (so each column is a vector of control points for that dim of the twist)
-        self.control_pose_matrix = np.vstack([np.array(element.twist()) for element in control_poses]) 
-        
-        self.degree = degree 
-        
+        self.control_pose_matrix = np.vstack(
+            [np.array(element.twist()) for element in control_poses]
+        )
+
+        self.degree = degree
+
         if knots is None:
-            knots=np.linspace(0,1,len(control_poses)-2,endpoint=True)
-            knots=np.append([0.0]*degree, knots) # ensures the curve starts on the first control pose
-            knots=np.append(knots, [1]*degree)  # ensures the curve ends on the last control pose
-        self.knots = knots  
+            knots = np.linspace(0, 1, len(control_poses) - 2, endpoint=True)
+            knots = np.append(
+                [0.0] * degree, knots
+            )  # ensures the curve starts on the first control pose
+            knots = np.append(
+                knots, [1] * degree
+            )  # ensures the curve ends on the last control pose
+        self.knots = knots
 
-        self.splines = [ BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) ]
+        self.splines = [
+            BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6)
+        ]
 
     def __call__(self, t: float) -> SE3:
-        """ Returns pose of spline at t.
+        """Returns pose of spline at t.
 
         t: Normalized time value [0,1] to evaluate the spline at.
         """
         twist = np.hstack([spline(t) for spline in self.splines])
         return SE3.Exp(twist)
-    
+
     def visualize(
-            self, 
-            num_samples: int, 
-            length:float = 1.0,
-            repeat: bool = False, 
-            ax: Optional[plt.Axes] = None,
-            kwargs_trplot: Dict[str, Any] = {"color": "green"},
-            kwargs_tranimate:  Dict[str, Any] = {"wait": True},
-            kwargs_plot: Dict[str, Any] = {}
-        ) -> None:
-        """ Displays an animation of the trajectory with the control poses.
-        """
-        out_poses = [self(t) for t in np.linspace(0,1,num_samples)]
+        self,
+        num_samples: int,
+        length: float = 1.0,
+        repeat: bool = False,
+        ax: Optional[plt.Axes] = None,
+        kwargs_trplot: Dict[str, Any] = {"color": "green"},
+        kwargs_tranimate: Dict[str, Any] = {"wait": True},
+        kwargs_plot: Dict[str, Any] = {},
+    ) -> None:
+        """Displays an animation of the trajectory with the control poses."""
+        out_poses = [self(t) for t in np.linspace(0, 1, num_samples)]
         x = [pose.x for pose in out_poses]
         y = [pose.y for pose in out_poses]
         z = [pose.z for pose in out_poses]
-        
+
         if ax is None:
-            fig = plt.figure(figsize=(10,10))
-            ax = fig.add_subplot(projection='3d')
+            fig = plt.figure(figsize=(10, 10))
+            ax = fig.add_subplot(projection="3d")
 
-        trplot([np.array(self.control_poses)], ax=ax, length= length, **kwargs_trplot) # plot control points
-        ax.plot(x,y,z, **kwargs_plot) # plot x,y,z trajectory
+        trplot(
+            [np.array(self.control_poses)], ax=ax, length=length, **kwargs_trplot
+        )  # plot control points
+        ax.plot(x, y, z, **kwargs_plot)  # plot x,y,z trajectory
 
-        tranimate(out_poses, repeat=repeat, length = length, **kwargs_tranimate) # animate pose along trajectory
+        tranimate(
+            out_poses, repeat=repeat, length=length, **kwargs_tranimate
+        )  # animate pose along trajectory
diff --git a/tests/test_spline.py b/tests/test_spline.py
index c9c4ec4f..f518fcfb 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -7,15 +7,14 @@
 
 from spatialmath import BSplineSE3, SE3
 
-class TestBSplineSE3(unittest.TestCase):
 
+class TestBSplineSE3(unittest.TestCase):
     control_poses = [
-        SE3.Trans(
-            [e, 2*np.cos(e/2 * np.pi), 2*np.sin(e/2 * np.pi)]
-        )
-        *SE3.Ry(e/8 * np.pi) for e in range(0,8)
+        SE3.Trans([e, 2 * np.cos(e / 2 * np.pi), 2 * np.sin(e / 2 * np.pi)])
+        * SE3.Ry(e / 8 * np.pi)
+        for e in range(0, 8)
     ]
-    
+
     @classmethod
     def tearDownClass(cls):
         plt.close("all")
@@ -31,4 +30,3 @@ def test_evaluation(self):
     def test_visualize(self):
         spline = BSplineSE3(self.control_poses)
         spline.visualize(num_samples=100, repeat=False)
-    

From 279c1caf1cbff9e4fa7602e01409eb9d8f40f2d3 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:33:44 -0400
Subject: [PATCH 09/11] Properly generalize uniformly spaced knots with degree
 of input.

---
 spatialmath/spline.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index a5a56fe9..388f23b8 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -52,7 +52,10 @@ def __init__(
         self.degree = degree
 
         if knots is None:
-            knots = np.linspace(0, 1, len(control_poses) - 2, endpoint=True)
+            # degree = len(internal_knots) + 2*degree - len(control_poses) + 1
+            # we have 2*degree edge knots for start and finish
+            # len(internal_knots) = 
+            knots = np.linspace(0, 1, len(control_poses) - degree + 1, endpoint=True)
             knots = np.append(
                 [0.0] * degree, knots
             )  # ensures the curve starts on the first control pose
@@ -62,7 +65,7 @@ def __init__(
         self.knots = knots
 
         self.splines = [
-            BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6)
+            BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) #twists are length 6
         ]
 
     def __call__(self, t: float) -> SE3:

From f81893a43641f6b9960011958f3ae78e54b10964 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:35:16 -0400
Subject: [PATCH 10/11] Better comment.

---
 spatialmath/spline.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 388f23b8..6586ba5a 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -38,7 +38,8 @@ def __init__(
         - control_poses: list of SE3 objects that govern the shape of the spline.
         - degree: int that controls degree of the polynomial that governs any given point on the spline.
         - knots: list of floats that govern which control points are active during evaluating the spline
-        at a given t input.
+        at a given t input. If none, they are automatically, uniformly generated based on number of control poses and
+        degree of spline.
         """
 
         self.control_poses = control_poses
@@ -54,7 +55,7 @@ def __init__(
         if knots is None:
             # degree = len(internal_knots) + 2*degree - len(control_poses) + 1
             # we have 2*degree edge knots for start and finish
-            # len(internal_knots) = 
+            # len(internal_knots) =
             knots = np.linspace(0, 1, len(control_poses) - degree + 1, endpoint=True)
             knots = np.append(
                 [0.0] * degree, knots
@@ -65,7 +66,8 @@ def __init__(
         self.knots = knots
 
         self.splines = [
-            BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) #twists are length 6
+            BSpline(knots, self.control_pose_matrix[:, i], degree)
+            for i in range(0, 6)  # twists are length 6
         ]
 
     def __call__(self, t: float) -> SE3:

From 350a25ca04664e2cf02189bb22938264f38a8c79 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 9 Jul 2024 14:36:44 -0400
Subject: [PATCH 11/11] ...

---
 spatialmath/spline.py | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 6586ba5a..f81dcec3 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -53,9 +53,6 @@ def __init__(
         self.degree = degree
 
         if knots is None:
-            # degree = len(internal_knots) + 2*degree - len(control_poses) + 1
-            # we have 2*degree edge knots for start and finish
-            # len(internal_knots) =
             knots = np.linspace(0, 1, len(control_poses) - degree + 1, endpoint=True)
             knots = np.append(
                 [0.0] * degree, knots