From 6074eb7d0a66981b51386a13441d3077a40bcfb2 Mon Sep 17 00:00:00 2001 From: Mark Yeatman <129521731+myeatman-bdai@users.noreply.github.com> Date: Tue, 9 Jul 2024 14:59:47 -0400 Subject: [PATCH 1/4] Add BSplineSE3 class. (#128) --- spatialmath/__init__.py | 2 + spatialmath/base/animate.py | 8 +-- spatialmath/spline.py | 105 ++++++++++++++++++++++++++++++++++++ tests/test_spline.py | 32 +++++++++++ 4 files changed, 144 insertions(+), 3 deletions(-) create mode 100644 spatialmath/spline.py create mode 100644 tests/test_spline.py diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py index 63287dcf..e6ef1f77 100644 --- a/spatialmath/__init__.py +++ b/spatialmath/__init__.py @@ -15,6 +15,7 @@ ) from spatialmath.quaternion import Quaternion, UnitQuaternion from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion +from spatialmath.spline import BSplineSE3 # from spatialmath.Plucker import * # from spatialmath import base as smb @@ -43,6 +44,7 @@ "LineSegment2", "Polygon2", "Ellipse", + "BSplineSE3", ] try: diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py index 8efa3a4e..3876a2ea 100755 --- a/spatialmath/base/animate.py +++ b/spatialmath/base/animate.py @@ -212,6 +212,7 @@ def update(frame, animation): # assume it is an SO(3) or SE(3) T = frame # ensure result is SE(3) + if T.shape == (3, 3): T = smb.r2t(T) @@ -308,7 +309,7 @@ def __init__(self, anim: Animate, h, xs, ys, zs): self.anim = anim def draw(self, T): - p = T @ self.p + p = T.A @ self.p self.h.set_data(p[0, :], p[1, :]) self.h.set_3d_properties(p[2, :]) @@ -365,7 +366,8 @@ def __init__(self, anim, h): self.anim = anim def draw(self, T): - p = T @ self.p + # import ipdb; ipdb.set_trace() + p = T.A @ self.p # reshape it p = p[0:3, :].T.reshape(3, 2, 3) @@ -419,7 +421,7 @@ def __init__(self, anim, h, x, y, z): self.anim = anim def draw(self, T): - p = T @ self.p + p = T.A @ self.p # x2, y2, _ = proj3d.proj_transform( # p[0], p[1], p[2], self.anim.ax.get_proj()) # self.h.set_position((x2, y2)) diff --git a/spatialmath/spline.py b/spatialmath/spline.py new file mode 100644 index 00000000..f81dcec3 --- /dev/null +++ b/spatialmath/spline.py @@ -0,0 +1,105 @@ +# 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 +import numpy as np +import matplotlib.pyplot as plt +from spatialmath.base.transforms3d import tranimate, trplot + + +class BSplineSE3: + """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], + 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_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: + """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) + + def visualize( + self, + num_samples: int, + length: float = 1.0, + 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 diff --git a/tests/test_spline.py b/tests/test_spline.py new file mode 100644 index 00000000..f518fcfb --- /dev/null +++ b/tests/test_spline.py @@ -0,0 +1,32 @@ +import numpy.testing as nt +import numpy as np +import matplotlib.pyplot as plt +import unittest +import sys +import pytest + +from spatialmath import BSplineSE3, SE3 + + +class TestBSplineSE3(unittest.TestCase): + control_poses = [ + SE3.Trans([e, 2 * np.cos(e / 2 * np.pi), 2 * np.sin(e / 2 * np.pi)]) + * SE3.Ry(e / 8 * np.pi) + for e in range(0, 8) + ] + + @classmethod + def tearDownClass(cls): + plt.close("all") + + def test_constructor(self): + BSplineSE3(self.control_poses) + + def test_evaluation(self): + spline = BSplineSE3(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.visualize(num_samples=100, repeat=False) From 53c0ee145123b9a2e694028b7efd7745fc9dcc22 Mon Sep 17 00:00:00 2001 From: Jien Cao <135634522+jcao-bdai@users.noreply.github.com> Date: Tue, 23 Jul 2024 15:09:27 -0400 Subject: [PATCH 2/4] Fixes several issues that broke the testing workflow (#132) --- spatialmath/base/graphics.py | 6 +++--- tests/base/test_symbolic.py | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 0fe40431..2ce18dc8 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1394,9 +1394,9 @@ def plot_cuboid( edges = [[0, 1, 3, 2, 0], [4, 5, 7, 6, 4], [0, 4], [1, 5], [3, 7], [2, 6]] lines = [] for edge in edges: - E = vertices[:, edge] - # ax.plot(E[0], E[1], E[2], **kwargs) - lines.append(E.T) + for line in zip(edge[:-1], edge[1:]): + E = vertices[:, line] + lines.append(E.T) if "color" in kwargs: if "alpha" in kwargs: alpha = kwargs["alpha"] diff --git a/tests/base/test_symbolic.py b/tests/base/test_symbolic.py index 3ff26f56..7a503b5b 100644 --- a/tests/base/test_symbolic.py +++ b/tests/base/test_symbolic.py @@ -58,26 +58,26 @@ def test_functions(self): self.assertTrue(isinstance(sqrt(1.0), float)) x = (theta - 1) * (theta + 1) - theta ** 2 - self.assertEqual(simplify(x).evalf(), -1) + self.assertTrue(math.isclose(simplify(x).evalf(), -1)) @unittest.skipUnless(_symbolics, "sympy required") def test_constants(self): x = zero() self.assertTrue(isinstance(x, sp.Expr)) - self.assertEqual(x.evalf(), 0) + self.assertTrue(math.isclose(x.evalf(), 0)) x = one() self.assertTrue(isinstance(x, sp.Expr)) - self.assertEqual(x.evalf(), 1) + self.assertTrue(math.isclose(x.evalf(), 1)) x = negative_one() self.assertTrue(isinstance(x, sp.Expr)) - self.assertEqual(x.evalf(), -1) + self.assertTrue(math.isclose(x.evalf(), -1)) x = pi() self.assertTrue(isinstance(x, sp.Expr)) - self.assertEqual(x.evalf(), math.pi) + self.assertTrue(math.isclose(x.evalf(), math.pi)) # ---------------------------------------------------------------------------------------# From d6134bf7702df1f9e50f89917220b8b60b9b6279 Mon Sep 17 00:00:00 2001 From: Jien Cao <135634522+jcao-bdai@users.noreply.github.com> Date: Mon, 29 Jul 2024 14:31:54 -0400 Subject: [PATCH 3/4] [Issue-126] fix several issues with animate (#131) Fix several issues related to https://github.com/bdaiinstitute/spatialmath-python/issues/126 - expose "ax" and "dims" arguments to animate(...), to be consistent with other API's; - update animate implementation to be consistent with trajectory frame's data type, in particular, SE3Array / SO3Array types instead of SE3 / SO3, in line with animate.run(...)'s implementation. --- spatialmath/base/animate.py | 25 +++++++++++++++++-------- spatialmath/base/transforms2d.py | 9 +++------ spatialmath/base/transforms3d.py | 9 +++------ 3 files changed, 23 insertions(+), 20 deletions(-) diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py index 3876a2ea..7654a5a0 100755 --- a/spatialmath/base/animate.py +++ b/spatialmath/base/animate.py @@ -104,6 +104,15 @@ def __init__( # ax.set_zlim(dims[4:6]) # # ax.set_aspect('equal') ax = smb.plotvol3(ax=ax, dim=dim) + if dim is not None: + dim = list(np.ndarray.flatten(np.array(dim))) + if len(dim) == 2: + dim = dim * 3 + elif len(dim) != 6: + raise ValueError(f"dim must have 2 or 6 elements, got {dim}. See docstring for details.") + ax.set_xlim(dim[0:2]) + ax.set_ylim(dim[2:4]) + ax.set_zlim(dim[4:]) self.ax = ax @@ -208,10 +217,12 @@ def update(frame, animation): if isinstance(frame, float): # passed a single transform, interpolate it T = smb.trinterp(start=self.start, end=self.end, s=frame) - else: - # assume it is an SO(3) or SE(3) + elif isinstance(frame, NDArray): + # type is SO3Array or SE3Array when Animate.trajectory is not None T = frame - # ensure result is SE(3) + else: + # [unlikely] other types are converted to np array + T = np.array(frame) if T.shape == (3, 3): T = smb.r2t(T) @@ -309,7 +320,7 @@ def __init__(self, anim: Animate, h, xs, ys, zs): self.anim = anim def draw(self, T): - p = T.A @ self.p + p = T @ self.p self.h.set_data(p[0, :], p[1, :]) self.h.set_3d_properties(p[2, :]) @@ -367,7 +378,7 @@ def __init__(self, anim, h): def draw(self, T): # import ipdb; ipdb.set_trace() - p = T.A @ self.p + p = T @ self.p # reshape it p = p[0:3, :].T.reshape(3, 2, 3) @@ -421,7 +432,7 @@ def __init__(self, anim, h, x, y, z): self.anim = anim def draw(self, T): - p = T.A @ self.p + p = T @ self.p # x2, y2, _ = proj3d.proj_transform( # p[0], p[1], p[2], self.anim.ax.get_proj()) # self.h.set_position((x2, y2)) @@ -546,8 +557,6 @@ def __init__( axes.set_xlim(dims[0:2]) axes.set_ylim(dims[2:4]) # ax.set_aspect('equal') - else: - axes.autoscale(enable=True, axis="both") self.ax = axes diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index c64ed036..682ea0ca 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -1510,12 +1510,9 @@ def tranimate2(T: Union[SO2Array, SE2Array], **kwargs): tranimate2(transl(1,2)@trot2(1), frame='A', arrow=False, dims=[0, 5]) tranimate2(transl(1,2)@trot2(1), frame='A', arrow=False, dims=[0, 5], movie='spin.mp4') """ - anim = smb.animate.Animate2(**kwargs) - try: - del kwargs["dims"] - except KeyError: - pass - + dims = kwargs.pop("dims", None) + ax = kwargs.pop("ax", None) + anim = smb.animate.Animate2(dims=dims, axes=ax, **kwargs) anim.trplot2(T, **kwargs) return anim.run(**kwargs) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index ceff8732..3617f965 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -3409,12 +3409,9 @@ def tranimate(T: Union[SO3Array, SE3Array], **kwargs) -> str: :seealso: `trplot`, `plotvol3` """ - anim = Animate(**kwargs) - try: - del kwargs["dims"] - except KeyError: - pass - + dim = kwargs.pop("dims", None) + ax = kwargs.pop("ax", None) + anim = Animate(dim=dim, ax=ax, **kwargs) anim.trplot(T, **kwargs) return anim.run(**kwargs) From 858601d2b4693988decae2c52f92bb4b0d3cee58 Mon Sep 17 00:00:00 2001 From: Jien Cao <135634522+jcao-bdai@users.noreply.github.com> Date: Thu, 1 Aug 2024 08:50:02 -0400 Subject: [PATCH 4/4] Bump version to 1.1.11 in prep to cut a release (#134) --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 9e309b81..83bf1050 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "spatialmath-python" -version = "1.1.10" +version = "1.1.11" authors = [ { name="Peter Corke", email="rvc@petercorke.com" }, ]