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 pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy