diff --git a/spatialmath/spline.py b/spatialmath/spline.py index c2da982e..e2b557d1 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -133,12 +133,12 @@ def __call__(self, t: float) -> SE3: """ return SE3.Rt(t=self.spline_xyz(t), R=self.spline_so3(t).as_matrix()) - def derivative(self, t: float) -> Twist3: - linear_vel = self.spline_xyz.derivative()(t) - angular_vel = self.spline_so3( - t, 1 + def derivative(self, t: float, order: int = 1) -> Twist3: + linear = self.spline_xyz.derivative(order)(t) + angular = self.spline_so3( + t, order ) # 1 is angular rate, 2 is angular acceleration - return Twist3(linear_vel, angular_vel) + return Twist3(linear, angular) class SplineFit: