- Notifications
You must be signed in to change notification settings - Fork5
Rigid transform using Lie groups and Dual Quaternions, written in CasADi!
License
ami-iit/liecasadi
Folders and files
| Name | Name | Last commit message | Last commit date | |
|---|---|---|---|---|
Repository files navigation
liecasadi implements Lie groups operation written in CasADi, mainly directed to optimization problem formulation.
Inspired byA micro Lie theory for state estimation in robotics and the libraryManif.
Create aconda environment
conda create -n liecasadienvand install the library
conda install liecasadiCreate avirtual environment, if you prefer. For example:
pip install virtualenvpython3 -m venv your_virtual_envsource your_virtual_env/bin/activateInside 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"| Group | Description |
|---|---|
| SO3 | 3D Rotations |
| SE3 | 3D Rigid Transform |
Being:
$X, Y \in SO3, \ SE3$ $w \in \text{SO3Tangent}, \ \text{SE3Tangent}$ $v \in \mathbb{R}^3$
| Operation | Code | |
|---|---|---|
| Inverse | X.inverse() | |
| Composition | X*Y | |
| Exponential | phi.exp() | |
| Act on vector | X.act(v) | |
| Logarithm | X.log() | |
| Manifold right plus | X + phi | |
| Manifold left plus | phi + X | |
| Manifold minus | X-Y |
fromliecasadiimportSE3,SO3,SE3Tangent,SO3Tangent# Random quaternion + normalizationquat= (np.random.rand(4)-0.5)*5quat=quat/np.linalg.norm(quat)# Random vectorvector3d= (np.random.rand(3)-0.5)*2*np.pi# Create SO3 objectrotation=SO3(quat)# Create Identityidentity=SO3.Identity()# Create SO3Tangent objecttangent=SO3Tangent(vector3d)# Random translation vectorpos= (np.random.rand(3)-0.5)*5# Create SE3 objecttransform=SE3(pos=pos,xyzw=quat)# Random vectorvector6d= (np.random.rand(3)-0.5)*5# Create SE3Tangent objecttangent=SO3Tangent(vector6d)
fromliecasadiimportSE3,DualQuaternionfromnumpyimportnp# orientation quaternion generationquat1= (np.random.rand(4)-0.5)*5quat1=quat1/np.linalg.norm(quat1)quat2= (np.random.rand(4)-0.5)*5quat2=quat2/np.linalg.norm(quat2)# translation vector generationpos1= (np.random.rand(3)-0.5)*5pos2= (np.random.rand(3)-0.5)*5dual_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 transformsq1xq2=dual_quaternion1*dual_quaternion2# to homogeneous matrixprint(q1xq2.as_matrix())# obtain translationprint(q1xq2.translation())# obtain rotationprint(q1xq2.rotation().as_matrix())# transform a pointpoint=np.random.randn(3,1)transformed_point=dual_quaternion1.transform_point(point)# create an identity dual quaternionI=DualQuaternion.Identity()
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! 🚀
About
Rigid transform using Lie groups and Dual Quaternions, written in CasADi!