There was a problem hiding this comment.
Looks good. Can you add the derivative computation to the unit tests and add a docstring to the derivative function.
Sorry, something went wrong.
| linear_vel = self.spline_xyz.derivative()(t) | ||
| angular_vel = self.spline_so3( | ||
| t, 1 | ||
| def derivative(self, t: float, order: int = 1) -> Twist3: |
There was a problem hiding this comment.
Add some warning that this only works for order = [1,2]. Current system throws a value error for order != [0,1,2] but will fail on order 0 with a "ValueError: bad value to Twist constructor" error. We should check and throw errors for order != [1,2].
Sorry, something went wrong.
There was a problem hiding this comment.
Thanks for testing that
Sorry, something went wrong.
Calculate higher order spline derivatives for the InterpSplineSE3 class.