Skip to content

Commit

Permalink
Merge pull request #4 from ami-iit/add_quaternion_derivative
Browse files Browse the repository at this point in the history
Add quaternion derivative
  • Loading branch information
Giulero authored Jul 29, 2022
2 parents 92a4b94 + 777f0e3 commit 9f42e98
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 29 deletions.
6 changes: 6 additions & 0 deletions examples/manifold_optimization.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,9 @@
# 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
Expand Down
4 changes: 3 additions & 1 deletion src/liecasadi/se3.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ class SE3:
def __repr__(self) -> str:
return f"Position: \t {self.pos} \nQuaternion: \t {self.xyzw}"

@staticmethod
def from_matrix(H: Matrix) -> "SE3":
assert H.shape == (4, 4)
return SE3(pos=H[:3, 3], xyzw=SO3.from_matrix(H[:3, :3]).as_quat().coeffs())
Expand All @@ -37,11 +38,13 @@ def transform(self) -> Matrix:
cs.horzcat([0, 0, 0, 1]).T,
)

@staticmethod
def from_position_quaternion(xyz: Vector, xyzw: Vector) -> "SE3":
assert xyz.shape in [(3,), (3, 1)]
assert xyzw.shape in [(4,), (4, 1)]
return SE3(pos=xyz, xyzw=xyzw)

@staticmethod
def from_translation_and_rotation(translation: Vector, rotation: SO3) -> "SE3":
return SE3(pos=translation, xyzw=rotation.as_quat().coeffs())

Expand Down Expand Up @@ -107,7 +110,6 @@ def exp(self):
+ (theta_eps - cs.sin(theta_eps)) / theta_eps @ cs.skew(u) @ cs.skew(u)
)
trans = V @ vec[:3]
# trans = SO3.from_matrix(V).act(vec[:3])
return SE3(pos=trans, xyzw=rot.as_quat().coeffs())

def vector(self):
Expand Down
54 changes: 26 additions & 28 deletions src/liecasadi/so3.py
Original file line number Diff line number Diff line change
Expand Up @@ -109,35 +109,33 @@ def __sub__(self, other) -> "SO3Tangent":
else:
raise RuntimeError("[SO3: __add__] Hey! Someone is not a Lie element.")

@staticmethod
def _quat_dot_map(d_omega: Vector, representation: str):
# if d_omega in spatial representation
A = 0.5 * cs.vertcat(
cs.horzcat(0, -d_omega[0], -d_omega[1], -d_omega[2]),
cs.horzcat(d_omega[0], 0, d_omega[2], -d_omega[1]),
cs.horzcat(d_omega[1], -d_omega[2], 0, d_omega[0]),
cs.horzcat(d_omega[2], d_omega[1], -d_omega[0], 0),
)
if representation == "spatial":
A[1:, 1:] = A[1:, 1:].T
return A

def add_integrated_velocity(
self, d_omega: Vector, representation: str = ["spatial", "body"]
def quaternion_derivative(
self,
omega: Vector,
omega_in_body_fixed: bool = False,
baumgarte_coefficient: float = None,
):
repr_types = ["spatial", "body"]
if representation not in repr_types:
raise ValueError(f"Invalid representation. Expected are {repr_types}")
quat = self.as_quat().coeffs()
quat = cs.vertcat(quat[3], quat[0], quat[1], quat[2])

def _exp(o):
A = SO3._quat_dot_map(o, representation)
nor = cs.norm_2(o)
return cs.cos(nor / 2) * cs.DM.eye(4) + 2 * cs.sin(nor / 2) / nor * A

quat = _exp(d_omega) * quat
return SO3(xyzw=cs.vertcat(quat[1], quat[2], quat[3], quat[0]))
if baumgarte_coefficient is not None:
baumgarte_term = (
baumgarte_coefficient
* cs.norm_2(omega)
* (1 - cs.norm_2(self.as_quat().coeffs()))
)
_omega = Quaternion(
cs.vertcat(
omega,
baumgarte_term,
)
)
else:
_omega = Quaternion(cs.vertcat(omega, 0))
# using the quaternion product formula
return (
0.5 * self.as_quat() * _omega
if omega_in_body_fixed
else 0.5 * _omega * self.as_quat()
).coeffs()

@staticmethod
def product(q1: Quaternion, q2: Quaternion) -> Quaternion:
Expand All @@ -151,7 +149,7 @@ class SO3Tangent:
vec: TangentVector

def __repr__(self) -> str:
return "SO3Tangent vector:" + str(self.vec)
return f"SO3Tangent vector:{str(self.vec)}"

def exp(self) -> SO3:
theta = cs.norm_2(self.vec + cs.np.finfo(np.float64).eps)
Expand Down

0 comments on commit 9f42e98

Please sign in to comment.