liecasadi
liecasadi copied to clipboard
Rigid transform using Lie groups and Dual Quaternions, written in CasADi!
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! :rocket: