From 8a16cfc1e94a19a7391d037b0f0269041f1e7dc9 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 2 Mar 2023 14:59:43 +0100 Subject: [PATCH 1/6] Add slerp function --- src/liecasadi/quaternion.py | 41 +++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py index 3dc3028..cd4f2bd 100644 --- a/src/liecasadi/quaternion.py +++ b/src/liecasadi/quaternion.py @@ -3,6 +3,7 @@ # GNU Lesser General Public License v2.1 or any later version. import dataclasses +from typing import List import casadi as cs @@ -45,6 +46,9 @@ def __neg__(self) -> "Quaternion": def __rsub__(self, other: "Quaternion") -> "Quaternion": return Quaternion(xyzw=self.xyzw - other.xyzw) + def __truediv__(self, other: Scalar) -> "Quaternion": + return Quaternion(xyzw=self.xyzw / other) + def conjugate(self) -> "Quaternion": return Quaternion(xyzw=cs.vertcat(-self.xyzw[:3], self.xyzw[3])) @@ -79,3 +83,40 @@ def z(self) -> float: @property def w(self) -> float: return self.xyzw[3] + + def inverse(self): + return self.conjugate() / cs.dot(self.xyzw, self.xyzw) + + @staticmethod + def slerp(q1: "Quaternion", q2: "Quaternion", n: Scalar) -> List[Vector]: + """Spherical linear interpolation between two quaternions + check https://en.wikipedia.org/wiki/Slerp for more details + + Args: + q1 (Quaternion): First quaternion + q2 (Quaternion): Second quaternion + n (Scalar): Number of interpolation steps + + Returns: + Quaternion: Interpolated quaternion + """ + q1 = q1.coeffs() + q2 = q2.coeffs() + return [Quaternion.slerp_step(q1, q2, t) for t in cs.np.linspace(0, 1, n + 1)] + + @staticmethod + def slerp_step(q1: Vector, q2: Vector, t: Scalar) -> Vector: + """Step for the splerp function + + Args: + q1 (Vector): First quaternion + q2 (Vector): Second quaternion + t (Scalar): Interpolation parameter + + Returns: + Vector: Interpolated quaternion + """ + + dot = cs.dot(q1, q2) + angle = cs.acos(dot) + return (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle) From 6ce2760cb04b2c156fd76990962d074c3b3eedc9 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 3 Mar 2023 10:55:46 +0100 Subject: [PATCH 2/6] Clean slerp creation --- src/liecasadi/quaternion.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py index cd4f2bd..1c81f92 100644 --- a/src/liecasadi/quaternion.py +++ b/src/liecasadi/quaternion.py @@ -88,7 +88,7 @@ def inverse(self): return self.conjugate() / cs.dot(self.xyzw, self.xyzw) @staticmethod - def slerp(q1: "Quaternion", q2: "Quaternion", n: Scalar) -> List[Vector]: + def slerp(q1: "Quaternion", q2: "Quaternion", n: Scalar) -> List["Quaternion"]: """Spherical linear interpolation between two quaternions check https://en.wikipedia.org/wiki/Slerp for more details @@ -98,11 +98,11 @@ def slerp(q1: "Quaternion", q2: "Quaternion", n: Scalar) -> List[Vector]: n (Scalar): Number of interpolation steps Returns: - Quaternion: Interpolated quaternion + List[Quaternion]: Interpolated quaternion """ q1 = q1.coeffs() q2 = q2.coeffs() - return [Quaternion.slerp_step(q1, q2, t) for t in cs.np.linspace(0, 1, n + 1)] + return [Quaternion.slerp_step(q1, q2, t) for t in cs.np.linspace(0, 1, n)] @staticmethod def slerp_step(q1: Vector, q2: Vector, t: Scalar) -> Vector: @@ -119,4 +119,6 @@ def slerp_step(q1: Vector, q2: Vector, t: Scalar) -> Vector: dot = cs.dot(q1, q2) angle = cs.acos(dot) - return (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle) + return Quaternion( + (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle) + ) From b7650dce65c0cbfb561bc425ed65cab858bf54f0 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 3 Mar 2023 10:56:02 +0100 Subject: [PATCH 3/6] Slerp on SO3 --- src/liecasadi/so3.py | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/liecasadi/so3.py b/src/liecasadi/so3.py index ff3f457..e46a1ef 100644 --- a/src/liecasadi/so3.py +++ b/src/liecasadi/so3.py @@ -4,7 +4,7 @@ import dataclasses from dataclasses import field -from typing import Union +from typing import Union, List import casadi as cs import numpy as np @@ -116,7 +116,6 @@ def quaternion_derivative( omega_in_body_fixed: bool = False, baumgarte_coefficient: Union[float, None] = None, ): - if baumgarte_coefficient is not None: baumgarte_term = ( baumgarte_coefficient @@ -139,10 +138,22 @@ def quaternion_derivative( ).coeffs() @staticmethod - def product(q1: Vector, q2: Vector) -> Vector: - p1 = q1[3] * q2[3] - cs.dot(q1[:3], q2[:3]) - p2 = q1[3] * q2[:3] + q2[3] * q1[:3] + cs.cross(q1[:3], q2[:3]) - return cs.vertcat(p2, p1) + def slerp(r1: "SO3", r2: "SO3", n: int) -> List["SO3"]: + """ + Spherical linear interpolation between two rotations. + + Args: + r1 (SO3): First quaternion + r2 (SO3): Second quaternion + n (Scalar): Number of interpolation steps + + Returns: + List[SO3]: Interpolated rotations + """ + q1 = r1.as_quat() + q2 = r2.as_quat() + interpolated_quats = Quaternion.slerp(q1, q2, n) + return [SO3(xyzw=q.coeffs()) for q in interpolated_quats] @dataclasses.dataclass From ce5949fb04d97427f6206d0f49d51d9e59c6511e Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 3 Mar 2023 10:56:59 +0100 Subject: [PATCH 4/6] This method should allow to pass SO3.as_quat() to casadi methods without calling .coeffs() --- src/liecasadi/quaternion.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py index 1c81f92..ce5c352 100644 --- a/src/liecasadi/quaternion.py +++ b/src/liecasadi/quaternion.py @@ -14,6 +14,9 @@ class Quaternion: xyzw: Vector + def __getattr__(self, attr): + return getattr(self.xyzw, attr) + def __repr__(self) -> str: return f"Quaternion: {self.xyzw}" From d7aab6f19eec76e795963ba831cdab50ba6b8879 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 3 Mar 2023 10:57:28 +0100 Subject: [PATCH 5/6] Modify optimization example accordingly with the previous commit --- examples/manifold_optimization.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/examples/manifold_optimization.py b/examples/manifold_optimization.py index 4bc167f..92dd7c2 100644 --- a/examples/manifold_optimization.py +++ b/examples/manifold_optimization.py @@ -5,6 +5,8 @@ # conda install matplotlib scipy import casadi as cs +import matplotlib + import matplotlib.pyplot as plt import numpy as np from matplotlib import animation @@ -25,13 +27,13 @@ for k in range(N): vector_SO3 = SO3Tangent(vel[k] * dt) rotation_SO3 = SO3(quat[k]) - opti.subject_to(quat[k + 1] == (vector_SO3 + rotation_SO3).as_quat().coeffs()) + opti.subject_to(quat[k + 1] == (vector_SO3 + rotation_SO3).as_quat()) C = sum(cs.sumsqr(vel[i]) for i in range(N)) + T # Initial rotation and velocity -opti.subject_to(quat[0] == SO3.Identity().as_quat().coeffs()) +opti.subject_to(quat[0] == SO3.Identity().as_quat()) opti.subject_to(vel[0] == 0) opti.subject_to(opti.bounded(0, T, 10)) @@ -47,7 +49,7 @@ opti.subject_to(vel[N - 1] == 0) final_delta_increment = SO3Tangent([cs.pi / 3, cs.pi / 6, cs.pi / 2]) -opti.subject_to(quat[N] == (final_delta_increment + SO3.Identity()).as_quat().coeffs()) +opti.subject_to(quat[N] == (final_delta_increment + SO3.Identity()).as_quat()) opti.minimize(C) @@ -71,7 +73,8 @@ plt.plot(np.linspace(0, time, N), v) figure = plt.figure() -axes = mplot3d.Axes3D(figure) +axes = figure.add_subplot(projection="3d") + x_cords = np.array([1, 0, 0]) y_cords = np.array([0, 1, 0]) z_cords = np.array([0, 0, 1]) From c9740757d6c66cccb09940850a34bb45a3e92e32 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 3 Mar 2023 10:57:50 +0100 Subject: [PATCH 6/6] Add example of slerp on SO3 --- examples/slerp_so3.py | 90 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 examples/slerp_so3.py diff --git a/examples/slerp_so3.py b/examples/slerp_so3.py new file mode 100644 index 0000000..6068b92 --- /dev/null +++ b/examples/slerp_so3.py @@ -0,0 +1,90 @@ +# Please note that for running this example you need to install `matplotlib` and `scipy`. +# You can do this by running the following command in your terminal: +# pip install matplotlib scipy +# If you are using anaconda, you can also run the following command: +# conda install matplotlib scipy + +import casadi as cs +import matplotlib.pyplot as plt +import numpy as np +from matplotlib import animation + +from liecasadi import SO3, SO3Tangent + +N = 10 + +r1 = SO3.Identity() + +final_delta_increment = SO3Tangent([cs.pi / 3, cs.pi / 6, cs.pi / 2]) + +r2 = final_delta_increment + SO3.Identity() + +x = SO3.slerp(r1, r2, N) + +# If you want to work directly with quaternion, you can use the following code: +# x = Quaternion.slerp(q1, q1, N) +# where q1 and q2 are Quaternion objects. + +figure = plt.figure() +axes = figure.add_subplot(projection="3d") +x_cords = np.array([1, 0, 0]) +y_cords = np.array([0, 1, 0]) +z_cords = np.array([0, 0, 1]) + +axes.set_box_aspect((1, 1, 1)) + +(xax,) = axes.plot([0, 1], [0, 0], [0, 0], "red") +(yax,) = axes.plot([0, 0], [0, 1], [0, 0], "green") +(zax,) = axes.plot([0, 0], [0, 0], [0, 1], "blue") + +print("qui", x[N - 1].act(x_cords)) + +# final orientation +x_N = np.array(x[N - 1].act(x_cords)).reshape( + 3, +) +y_N = np.array(x[N - 1].act(y_cords)).reshape( + 3, +) +z_N = np.array(x[N - 1].act(z_cords)).reshape( + 3, +) + +(xaxN,) = axes.plot([0, x_N[0]], [0, x_N[1]], [0, x_N[2]], "red") +(yaxN,) = axes.plot([0, y_N[0]], [0, y_N[1]], [0, y_N[2]], "green") +(zaxN,) = axes.plot([0, z_N[0]], [0, z_N[1]], [0, z_N[2]], "blue") + + +def update_points(i): + x_i = np.array(x[i].act(x_cords)).reshape( + 3, + ) + y_i = np.array(x[i].act(y_cords)).reshape( + 3, + ) + z_i = np.array(x[i].act(z_cords)).reshape( + 3, + ) + # update properties + xax.set_data(np.array([[0, x_i[0]], [0, x_i[1]]])) + xax.set_3d_properties(np.array([0, x_i[2]]), "z") + + yax.set_data(np.array([[0, y_i[0]], [0, y_i[1]]])) + yax.set_3d_properties(np.array([0, y_i[2]]), "z") + + zax.set_data(np.array([[0, z_i[0]], [0, z_i[1]]])) + zax.set_3d_properties(np.array([0, z_i[2]]), "z") + + # return modified axis + return ( + xax, + yax, + zax, + ) + + +ani = animation.FuncAnimation(figure, update_points, frames=N, repeat=False) +writergif = animation.PillowWriter(fps=5) +ani.save("animation.gif", writer=writergif) + +plt.show()