From 4dcf25ef309b2aec7284e86bc3c2e186d1222062 Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 11 Jul 2024 11:13:12 -0400 Subject: [PATCH 1/6] Add BSplineTwist3 --- spatialmath/spline.py | 78 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/spatialmath/spline.py b/spatialmath/spline.py index f81dcec3..f9d510b7 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -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 + From f94601d9d403a78c964d85ed04e4d96799a84074 Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 11 Jul 2024 11:14:49 -0400 Subject: [PATCH 2/6] Remove copied imports --- spatialmath/spline.py | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/spatialmath/spline.py b/spatialmath/spline.py index f9d510b7..845821f2 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -9,7 +9,7 @@ from typing import Any, Dict, List, Optional from scipy.interpolate import BSpline -from spatialmath import SE3 +from spatialmath import SE3, Twist3 import numpy as np import matplotlib.pyplot as plt from spatialmath.base.transforms3d import tranimate, trplot @@ -104,23 +104,7 @@ def visualize( 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. From 33551d2542c4bcbd2579df1bea69530aea42d26f Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 18 Jul 2024 08:47:31 -0400 Subject: [PATCH 3/6] Save point on mostly working version. --- spatialmath/__init__.py | 4 +- spatialmath/spline copy.py | 535 +++++++++++++++++++++++++++++++++++++ spatialmath/spline.py | 275 +++++++++++++------ tests/test_spline.py | 8 +- 4 files changed, 732 insertions(+), 90 deletions(-) create mode 100644 spatialmath/spline copy.py diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py index e6ef1f77..01156c7f 100644 --- a/spatialmath/__init__.py +++ b/spatialmath/__init__.py @@ -15,7 +15,7 @@ ) from spatialmath.quaternion import Quaternion, UnitQuaternion from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion -from spatialmath.spline import BSplineSE3 +from spatialmath.spline import CubicBSplineSE3 # from spatialmath.Plucker import * # from spatialmath import base as smb @@ -44,7 +44,7 @@ "LineSegment2", "Polygon2", "Ellipse", - "BSplineSE3", + "CubicBSplineSE3", ] try: diff --git a/spatialmath/spline copy.py b/spatialmath/spline copy.py new file mode 100644 index 00000000..bcba4cad --- /dev/null +++ b/spatialmath/spline copy.py @@ -0,0 +1,535 @@ +# 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, SO3 +import numpy as np +import matplotlib.pyplot as plt +from spatialmath.base.transforms3d import tranimate, trplot +from scipy.optimize import minimize +from scipy.interpolate import splrep + + +def weighted_average_SE3_metric(a: Twist3 | SE3, b: Twist3 | SE3, w: float = 0.5) -> float: + """A positive definite distance metric for SE3. + + The metric is: (1-w)*translation_distance + w*angular_distance + + Note that there isn't a + great "natural" choice for a metric on SE3 (see Appendix A.3.2 in + 'A Mathematical Introduction to Robotic Manipulation' by Murray, Li, Sastry) + + spatialmath has options for the rotational metrics from + 'Metrics for 3D Rotations: Comparison and Analysis' by Du Q. Huynh. + This uses the 'best choice' from that paper. + + Args: + a: a twist from SE3.twist, or an SE3 object + b: a twist from SE3.twist, or an SE3 object + w: a float that represents the relative weighting between the rotational and translation distances. + + Returns: + a float for the 'distance' between two SE3 poses + + """ + if w> 1 or w < 0: + raise ValueError(f"Weight w={w} is outside the range [0,1].") + + if isinstance(a, Twist3): + a = SE3.Exp(a) + + if isinstance(b, Twist3): + b = SE3.Exp(b) + + # if np.linalg.norm(a - b) < 1e-6: + # return 0.0 + + angular_distance = a.angdist(b) + translation_distance = np.linalg.norm(a.t - b.t) + + return (1 - w) * translation_distance + w * angular_distance + + +class TrajectoryTwist: + + def __init__(self, poses: List[SE3], timestamps: List[float]) -> None: + self.poses = poses + self.timestamps = timestamps + self.twists = [] + + def metric(self, target_pose: SE3, seed_twist: Twist3): + return weighted_average_SE3_metric(target_pose, SE3.Exp(seed_twist)) + + def fit(self): + twists = [] + for pose in self.poses: + if len(twists) == 0: + twists.append(pose.twist()) + else: + f = lambda x: self.metric(pose, x) + twists.append(Twist3(minimize(f, twists[-1].A).x)) + + self.twists = twists + + def visualize(self): + fig = plt.figure(figsize=(40, 20)) + ax = fig.add_subplot(1, 2, 1, projection="3d") + trplot( + [np.array(self.poses)], + ax=ax, + length=0.2 + ) # plot control points + # import ipdb; ipdb.set_trace() + trplot( + [np.array([twist.exp() for twist in self.twists])], + ax=ax, + length=0.2, + color="cornflowerblue", + ) # plot data points + twist_data = np.vstack([twist.A for twist in self.twists]) + ax_2: plt.Axes = fig.add_subplot(1, 2, 2) + ax_2.plot( + self.timestamps, + twist_data[:, 0], + "o", + self.timestamps, + twist_data[:, 1], + "o", + self.timestamps, + twist_data[:, 2], + "o", + self.timestamps, + twist_data[:, 3], + "o", + self.timestamps, + twist_data[:, 4], + "o", + self.timestamps, + twist_data[:, 5], + "o", + ) + plt.show() + + +class CubicBSplineSE3: + """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_poses: List[SE3], + ) -> 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.basis_coefficent_matrix = 1/6*np.array([ + [6, 0, 0, 0], + [5, 3, -3, 1], + [1, 3, 3, -2], + [0, 0, 0, 1] + ]) + self.n = len(control_poses) + self.control_poses = control_poses + self.degree = 3 + + # use open uniform knot vector + knots = np.linspace(0, 1, len(control_poses)-2, endpoint=True) + knots = np.append( + [0] * self.degree, knots + ) # ensures the curve starts on the first control pose + knots = np.append( + knots, [1] * self.degree + ) # ensures the curve ends on the last control pose + self.knots = knots + + + def __call__(self, time:float): + """Returns pose of spline at t. + + t: Normalized time value [0,1] to evaluate the spline at. + """ + current_pose = SE3() + + spline_no_coeff = BSpline.design_matrix([time], self.knots, self.degree) #the B in sum(alpha_i * B_i) = S(t) + + rows,cols = spline_no_coeff.nonzero() + for row,col in zip(rows,cols): + current_pose = self.control_poses[col].interp1(spline_no_coeff[row,col]) * current_pose + + + return current_pose + + def visualize( + self, + num_samples: int, + length: float = 0.2, + 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") + + 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 + + +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 + + +class FitBSplineTwist3: + + def __init__(self, twist_data: np.ndarray, timestamps, knots) -> None: + """twist data is nx6 matrix where a row is a twist""" + + self.twist_data = twist_data + self.pose_data = [SE3.Exp(twist) for twist in twist_data] + self.timestamps = timestamps + self.spline = BSplineTwist3([Twist3() for i in range(0,8)]) + self.knots = knots + self.degree = 3 + self.norm_ratio = 0.0 + + t = np.array(self.timestamps) + t = t - t[0] # shift start to 0 + t = t / t[-1] # normalize range to [0,1] + + self.normalized_timestamps = t + + + def update_control_twists(self, control_twists: np.array): + """control_twists is nx6 matrix where a row is a twist""" + self.spline = BSplineTwist3([row for row in control_twists]) + + def L1_over_euclidian_distance_error(self, control_twists: Optional[np.array] = None): + """ control twists are input as a vector from a flattened nx6 matrix here + """ + if control_twists is not None: + control_twists = control_twists.reshape(int(len(control_twists)/6), 6) + self.update_control_twists(control_twists) + + distances = np.array([np.linalg.norm(SE3.Exp(self.spline(timestamp)).t - pose.t) for timestamp, pose in zip(self.timestamps,self.pose_data, strict=True)]) + return np.linalg.norm(distances, ord=np.inf) + + def L1_over_SE3_metric_error(self, control_twists: Optional[np.array] = None): + """ control twists are input as a vector from a flattened nx6 matrix here + """ + if control_twists is not None: + control_twists = control_twists.reshape(int(len(control_twists)/6), 6) + self.update_control_twists(control_twists) + + distances = np.array([weighted_average_SE3_metric(SE3.Exp(self.spline(timestamp)), pose, self.norm_ratio) for timestamp, pose in zip(self.timestamps,self.pose_data, strict=True)]) + return np.linalg.norm(distances, ord=np.inf) + + def fit2(self): + result = minimize(self.L1_over_SE3_metric_error, self.spline.control_twist_matrix.flatten()) + self.update_control_twists(result.x.reshape(int(len(result.x)/6), 6)) + + def fit(self) -> None: + """Fits a b spline to the input SE3 trajectory and outputs control poses. + + Assumes timestamps are monotonically increasing / are sorted. + """ + # data_as_twists = np.vstack([np.array(pose.twist()) for pose in self.twist_data]) + # import ipdb; ipdb.set_trace() + control_twists = np.vstack( + [ + splrep(self.normalized_timestamps, self.twist_data[:, i], k=self.degree, t=self.knots)[1][ + 0 : len(self.knots) + self.degree + 1 + ] + for i in range(6) + ] + ).T + + self.spline = BSplineTwist3([row for row in control_twists]) + + def visualize( + self, + num_samples: int, + length: float = 0.5, + repeat: bool = True, + kwargs_trplot: Dict[str, Any] = {"color": "green"}, + kwargs_tranimate: Dict[str, Any] = {"wait": True, "dims": [-5, 5, -5, 5, -5, 5]}, + kwargs_plot: Dict[str, Any] = {}, + ) -> None: + """Displays an animation of the trajectory with the control poses.""" + + fig = plt.figure(figsize=(40, 20)) + ax_1: plt.Axes = fig.add_subplot(1, 2, 1) + + times = np.linspace(0, 1, num_samples) + + fit_twists = np.vstack([self.spline(t) for t in times]) + # fit_twists = np.vstack([pose.UnitQuaternion().A for pose in fit_poses]) + ax_1.plot( + times, + fit_twists[:, 0], + times, + fit_twists[:, 1], + times, + fit_twists[:, 2], + times, + fit_twists[:, 3], + times, + fit_twists[:, 4], + times, + fit_twists[:, 5], + ) + + # pose_data_twists = np.vstack([pose.twist().A for pose in self.twist_data]) + ax_1.plot( + self.timestamps, + self.twist_data[:, 0], + "o", + self.timestamps, + self.twist_data[:, 1], + "o", + self.timestamps, + self.twist_data[:, 2], + "o", + self.timestamps, + self.twist_data[:, 3], + "o", + self.timestamps, + self.twist_data[:, 4], + "o", + self.timestamps, + self.twist_data[:, 5], + "o", + ) + ax_1.plot( + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 0], + "x", + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 1], + "x", + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 2], + "x", + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 3], + "x", + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 4], + "x", + self.spline.knots[self.degree-1:-self.degree+1], + self.spline.control_twist_matrix[:, 5], + "x", + ) + ax_1.legend( + labels=[ + "x_fit", + "y_fit", + "z_fit", + "x_rot_fit", + "y_rot_fit", + "z_rot_fit", + "x_true", + "y_true", + "z_true", + "x_rot_true", + "y_rot_true", + "z_rot_true", + "x_control", + "y_control", + "z_control", + "x_rot_control", + "y_rot_control", + "z_rot_control", + ] + ) + + ax_2: plt.Axes = fig.add_subplot(1, 2, 2, projection="3d") + # ax_2.plot(x, y, z, **kwargs_plot) # plot x,y,z trajectory + # import ipdb; ipdb.set_trace() + trplot( + [np.array([SE3.Exp(twist) for twist in self.spline.control_twists])], + ax=ax_2, + length=length, + **kwargs_trplot, + ) # plot control points + trplot( + [np.array([SE3.Exp(twist) for twist in self.twist_data])], + ax=ax_2, + length=0.25 * length, + color="cornflowerblue", + ) # plot data points + + fit_poses = [SE3.Exp(twist) for twist in fit_twists] + # for pose in fit_poses: + # print(pose.ishom()) + tranimate(fit_poses, repeat=repeat, length=length, **kwargs_tranimate) # animate pose along trajectory + + +def example_bspline_fit_scipy(): + + num_data_points = 9 + num_samples = 100 + frequency = 2 + scale = 2 + + timestamps = np.linspace(0, 1, num_data_points) + x = np.array([t for t in timestamps]) + trajectory = [ + SE3.Rt(t = [e, np.sin(e * 2*np.pi* frequency), np.sin(e * 2*np.pi * frequency)], R= SO3.Rx( e*2*np.pi* frequency)) + for e in x + ] + print("Control points") + for pose in trajectory: + print(pose) + + se3_spline = CubicBSplineSE3(control_poses=trajectory) + # print("Spline") + # for knot in np.unique(se3_spline.knots): + # print(se3_spline.eval_spline(knot-0.001)) + # print(se3_spline.eval_spline(knot+0.001)) + + # import ipdb; ipdb.set_trace() + se3_spline.visualize(num_samples=100, repeat=True) + + # import ipdb; ipdb.set_trace() + # trajectory = [ + # SE3.Rt( + # t=[scale * t, scale * np.cos(t * frequency * np.pi), scale * np.cos(t * frequency * np.pi)], + # R=SO3.Ry(t * 1.5 * frequency * np.pi), + # ) + # for t in timestamps + # ] + # delta_trajectories = np.vstack([trajectory[i+1].delta(trajectory[i]) for i in range(0,len(trajectory)-1)]) + + # trajectory = np.vstack( + # [ + # Twist3( + # [ + # scale * t, + # scale * np.cos(t * frequency * np.pi), + # scale * np.sin(t * frequency * np.pi), + # t * 1.5 * frequency * np.pi, + # 1, + # 2, + # ] + # ).A + # for t in timestamps + # ] + # ) + + # num_knots = 4 + # knots = np.linspace(0, 1, num_knots + 2)[1:-1] + # print(f"knots: {knots}") + + # fit = FitBSplineTwist3(twist_data=delta_trajectories, timestamps=timestamps[0:-1], knots=knots) + # fit.pose_data = trajectory + # fit.fit() + # print(fit.L1_euclidian_distance_error()) + # fit.visualize(num_samples=num_samples) + # fit.norm_ratio = 0.0 + # fit.fit2() + # fit.norm_ratio = 0.5 + # fit.fit2() + # print(fit.L1_over_euclidian_distance_error()) + + # fit.visualize(num_samples=num_samples, repeat=True) + + # traj_class = TrajectoryTwist(poses=trajectory, timestamps=timestamps) + # traj_class.fit() + # import ipdb; ipdb.set_trace() + # traj_class.visualize() + + +if __name__ == "__main__": + example_bspline_fit_scipy() diff --git a/spatialmath/spline.py b/spatialmath/spline.py index 845821f2..ab35dcfe 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -9,13 +9,48 @@ from typing import Any, Dict, List, Optional from scipy.interpolate import BSpline -from spatialmath import SE3, Twist3 +from spatialmath import SE3, Twist3, SO3 import numpy as np import matplotlib.pyplot as plt from spatialmath.base.transforms3d import tranimate, trplot +from scipy.optimize import minimize +from scipy.interpolate import splrep -class BSplineSE3: +def weighted_average_SE3_metric(a: SE3, b: SE3, w: float = 0.5) -> float: + """A positive definite distance metric for SE3. + + The metric is: (1-w)*translation_distance + w*angular_distance + + Note that there isn't a + great "natural" choice for a metric on SE3 (see Appendix A.3.2 in + 'A Mathematical Introduction to Robotic Manipulation' by Murray, Li, Sastry) + + spatialmath has options for the rotational metrics from + 'Metrics for 3D Rotations: Comparison and Analysis' by Du Q. Huynh. + This uses the 'best choice' from that paper. + + Args: + a: a twist from SE3.twist, or an SE3 object + b: a twist from SE3.twist, or an SE3 object + w: a float that represents the relative weighting between the rotational and translation distances. + + Returns: + a float for the 'distance' between two SE3 poses + + """ + if w> 1 or w < 0: + raise ValueError(f"Weight w={w} is outside the range [0,1].") + + # if np.linalg.norm(a - b) < 1e-6: + # return 0.0 + + angular_distance = a.angdist(b) + translation_distance = np.linalg.norm(a.t - b.t) + + return (1 - w) * translation_distance + w * angular_distance + +class CubicBSplineSE3: """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 @@ -26,59 +61,52 @@ class BSplineSE3: https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline """ + degree = 3 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. + with a open uniform knot vector. - 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_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 - - if knots is None: - 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 - 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) # twists are length 6 - ] - - def __call__(self, t: float) -> SE3: + self.knots = self.knots_from_num_control_poses(len(control_poses)) + + def __call__(self, time:float): """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) + """ + current_pose = SE3() + + spline_no_coeff = BSpline.design_matrix([time], self.knots, self.degree) #the B in sum(alpha_i * B_i) = S(t) + + rows,cols = spline_no_coeff.nonzero() + for row,col in zip(rows,cols): + control_pose: SE3 = self.control_poses[col] + current_pose = control_pose.interp1(spline_no_coeff[row,col]) * current_pose + + return current_pose + + @classmethod + def knots_from_num_control_poses(self, num_control_poses: int): + # use open uniform knot vector + knots = np.linspace(0, 1, num_control_poses-2, endpoint=True) + knots = np.append( + [0] * CubicBSplineSE3.degree, knots + ) # ensures the curve starts on the first control pose + knots = np.append( + knots, [1] * CubicBSplineSE3.degree + ) # ensures the curve ends on the last control pose + return knots def visualize( self, num_samples: int, - length: float = 1.0, + length: float = 0.2, repeat: bool = False, ax: Optional[plt.Axes] = None, kwargs_trplot: Dict[str, Any] = {"color": "green"}, @@ -105,63 +133,142 @@ def visualize( ) # animate pose along trajectory -class BSplineTwist3: - """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline. +class FitCubicBSplineSE3: - 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. + def __init__(self, pose_data: List[SE3], timestamps) -> None: + """ + Timestamps should be normalized to [0,1] and sorted. + Outputs control poses, but the optimization is done on the [pos,axis-angle], flattened into a 1d vector. + """ + self.pose_data = pose_data + self.timestamps = timestamps + self.num_control_pose = 4 + self.se3_rep_size = 6 #[x, y, z, axis-angle vec] + + # + sample_points = np.linspace(self.timestamps[0], self.timestamps[-1], self.num_control_pose) + closest_timestamp_indices = np.searchsorted(self.timestamps, sample_points) + for i, index in enumerate(closest_timestamp_indices): + if index < 0: + closest_timestamp_indices[i] = 0 + if index >= len(timestamps): + closest_timestamp_indices[i] = len(timestamps) - 1 + + # + self.spline = CubicBSplineSE3(control_poses=[self.pose_data[index] for index in closest_timestamp_indices]) + + @classmethod + def make_SE3_pose(self, row: np.ndarray): + t = row[0:3] + so3_twist = row[3:6] + + return SE3.Rt(t = t, R = SO3.Exp(so3_twist)) + + @classmethod + def make_SE3_rep(self, pose: SE3): + so3_twist = SO3(pose.R).log(twist=True) + return np.concatenate([pose.t, so3_twist]) - For detailed information about B-splines, please see this wikipedia article. - https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline - """ - def __init__( + def objective_function_pos(self, pos: np.ndarray): + "L1 norm of SE3 distances between data points and spline" + + # data massage + pos_matrix = pos.reshape(self.num_control_pose, 3) + spline = CubicBSplineSE3(control_poses=[SE3.Trans(row) for row in pos_matrix]) + + # objective + error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 0.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] + + return np.linalg.norm(error_vector, ord=2) + + + def objective_function_so3(self, so3_twists_flat: np.ndarray): + "L1 norm of SE3 distances between data points and spline" + + # data massage + so3_twists_matrix = so3_twists_flat.reshape(self.num_control_pose, 3) + spline = CubicBSplineSE3(control_poses=[SE3(SO3.Exp(row)) for row in so3_twists_matrix]) + + # objective + error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 0.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] + + return np.linalg.norm(error_vector, ord=2) + + def fit(self, disp: bool = False): + + pos_result = self.fit_pos(disp) + so3_result = self.fit_so3(disp) + + control_pose_matrix = np.hstack([ + pos_result.x.reshape(self.num_control_pose, 3), + so3_result.x.reshape(self.num_control_pose, 3) + ]) + + self.spline = CubicBSplineSE3(control_poses=[self.make_SE3_pose(row) for row in control_pose_matrix]) + return pos_result, so3_result + + def fit_pos(self, disp: bool = False): + + pos_flat = np.concatenate([pose.t for pose in self.spline.control_poses]) + result = minimize(self.objective_function_pos, pos_flat, method="slsqp", options = {"disp":disp}) + return result + + def fit_so3(self, disp: bool = False): + so3_twists_flat = np.concatenate([SO3(pose.R).log(twist=True)for pose in self.spline.control_poses]) + result = minimize(self.objective_function_so3, so3_twists_flat, method="slsqp", options = {"disp":disp}) + return result + + def visualize( self, - control_twists: List[Twist3], - degree: int = 3, - knots: Optional[List[float]] = None, + num_samples: int, + length: float = 0.2, + 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: - """Construct BSplineSE3 object. The default arguments generate a cubic B-spline - with uniformly spaced knots. + """Displays an animation of the trajectory with the control poses.""" + out_poses = [self.spline(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] - - 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. - """ + if ax is None: + fig = plt.figure(figsize=(10, 10)) + ax = fig.add_subplot(projection="3d") - self.control_twists = control_twists + trplot( + [np.array(self.spline.control_poses)], ax=ax, length=length, color= "green" + ) # plot control points + trplot( + [np.array(self.pose_data)], ax=ax, length=length, color= "cornflowerblue", + ) # plot control points + ax.plot(x, y, z, **kwargs_plot) # plot x,y,z trajectory - # 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] - ) + tranimate( + out_poses, repeat=repeat, length=length, **kwargs_tranimate + ) # animate pose along trajectory - self.degree = degree +def example_bspline_fit_scipy(): - 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 + num_data_points = 33 + num_samples = 100 + frequency = 2 + scale = 2 - self.splines = [ - BSpline(knots, self.control_twist_matrix[:, i], degree) - for i in range(0, 6) # twists are length 6 - ] + timestamps = np.linspace(0, 1, num_data_points) + trajectory = [ + SE3.Rt(t = [t*scale, np.sin(t * 2*np.pi* frequency), np.sin(t * 2*np.pi * frequency)], R= SO3.Rx( t*2*np.pi* frequency)) + for t in timestamps + ] - def __call__(self, t: float) -> SE3: - """Returns pose of spline at t. + fit_se3_spline = FitCubicBSplineSE3(trajectory, timestamps) + + result = fit_se3_spline.fit(disp=True) + fit_se3_spline.visualize(num_samples=num_samples, repeat=True) - t: Normalized time value [0,1] to evaluate the spline at. - """ - twist = np.hstack([spline(t) for spline in self.splines]) - return twist +if __name__ == "__main__": + example_bspline_fit_scipy() diff --git a/tests/test_spline.py b/tests/test_spline.py index f518fcfb..05f23508 100644 --- a/tests/test_spline.py +++ b/tests/test_spline.py @@ -5,7 +5,7 @@ import sys import pytest -from spatialmath import BSplineSE3, SE3 +from spatialmath import CubicBSplineSE3, SE3 class TestBSplineSE3(unittest.TestCase): @@ -20,13 +20,13 @@ def tearDownClass(cls): plt.close("all") def test_constructor(self): - BSplineSE3(self.control_poses) + CubicBSplineSE3(self.control_poses) def test_evaluation(self): - spline = BSplineSE3(self.control_poses) + spline = CubicBSplineSE3(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 = CubicBSplineSE3(self.control_poses) spline.visualize(num_samples=100, repeat=False) From 49420a6a3222fc6f12a0ec742562ef310c7a9682 Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 18 Jul 2024 13:04:46 -0400 Subject: [PATCH 4/6] Working fit example. --- spatialmath/pose3d.py | 7 +++ spatialmath/spline.py | 109 +++++++++++++++++++++++++++--------------- 2 files changed, 77 insertions(+), 39 deletions(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index 4c2dae06..0e0c760e 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -1041,6 +1041,13 @@ def shape(self) -> Tuple[int, int]: """ return (4, 4) + @SO3.R.setter + def R(self, r: SO3Array) -> None: + if len(self) > 1: + raise ValueError("can only assign rotation to length 1 object") + so3 = SO3(r) + self.A[:3, :3] = so3.R + @property def t(self) -> R3: """ diff --git a/spatialmath/spline.py b/spatialmath/spline.py index ab35dcfe..bdc69086 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -80,16 +80,22 @@ def __call__(self, time:float): t: Normalized time value [0,1] to evaluate the spline at. """ - current_pose = SE3() + spline_no_coeff = BSpline.design_matrix([time], self.knots, self.degree) #the B in sum(alpha_i * B_i) = S(t) - rows,cols = spline_no_coeff.nonzero() + + current_so3 = SO3() + for row,col in zip(rows,cols): + control_so3: SO3 = SO3(self.control_poses[col]) + current_so3 = control_so3.interp1(spline_no_coeff[row,col]) * current_so3 + + xyz = np.array([0,0,0]) for row,col in zip(rows,cols): - control_pose: SE3 = self.control_poses[col] - current_pose = control_pose.interp1(spline_no_coeff[row,col]) * current_pose + control_point = self.control_poses[col].t + xyz = xyz + control_point*spline_no_coeff[row,col] - return current_pose + return SE3.Rt(t = xyz, R = current_so3) @classmethod def knots_from_num_control_poses(self, num_control_poses: int): @@ -135,14 +141,14 @@ def visualize( class FitCubicBSplineSE3: - def __init__(self, pose_data: List[SE3], timestamps) -> None: + def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int =6) -> None: """ Timestamps should be normalized to [0,1] and sorted. Outputs control poses, but the optimization is done on the [pos,axis-angle], flattened into a 1d vector. """ self.pose_data = pose_data self.timestamps = timestamps - self.num_control_pose = 4 + self.num_control_pose = num_control_points self.se3_rep_size = 6 #[x, y, z, axis-angle vec] # @@ -155,7 +161,7 @@ def __init__(self, pose_data: List[SE3], timestamps) -> None: closest_timestamp_indices[i] = len(timestamps) - 1 # - self.spline = CubicBSplineSE3(control_poses=[self.pose_data[index] for index in closest_timestamp_indices]) + self.spline = CubicBSplineSE3(control_poses=[SE3.CopyFrom(self.pose_data[index]) for index in closest_timestamp_indices]) @classmethod def make_SE3_pose(self, row: np.ndarray): @@ -169,8 +175,7 @@ def make_SE3_rep(self, pose: SE3): so3_twist = SO3(pose.R).log(twist=True) return np.concatenate([pose.t, so3_twist]) - - def objective_function_pos(self, pos: np.ndarray): + def objective_function_xyz(self, pos: np.ndarray): "L1 norm of SE3 distances between data points and spline" # data massage @@ -180,8 +185,7 @@ def objective_function_pos(self, pos: np.ndarray): # objective error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 0.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] - return np.linalg.norm(error_vector, ord=2) - + return np.linalg.norm(error_vector, ord=np.inf) def objective_function_so3(self, so3_twists_flat: np.ndarray): "L1 norm of SE3 distances between data points and spline" @@ -191,40 +195,58 @@ def objective_function_so3(self, so3_twists_flat: np.ndarray): spline = CubicBSplineSE3(control_poses=[SE3(SO3.Exp(row)) for row in so3_twists_matrix]) # objective - error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 0.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] + error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 1.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] - return np.linalg.norm(error_vector, ord=2) + return np.linalg.norm(error_vector, ord=np.inf) + def objective_function_pose(self, control_pose_rep: np.ndarray): + "L1 norm of SE3 distances between data points and spline" + + # data massage + control_pose_matrix = control_pose_rep.reshape(self.num_control_pose, self.se3_rep_size) + spline = CubicBSplineSE3(control_poses=[self.make_SE3_pose(row) for row in control_pose_matrix]) + + # objective + error_vector = [ weighted_average_SE3_metric(spline(t), pose) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] + + return np.linalg.norm(error_vector, ord=np.inf) + def fit(self, disp: bool = False): - pos_result = self.fit_pos(disp) so3_result = self.fit_so3(disp) - - control_pose_matrix = np.hstack([ - pos_result.x.reshape(self.num_control_pose, 3), - so3_result.x.reshape(self.num_control_pose, 3) - ]) + pos_result = self.fit_xyz(disp) - self.spline = CubicBSplineSE3(control_poses=[self.make_SE3_pose(row) for row in control_pose_matrix]) return pos_result, so3_result - def fit_pos(self, disp: bool = False): + def fit_xyz(self, disp: bool = False): - pos_flat = np.concatenate([pose.t for pose in self.spline.control_poses]) - result = minimize(self.objective_function_pos, pos_flat, method="slsqp", options = {"disp":disp}) + xyz_flat = np.concatenate([pose.t for pose in self.spline.control_poses]) + result = minimize(self.objective_function_xyz, xyz_flat, method="slsqp", options = {"disp":disp}) + xyz_mat = result.x.reshape(self.num_control_pose, 3) + for i, xyz in enumerate(xyz_mat): + self.spline.control_poses[i].t = xyz + return result def fit_so3(self, disp: bool = False): so3_twists_flat = np.concatenate([SO3(pose.R).log(twist=True)for pose in self.spline.control_poses]) result = minimize(self.objective_function_so3, so3_twists_flat, method="slsqp", options = {"disp":disp}) + so3_twists_mat = result.x.reshape(self.num_control_pose, 3) + for i, so3_twist in enumerate(so3_twists_mat): + self.spline.control_poses[i].R = SO3.Exp(so3_twist) return result + def ang_distance(self): + return [pose.angdist(self.spline(timestamp)) for pose, timestamp in zip(self.pose_data, self.timestamps)] + + def euclidean_distance(self): + return [np.linalg.norm(pose.t - self.spline(timestamp).t) for pose, timestamp in zip(self.pose_data, self.timestamps)] + def visualize( self, num_samples: int, length: float = 0.2, 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] = {}, @@ -235,40 +257,49 @@ def visualize( 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(1, 2, 1) + ax.plot(self.timestamps, self.ang_distance(), "--x") + ax.plot(self.timestamps, self.euclidean_distance(), "--x") + ax.legend(["angular distance", "euclidiean distance"]) + ax.set_xlabel("Normalized time") + + ax = fig.add_subplot(1, 2, 2, projection="3d") trplot( - [np.array(self.spline.control_poses)], ax=ax, length=length, color= "green" + [np.array(self.spline.control_poses)], ax=ax, length=length*1.2, color= "green" ) # plot control points + ax.plot(x, y, z, **kwargs_plot) # plot x,y,z trajectory trplot( [np.array(self.pose_data)], ax=ax, length=length, color= "cornflowerblue", - ) # plot control points - ax.plot(x, y, z, **kwargs_plot) # plot x,y,z trajectory + ) # plot data points tranimate( out_poses, repeat=repeat, length=length, **kwargs_tranimate ) # animate pose along trajectory -def example_bspline_fit_scipy(): - num_data_points = 33 +def example_bspline_fit(): + + num_data_points = 16 num_samples = 100 - frequency = 2 - scale = 2 + frequency = 0.5 + scale = 4 timestamps = np.linspace(0, 1, num_data_points) trajectory = [ - SE3.Rt(t = [t*scale, np.sin(t * 2*np.pi* frequency), np.sin(t * 2*np.pi * frequency)], R= SO3.Rx( t*2*np.pi* frequency)) + SE3.Rt(t = [t*scale, scale*np.sin(t * 2*np.pi* frequency), scale*np.cos(t * 2*np.pi * frequency)], + R= SO3.Rx( t*2*np.pi* frequency)) for t in timestamps ] - fit_se3_spline = FitCubicBSplineSE3(trajectory, timestamps) + fit_se3_spline = FitCubicBSplineSE3(trajectory, timestamps, num_control_points=6) result = fit_se3_spline.fit(disp=True) - fit_se3_spline.visualize(num_samples=num_samples, repeat=True) + fit_se3_spline.visualize(num_samples=num_samples, repeat=True, length=0.4, kwargs_tranimate={"wait": True, "interval" : 400}) if __name__ == "__main__": - example_bspline_fit_scipy() + example_bspline_fit() From 32b7af5fcd1b45b0f011fd5655e6ae65e20f65ec Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 18 Jul 2024 13:31:21 -0400 Subject: [PATCH 5/6] Clean up. --- spatialmath/spline copy.py | 535 ------------------------------------- spatialmath/spline.py | 137 ++++------ 2 files changed, 45 insertions(+), 627 deletions(-) delete mode 100644 spatialmath/spline copy.py diff --git a/spatialmath/spline copy.py b/spatialmath/spline copy.py deleted file mode 100644 index bcba4cad..00000000 --- a/spatialmath/spline copy.py +++ /dev/null @@ -1,535 +0,0 @@ -# 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, SO3 -import numpy as np -import matplotlib.pyplot as plt -from spatialmath.base.transforms3d import tranimate, trplot -from scipy.optimize import minimize -from scipy.interpolate import splrep - - -def weighted_average_SE3_metric(a: Twist3 | SE3, b: Twist3 | SE3, w: float = 0.5) -> float: - """A positive definite distance metric for SE3. - - The metric is: (1-w)*translation_distance + w*angular_distance - - Note that there isn't a - great "natural" choice for a metric on SE3 (see Appendix A.3.2 in - 'A Mathematical Introduction to Robotic Manipulation' by Murray, Li, Sastry) - - spatialmath has options for the rotational metrics from - 'Metrics for 3D Rotations: Comparison and Analysis' by Du Q. Huynh. - This uses the 'best choice' from that paper. - - Args: - a: a twist from SE3.twist, or an SE3 object - b: a twist from SE3.twist, or an SE3 object - w: a float that represents the relative weighting between the rotational and translation distances. - - Returns: - a float for the 'distance' between two SE3 poses - - """ - if w> 1 or w < 0: - raise ValueError(f"Weight w={w} is outside the range [0,1].") - - if isinstance(a, Twist3): - a = SE3.Exp(a) - - if isinstance(b, Twist3): - b = SE3.Exp(b) - - # if np.linalg.norm(a - b) < 1e-6: - # return 0.0 - - angular_distance = a.angdist(b) - translation_distance = np.linalg.norm(a.t - b.t) - - return (1 - w) * translation_distance + w * angular_distance - - -class TrajectoryTwist: - - def __init__(self, poses: List[SE3], timestamps: List[float]) -> None: - self.poses = poses - self.timestamps = timestamps - self.twists = [] - - def metric(self, target_pose: SE3, seed_twist: Twist3): - return weighted_average_SE3_metric(target_pose, SE3.Exp(seed_twist)) - - def fit(self): - twists = [] - for pose in self.poses: - if len(twists) == 0: - twists.append(pose.twist()) - else: - f = lambda x: self.metric(pose, x) - twists.append(Twist3(minimize(f, twists[-1].A).x)) - - self.twists = twists - - def visualize(self): - fig = plt.figure(figsize=(40, 20)) - ax = fig.add_subplot(1, 2, 1, projection="3d") - trplot( - [np.array(self.poses)], - ax=ax, - length=0.2 - ) # plot control points - # import ipdb; ipdb.set_trace() - trplot( - [np.array([twist.exp() for twist in self.twists])], - ax=ax, - length=0.2, - color="cornflowerblue", - ) # plot data points - twist_data = np.vstack([twist.A for twist in self.twists]) - ax_2: plt.Axes = fig.add_subplot(1, 2, 2) - ax_2.plot( - self.timestamps, - twist_data[:, 0], - "o", - self.timestamps, - twist_data[:, 1], - "o", - self.timestamps, - twist_data[:, 2], - "o", - self.timestamps, - twist_data[:, 3], - "o", - self.timestamps, - twist_data[:, 4], - "o", - self.timestamps, - twist_data[:, 5], - "o", - ) - plt.show() - - -class CubicBSplineSE3: - """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_poses: List[SE3], - ) -> 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.basis_coefficent_matrix = 1/6*np.array([ - [6, 0, 0, 0], - [5, 3, -3, 1], - [1, 3, 3, -2], - [0, 0, 0, 1] - ]) - self.n = len(control_poses) - self.control_poses = control_poses - self.degree = 3 - - # use open uniform knot vector - knots = np.linspace(0, 1, len(control_poses)-2, endpoint=True) - knots = np.append( - [0] * self.degree, knots - ) # ensures the curve starts on the first control pose - knots = np.append( - knots, [1] * self.degree - ) # ensures the curve ends on the last control pose - self.knots = knots - - - def __call__(self, time:float): - """Returns pose of spline at t. - - t: Normalized time value [0,1] to evaluate the spline at. - """ - current_pose = SE3() - - spline_no_coeff = BSpline.design_matrix([time], self.knots, self.degree) #the B in sum(alpha_i * B_i) = S(t) - - rows,cols = spline_no_coeff.nonzero() - for row,col in zip(rows,cols): - current_pose = self.control_poses[col].interp1(spline_no_coeff[row,col]) * current_pose - - - return current_pose - - def visualize( - self, - num_samples: int, - length: float = 0.2, - 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") - - 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 - - -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 - - -class FitBSplineTwist3: - - def __init__(self, twist_data: np.ndarray, timestamps, knots) -> None: - """twist data is nx6 matrix where a row is a twist""" - - self.twist_data = twist_data - self.pose_data = [SE3.Exp(twist) for twist in twist_data] - self.timestamps = timestamps - self.spline = BSplineTwist3([Twist3() for i in range(0,8)]) - self.knots = knots - self.degree = 3 - self.norm_ratio = 0.0 - - t = np.array(self.timestamps) - t = t - t[0] # shift start to 0 - t = t / t[-1] # normalize range to [0,1] - - self.normalized_timestamps = t - - - def update_control_twists(self, control_twists: np.array): - """control_twists is nx6 matrix where a row is a twist""" - self.spline = BSplineTwist3([row for row in control_twists]) - - def L1_over_euclidian_distance_error(self, control_twists: Optional[np.array] = None): - """ control twists are input as a vector from a flattened nx6 matrix here - """ - if control_twists is not None: - control_twists = control_twists.reshape(int(len(control_twists)/6), 6) - self.update_control_twists(control_twists) - - distances = np.array([np.linalg.norm(SE3.Exp(self.spline(timestamp)).t - pose.t) for timestamp, pose in zip(self.timestamps,self.pose_data, strict=True)]) - return np.linalg.norm(distances, ord=np.inf) - - def L1_over_SE3_metric_error(self, control_twists: Optional[np.array] = None): - """ control twists are input as a vector from a flattened nx6 matrix here - """ - if control_twists is not None: - control_twists = control_twists.reshape(int(len(control_twists)/6), 6) - self.update_control_twists(control_twists) - - distances = np.array([weighted_average_SE3_metric(SE3.Exp(self.spline(timestamp)), pose, self.norm_ratio) for timestamp, pose in zip(self.timestamps,self.pose_data, strict=True)]) - return np.linalg.norm(distances, ord=np.inf) - - def fit2(self): - result = minimize(self.L1_over_SE3_metric_error, self.spline.control_twist_matrix.flatten()) - self.update_control_twists(result.x.reshape(int(len(result.x)/6), 6)) - - def fit(self) -> None: - """Fits a b spline to the input SE3 trajectory and outputs control poses. - - Assumes timestamps are monotonically increasing / are sorted. - """ - # data_as_twists = np.vstack([np.array(pose.twist()) for pose in self.twist_data]) - # import ipdb; ipdb.set_trace() - control_twists = np.vstack( - [ - splrep(self.normalized_timestamps, self.twist_data[:, i], k=self.degree, t=self.knots)[1][ - 0 : len(self.knots) + self.degree + 1 - ] - for i in range(6) - ] - ).T - - self.spline = BSplineTwist3([row for row in control_twists]) - - def visualize( - self, - num_samples: int, - length: float = 0.5, - repeat: bool = True, - kwargs_trplot: Dict[str, Any] = {"color": "green"}, - kwargs_tranimate: Dict[str, Any] = {"wait": True, "dims": [-5, 5, -5, 5, -5, 5]}, - kwargs_plot: Dict[str, Any] = {}, - ) -> None: - """Displays an animation of the trajectory with the control poses.""" - - fig = plt.figure(figsize=(40, 20)) - ax_1: plt.Axes = fig.add_subplot(1, 2, 1) - - times = np.linspace(0, 1, num_samples) - - fit_twists = np.vstack([self.spline(t) for t in times]) - # fit_twists = np.vstack([pose.UnitQuaternion().A for pose in fit_poses]) - ax_1.plot( - times, - fit_twists[:, 0], - times, - fit_twists[:, 1], - times, - fit_twists[:, 2], - times, - fit_twists[:, 3], - times, - fit_twists[:, 4], - times, - fit_twists[:, 5], - ) - - # pose_data_twists = np.vstack([pose.twist().A for pose in self.twist_data]) - ax_1.plot( - self.timestamps, - self.twist_data[:, 0], - "o", - self.timestamps, - self.twist_data[:, 1], - "o", - self.timestamps, - self.twist_data[:, 2], - "o", - self.timestamps, - self.twist_data[:, 3], - "o", - self.timestamps, - self.twist_data[:, 4], - "o", - self.timestamps, - self.twist_data[:, 5], - "o", - ) - ax_1.plot( - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 0], - "x", - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 1], - "x", - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 2], - "x", - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 3], - "x", - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 4], - "x", - self.spline.knots[self.degree-1:-self.degree+1], - self.spline.control_twist_matrix[:, 5], - "x", - ) - ax_1.legend( - labels=[ - "x_fit", - "y_fit", - "z_fit", - "x_rot_fit", - "y_rot_fit", - "z_rot_fit", - "x_true", - "y_true", - "z_true", - "x_rot_true", - "y_rot_true", - "z_rot_true", - "x_control", - "y_control", - "z_control", - "x_rot_control", - "y_rot_control", - "z_rot_control", - ] - ) - - ax_2: plt.Axes = fig.add_subplot(1, 2, 2, projection="3d") - # ax_2.plot(x, y, z, **kwargs_plot) # plot x,y,z trajectory - # import ipdb; ipdb.set_trace() - trplot( - [np.array([SE3.Exp(twist) for twist in self.spline.control_twists])], - ax=ax_2, - length=length, - **kwargs_trplot, - ) # plot control points - trplot( - [np.array([SE3.Exp(twist) for twist in self.twist_data])], - ax=ax_2, - length=0.25 * length, - color="cornflowerblue", - ) # plot data points - - fit_poses = [SE3.Exp(twist) for twist in fit_twists] - # for pose in fit_poses: - # print(pose.ishom()) - tranimate(fit_poses, repeat=repeat, length=length, **kwargs_tranimate) # animate pose along trajectory - - -def example_bspline_fit_scipy(): - - num_data_points = 9 - num_samples = 100 - frequency = 2 - scale = 2 - - timestamps = np.linspace(0, 1, num_data_points) - x = np.array([t for t in timestamps]) - trajectory = [ - SE3.Rt(t = [e, np.sin(e * 2*np.pi* frequency), np.sin(e * 2*np.pi * frequency)], R= SO3.Rx( e*2*np.pi* frequency)) - for e in x - ] - print("Control points") - for pose in trajectory: - print(pose) - - se3_spline = CubicBSplineSE3(control_poses=trajectory) - # print("Spline") - # for knot in np.unique(se3_spline.knots): - # print(se3_spline.eval_spline(knot-0.001)) - # print(se3_spline.eval_spline(knot+0.001)) - - # import ipdb; ipdb.set_trace() - se3_spline.visualize(num_samples=100, repeat=True) - - # import ipdb; ipdb.set_trace() - # trajectory = [ - # SE3.Rt( - # t=[scale * t, scale * np.cos(t * frequency * np.pi), scale * np.cos(t * frequency * np.pi)], - # R=SO3.Ry(t * 1.5 * frequency * np.pi), - # ) - # for t in timestamps - # ] - # delta_trajectories = np.vstack([trajectory[i+1].delta(trajectory[i]) for i in range(0,len(trajectory)-1)]) - - # trajectory = np.vstack( - # [ - # Twist3( - # [ - # scale * t, - # scale * np.cos(t * frequency * np.pi), - # scale * np.sin(t * frequency * np.pi), - # t * 1.5 * frequency * np.pi, - # 1, - # 2, - # ] - # ).A - # for t in timestamps - # ] - # ) - - # num_knots = 4 - # knots = np.linspace(0, 1, num_knots + 2)[1:-1] - # print(f"knots: {knots}") - - # fit = FitBSplineTwist3(twist_data=delta_trajectories, timestamps=timestamps[0:-1], knots=knots) - # fit.pose_data = trajectory - # fit.fit() - # print(fit.L1_euclidian_distance_error()) - # fit.visualize(num_samples=num_samples) - # fit.norm_ratio = 0.0 - # fit.fit2() - # fit.norm_ratio = 0.5 - # fit.fit2() - # print(fit.L1_over_euclidian_distance_error()) - - # fit.visualize(num_samples=num_samples, repeat=True) - - # traj_class = TrajectoryTwist(poses=trajectory, timestamps=timestamps) - # traj_class.fit() - # import ipdb; ipdb.set_trace() - # traj_class.visualize() - - -if __name__ == "__main__": - example_bspline_fit_scipy() diff --git a/spatialmath/spline.py b/spatialmath/spline.py index bdc69086..82bc3aee 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -17,45 +17,13 @@ from scipy.interpolate import splrep -def weighted_average_SE3_metric(a: SE3, b: SE3, w: float = 0.5) -> float: - """A positive definite distance metric for SE3. - - The metric is: (1-w)*translation_distance + w*angular_distance - - Note that there isn't a - great "natural" choice for a metric on SE3 (see Appendix A.3.2 in - 'A Mathematical Introduction to Robotic Manipulation' by Murray, Li, Sastry) - - spatialmath has options for the rotational metrics from - 'Metrics for 3D Rotations: Comparison and Analysis' by Du Q. Huynh. - This uses the 'best choice' from that paper. - - Args: - a: a twist from SE3.twist, or an SE3 object - b: a twist from SE3.twist, or an SE3 object - w: a float that represents the relative weighting between the rotational and translation distances. - - Returns: - a float for the 'distance' between two SE3 poses - - """ - if w> 1 or w < 0: - raise ValueError(f"Weight w={w} is outside the range [0,1].") - - # if np.linalg.norm(a - b) < 1e-6: - # return 0.0 - - angular_distance = a.angdist(b) - translation_distance = np.linalg.norm(a.t - b.t) - - return (1 - w) * translation_distance + w * angular_distance - class CubicBSplineSE3: - """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline. + """A class to parameterize a trajectory in SE3 with a cubic 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 position and orientation are calculated disjointly. The position uses the + "classic" B-spline formulation. The orientation calculation is based on + interpolation between the identity element and the control SO3 element, them applying + each interpolated SO3 element as a group action. For detailed information about B-splines, please see this wikipedia article. https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline @@ -66,8 +34,7 @@ def __init__( self, control_poses: List[SE3], ) -> None: - """Construct BSplineSE3 object. The default arguments generate a cubic B-spline - with a open uniform knot vector. + """Construct a CubicBSplineSE3 object with a open uniform knot vector. - control_poses: list of SE3 objects that govern the shape of the spline. """ @@ -80,8 +47,7 @@ def __call__(self, time:float): t: Normalized time value [0,1] to evaluate the spline at. """ - - + spline_no_coeff = BSpline.design_matrix([time], self.knots, self.degree) #the B in sum(alpha_i * B_i) = S(t) rows,cols = spline_no_coeff.nonzero() @@ -99,6 +65,7 @@ def __call__(self, time:float): @classmethod def knots_from_num_control_poses(self, num_control_poses: int): + """ Return open uniform knots vector based on number of control poses. """ # use open uniform knot vector knots = np.linspace(0, 1, num_control_poses-2, endpoint=True) knots = np.append( @@ -141,7 +108,7 @@ def visualize( class FitCubicBSplineSE3: - def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int =6) -> None: + def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int = 6, method = "slsqp") -> None: """ Timestamps should be normalized to [0,1] and sorted. Outputs control poses, but the optimization is done on the [pos,axis-angle], flattened into a 1d vector. @@ -149,9 +116,9 @@ def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int =6) self.pose_data = pose_data self.timestamps = timestamps self.num_control_pose = num_control_points - self.se3_rep_size = 6 #[x, y, z, axis-angle vec] + self.method = method - # + # initialize control poses with data points sample_points = np.linspace(self.timestamps[0], self.timestamps[-1], self.num_control_pose) closest_timestamp_indices = np.searchsorted(self.timestamps, sample_points) for i, index in enumerate(closest_timestamp_indices): @@ -159,87 +126,73 @@ def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int =6) closest_timestamp_indices[i] = 0 if index >= len(timestamps): closest_timestamp_indices[i] = len(timestamps) - 1 - - # + self.spline = CubicBSplineSE3(control_poses=[SE3.CopyFrom(self.pose_data[index]) for index in closest_timestamp_indices]) - @classmethod - def make_SE3_pose(self, row: np.ndarray): - t = row[0:3] - so3_twist = row[3:6] - - return SE3.Rt(t = t, R = SO3.Exp(so3_twist)) - - @classmethod - def make_SE3_rep(self, pose: SE3): - so3_twist = SO3(pose.R).log(twist=True) - return np.concatenate([pose.t, so3_twist]) - - def objective_function_xyz(self, pos: np.ndarray): - "L1 norm of SE3 distances between data points and spline" + def objective_function_xyz(self, xyz_flat: np.ndarray): + """L-infinity norm of euclidean distance between data points and spline""" # data massage - pos_matrix = pos.reshape(self.num_control_pose, 3) - spline = CubicBSplineSE3(control_poses=[SE3.Trans(row) for row in pos_matrix]) + self._assign_xyz_to_control_poses(xyz_flat) # objective - error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 0.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] - + error_vector = self.euclidean_distance() return np.linalg.norm(error_vector, ord=np.inf) def objective_function_so3(self, so3_twists_flat: np.ndarray): - "L1 norm of SE3 distances between data points and spline" + """L-infinity norm of angular distance between data points and spline""" # data massage - so3_twists_matrix = so3_twists_flat.reshape(self.num_control_pose, 3) - spline = CubicBSplineSE3(control_poses=[SE3(SO3.Exp(row)) for row in so3_twists_matrix]) + self._assign_so3_twist_to_control_poses(so3_twists_flat) # objective - error_vector = [ weighted_average_SE3_metric(spline(t), pose, w = 1.0) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] - + error_vector = self.ang_distance() return np.linalg.norm(error_vector, ord=np.inf) - def objective_function_pose(self, control_pose_rep: np.ndarray): - "L1 norm of SE3 distances between data points and spline" - - # data massage - control_pose_matrix = control_pose_rep.reshape(self.num_control_pose, self.se3_rep_size) - spline = CubicBSplineSE3(control_poses=[self.make_SE3_pose(row) for row in control_pose_matrix]) - - # objective - error_vector = [ weighted_average_SE3_metric(spline(t), pose) for t,pose in zip(self.timestamps, self.pose_data, strict=True) ] - - return np.linalg.norm(error_vector, ord=np.inf) - def fit(self, disp: bool = False): - + """ Find the spline control points that minimize the distance from the spline to the data points. + """ so3_result = self.fit_so3(disp) pos_result = self.fit_xyz(disp) return pos_result, so3_result def fit_xyz(self, disp: bool = False): - + """ Solve fitting problem for x,y,z coordinates. + """ xyz_flat = np.concatenate([pose.t for pose in self.spline.control_poses]) - result = minimize(self.objective_function_xyz, xyz_flat, method="slsqp", options = {"disp":disp}) - xyz_mat = result.x.reshape(self.num_control_pose, 3) - for i, xyz in enumerate(xyz_mat): - self.spline.control_poses[i].t = xyz + result = minimize(self.objective_function_xyz, xyz_flat, method=self.method, options = {"disp":disp}) + self._assign_xyz_to_control_poses(result.x) return result def fit_so3(self, disp: bool = False): - so3_twists_flat = np.concatenate([SO3(pose.R).log(twist=True)for pose in self.spline.control_poses]) - result = minimize(self.objective_function_so3, so3_twists_flat, method="slsqp", options = {"disp":disp}) - so3_twists_mat = result.x.reshape(self.num_control_pose, 3) + """ Solve fitting problem for SO3 coordinates. + """ + so3_twists_flat = np.concatenate([SO3(pose.R).log(twist=True) for pose in self.spline.control_poses]) + result = minimize(self.objective_function_so3, so3_twists_flat, method = self.method, options = {"disp":disp}) + self._assign_so3_twist_to_control_poses(result.x) + + return result + + def _assign_xyz_to_control_poses(self, xyz_flat: np.ndarray) -> None: + xyz_mat = xyz_flat.reshape(self.num_control_pose, 3) + for i, xyz in enumerate(xyz_mat): + self.spline.control_poses[i].t = xyz + + def _assign_so3_twist_to_control_poses(self, so3_twists_flat: np.ndarray) -> None: + so3_twists_mat = so3_twists_flat.reshape(self.num_control_pose, 3) for i, so3_twist in enumerate(so3_twists_mat): self.spline.control_poses[i].R = SO3.Exp(so3_twist) - return result def ang_distance(self): + """ Returns vector of angular distance between spline and data points. + """ return [pose.angdist(self.spline(timestamp)) for pose, timestamp in zip(self.pose_data, self.timestamps)] def euclidean_distance(self): + """ Returns vector of euclidean distance between spline and data points. + """ return [np.linalg.norm(pose.t - self.spline(timestamp).t) for pose, timestamp in zip(self.pose_data, self.timestamps)] def visualize( @@ -251,7 +204,7 @@ def visualize( kwargs_tranimate: Dict[str, Any] = {"wait": True}, kwargs_plot: Dict[str, Any] = {}, ) -> None: - """Displays an animation of the trajectory with the control poses.""" + """Displays an animation of the trajectory with the control poses and data points.""" out_poses = [self.spline(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] From 8cc56b8e512984bb08c6dec6fc4db54faef2929f Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 18 Jul 2024 13:44:24 -0400 Subject: [PATCH 6/6] Move example to unit test. --- spatialmath/__init__.py | 3 ++- spatialmath/spline.py | 34 ++++++---------------------------- tests/test_spline.py | 36 ++++++++++++++++++++++++++++++++---- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py index 01156c7f..5a329f58 100644 --- a/spatialmath/__init__.py +++ b/spatialmath/__init__.py @@ -15,7 +15,7 @@ ) from spatialmath.quaternion import Quaternion, UnitQuaternion from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion -from spatialmath.spline import CubicBSplineSE3 +from spatialmath.spline import CubicBSplineSE3, FitCubicBSplineSE3 # from spatialmath.Plucker import * # from spatialmath import base as smb @@ -45,6 +45,7 @@ "Polygon2", "Ellipse", "CubicBSplineSE3", + "FitCubicBSplineSE3" ] try: diff --git a/spatialmath/spline.py b/spatialmath/spline.py index 82bc3aee..00823c1d 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -129,21 +129,23 @@ def __init__(self, pose_data: List[SE3], timestamps, num_control_points: int = 6 self.spline = CubicBSplineSE3(control_poses=[SE3.CopyFrom(self.pose_data[index]) for index in closest_timestamp_indices]) - def objective_function_xyz(self, xyz_flat: np.ndarray): + def objective_function_xyz(self, xyz_flat: Optional[np.ndarray] = None): """L-infinity norm of euclidean distance between data points and spline""" # data massage - self._assign_xyz_to_control_poses(xyz_flat) + if xyz_flat is not None: + self._assign_xyz_to_control_poses(xyz_flat) # objective error_vector = self.euclidean_distance() return np.linalg.norm(error_vector, ord=np.inf) - def objective_function_so3(self, so3_twists_flat: np.ndarray): + def objective_function_so3(self, so3_twists_flat: Optional[np.ndarray] = None): """L-infinity norm of angular distance between data points and spline""" # data massage - self._assign_so3_twist_to_control_poses(so3_twists_flat) + if so3_twists_flat is not None: + self._assign_so3_twist_to_control_poses(so3_twists_flat) # objective error_vector = self.ang_distance() @@ -232,27 +234,3 @@ def visualize( tranimate( out_poses, repeat=repeat, length=length, **kwargs_tranimate ) # animate pose along trajectory - - -def example_bspline_fit(): - - num_data_points = 16 - num_samples = 100 - frequency = 0.5 - scale = 4 - - timestamps = np.linspace(0, 1, num_data_points) - trajectory = [ - SE3.Rt(t = [t*scale, scale*np.sin(t * 2*np.pi* frequency), scale*np.cos(t * 2*np.pi * frequency)], - R= SO3.Rx( t*2*np.pi* frequency)) - for t in timestamps - ] - - fit_se3_spline = FitCubicBSplineSE3(trajectory, timestamps, num_control_points=6) - - result = fit_se3_spline.fit(disp=True) - fit_se3_spline.visualize(num_samples=num_samples, repeat=True, length=0.4, kwargs_tranimate={"wait": True, "interval" : 400}) - - -if __name__ == "__main__": - example_bspline_fit() diff --git a/tests/test_spline.py b/tests/test_spline.py index 05f23508..edd71ec2 100644 --- a/tests/test_spline.py +++ b/tests/test_spline.py @@ -2,10 +2,7 @@ import numpy as np import matplotlib.pyplot as plt import unittest -import sys -import pytest - -from spatialmath import CubicBSplineSE3, SE3 +from spatialmath import FitCubicBSplineSE3, CubicBSplineSE3, SE3, SO3 class TestBSplineSE3(unittest.TestCase): @@ -30,3 +27,34 @@ def test_evaluation(self): def test_visualize(self): spline = CubicBSplineSE3(self.control_poses) spline.visualize(num_samples=100, repeat=False) + +class TestFitBSplineSE3(unittest.TestCase): + + num_data_points = 16 + num_samples = 100 + num_control_points = 6 + + timestamps = np.linspace(0, 1, num_data_points) + trajectory = [ + SE3.Rt(t = [t*4, 4*np.sin(t * 2*np.pi* 0.5), 4*np.cos(t * 2*np.pi * 0.5)], + R= SO3.Rx( t*2*np.pi* 0.5)) + for t in timestamps + ] + + @classmethod + def tearDownClass(cls): + plt.close("all") + + def test_constructor(self): + pass + + def test_evaluation_and_visualization(self): + fit_se3_spline = FitCubicBSplineSE3(self.trajectory, self.timestamps, num_control_points=self.num_control_points) + + result = fit_se3_spline.fit(disp=True) + + assert len(result) == 2 + assert fit_se3_spline.objective_function_xyz() < 0.01 + assert fit_se3_spline.objective_function_so3() < 0.01 + + fit_se3_spline.visualize(num_samples=self.num_samples, repeat=False, length=0.4, kwargs_tranimate={"wait": True, "interval" : 10}) \ No newline at end of file