Skip to content

Rigid transform using Lie groups and Dual Quaternions, written in CasADi!

License

Notifications You must be signed in to change notification settings

ami-iit/liecasadi

Repository files navigation

liecasadi

liecasadi

liecasadi implements Lie groups operation written in CasADi, mainly directed to optimization problem formulation.

Inspired by A micro Lie theory for state estimation in robotics and the library Manif.

🐍 Install

Create a virtual environment, if you prefer. For example:

pip install virtualenv
python3 -m venv your_virtual_env
source your_virtual_env/bin/activate

Inside the virtual environment, install the library from pip:

pip install liecasadi

If you want the last version:

pip install "liecasadi @ git+https://github.com/ami-iit/lie-casadi.git"

Implemented Groups

Group Description
SO3 3D Rotations
SE3 3D Rigid Transform

🚀 Operations

Being:

  • $X, Y \in SO3, \ SE3$

  • $w \in \text{SO3Tangent}, \ \text{SE3Tangent}$

  • $v \in \mathbb{R}^3$

Operation Code
Inverse $X^{-1}$ X.inverse()
Composition $X \circ Y$ X*Y
Exponential $\text{exp}(w)$ phi.exp()
Act on vector $X \circ v$ X.act(v)
Logarithm $\text{log}(X)$ X.log()
Manifold right plus $X \oplus w = X \circ \text{exp}(w)$ X + phi
Manifold left plus $w \oplus X = \text{exp}(w) \circ X$ phi + X
Manifold minus $X-Y = \text{log}(Y^{-1} \circ X)$ X-Y

🦸‍♂️ Example

from liecasadi import SE3, SO3, SE3Tangent, SO3Tangent

# Random quaternion + normalization
quat = (np.random.rand(4) - 0.5) * 5
quat = quat / np.linalg.norm(quat)
# Random vector
vector3d = (np.random.rand(3) - 0.5) * 2 * np.pi

# Create SO3 object
rotation = SO3(quat)

# Create Identity
identity = SO3.Identity()

# Create SO3Tangent object
tangent = SO3Tangent(vector3d)

# Random translation vector
pos = (np.random.rand(3) - 0.5) * 5

# Create SE3 object
transform = SE3(pos=pos, xyzw=quat)

# Random vector
vector6d = (np.random.rand(3) - 0.5) * 5

# Create SE3Tangent object
tangent = SO3Tangent(vector6d)

Dual Quaternion example

from liecasadi import SE3, DualQuaternion
from numpy import np

# orientation quaternion generation
quat1 = (np.random.rand(4) - 0.5) * 5
quat1 = quat1 / np.linalg.norm(quat1)
quat2 = (np.random.rand(4) - 0.5) * 5
quat2 = quat2 / np.linalg.norm(quat2)

# translation vector generation
pos1 = (np.random.rand(3) - 0.5) * 5
pos2 = (np.random.rand(3) - 0.5) * 5

dual_quaternion1 = DualQuaternion(quat1, pos1)
dual_quaternion2 = DualQuaternion(quat2, pos2)

# from a homogenous matrix
# (using liecasadi.SE3 to generate the corresponding homogenous matrix)
H = SE3.from_position_quaternion(pos, quat).as_matrix()
dual_quaternion1 = DualQuaternion.from_matrix(H)

# Concatenation of rigid transforms
q1xq2 = dual_quaternion1 * dual_quaternion2

# to homogeneous matrix
print(q1xq2.as_matrix())

# obtain translation
print(q1xq2.translation())

# obtain rotation
print(q1xq2.rotation().as_matrix())

# transform a point
point = np.random.randn(3,1)
transformed_point = dual_quaternion1.transform_point(point)

# create an identity dual quaternion
I = DualQuaternion.Identity()

🦸‍♂️ Contributing

liecasadi is an open-source project. Contributions are very welcome!

Open an issue with your feature request or if you spot a bug. Then, you can also proceed with a Pull-requests! 🚀