Skip to content

Fit b spline se3 #129

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
myeatman-bdai wants to merge 6 commits into from
Closed

Fit b spline se3 #129

Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4dcf25e
Add BSplineTwist3
myeatman-bdai Jul 11, 2024
f94601d
Remove copied imports
myeatman-bdai Jul 11, 2024
33551d2
Save point on mostly working version.
myeatman-bdai Jul 18, 2024
49420a6
Working fit example.
myeatman-bdai Jul 18, 2024
32b7af5
Clean up.
myeatman-bdai Jul 18, 2024
8cc56b8
Move example to unit test.
myeatman-bdai Jul 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Next Next commit
Add BSplineTwist3
  • Loading branch information
@myeatman-bdai
myeatman-bdai committed Jul 11, 2024
commit 4dcf25ef309b2aec7284e86bc3c2e186d1222062
78 changes: 78 additions & 0 deletions spatialmath/spline.py
View file
Open in desktop

@petercorke petercorke Jan 11, 2025

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These are a really useful addition. The control poses

    def __init__(
         self,
         control_poses: List[SE3],
     ) -> None:

are passed as a list of SE3s not as a multi-element SE3. What's the thinking there?

There are multiple interpolator classes, each algorithm specific. Would it be better to have one interpolator class with a passed argument to say which one is used. This is kind of the approach that SciPy takes with optimisers and integrators that have a method parameter. Simplest approach would be a wrapper class that instantiates one of the specific classes, all of them with a common parent class.

Original file line number Diff line number Diff line change
Expand Up @@ -103,3 +103,81 @@ def visualize(
tranimate(
out_poses, repeat=repeat, length=length, **kwargs_tranimate
) # animate pose along trajectory

# 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, Dict, List, Optional
from scipy.interpolate import BSpline
from spatialmath import SE3, Twist3
import numpy as np
import matplotlib.pyplot as plt
from spatialmath.base.transforms3d import tranimate, trplot


class BSplineTwist3:
"""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.

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_twists: List[Twist3],
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. If none, they are automatically, uniformly generated based on number of control poses and
degree of spline.
"""

self.control_twists = control_twists

# 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_twist_matrix = np.vstack(
[element for element in control_twists]
)

self.degree = degree

if knots is None:
knots = np.linspace(0, 1, len(control_twists) - degree + 1, 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_twist_matrix[:, i], degree)
for i in range(0, 6) # twists are length 6
]

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(t) for spline in self.splines])
return twist