ami-iit / liecasadi Goto Github PK

View Code? Open in Web Editor NEW

47.0 16.0 2.0 51 KB

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

License: BSD 3-Clause "New" or "Revised" License

Python 100.00%

lie-groups lie-algebra casadi optimization robotics dual-quaternions python

liecasadi's Introduction

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! 🚀

liecasadi's People

Contributors

Stargazers

Watchers

Forkers

garyandtang wayne-xiu

liecasadi's Issues

DualQuaternion incorrectly initialising xyzw

Hi,

In general, the DualQuaternion class is quite awkward to use (e.g. it would be clean to have a from_SE3() etc. constructor), however when working with casadi arrays, the identity and transform_point functions appear not to work?

Example:

import numpy as ntp
import casadi as cs
from liecasadi import SE3, SO3, DualQuaternion

th_0 = cs.SX.sym('th_0', (1, 1))
th_1 = cs.SX.sym('th_1', (1, 1))
cs_pt = cs.SX.sym('pt', (3, 1))
cs_np_pt = cs_np.array([1, 0, 0])
ntp_pt = ntp.array([1, 0, 0])

R0 = SO3(cs.np.array([0, cs.sin(th_0/2), 0, cs.cos(th_0/2)]))
R1 = SO3(cs.np.array([0, 0, cs.sin(th_1/2), cs.cos(th_1/2)]))

T = DualQuaternion.from_matrix(SE3(
    pos=cs.np.zeros(3), xyzw=(R1 * R0).as_quat().coeffs()).as_matrix())

# Works as expected for numpy array
print(T.transform_point(ntp_pt))
# Works as expected for casadi.np array
print(T.transform_point(cs_np_pt))
# Crash with below error for casadi array
print(T.transform_point(cs_pt))

The error I get is:

NotImplementedError: Wrong number or type of arguments for overloaded function 'dot'.
Possible prototypes are:
dot(DM,DM)
dot(SX,SX)
dot(MX,MX)
You have: '(SX,[SX])'

I've identified the problem is at line 200 in DualQuaternion.py (by extension also in line 188):
p.Qd.xyzw = [xyz[0], xyz[1], xyz[2], 0]

Changing this to p.Qd.xyzw = cs.np.array([xyz[0], xyz[1], xyz[2], 0]) appears to solve the problem. I expect there are several other locations which may contain the same problem?

If you can confirm that I'm using the library as expected then this is a bug, with a pretty trivial solution, no? Then I don't mind opening a PR with a bug fix.

Quaternion array handling

Is there a way for the SO3 class to be applied to not just one quaternion but to a whole array of them and be able to use all the functions like multiplication column-wise?

Problems running the example

I tried installing the library as documented in the README and running the example.

By installing the example as documented in the README:

(castest) traversaro@IITICUBLAP257:~$ pip install "liecasadi @ git+https://github.com/ami-iit/lie-casadi.git"
Collecting liecasadi@ git+https://github.com/ami-iit/lie-casadi.git
  Cloning https://github.com/ami-iit/lie-casadi.git to /tmp/pip-install-jld36pa2/liecasadi_02974e872e9a41da95da2ba742a2bb6a
  Running command git clone --filter=blob:none --quiet https://github.com/ami-iit/lie-casadi.git /tmp/pip-install-jld36pa2/liecasadi_02974e872e9a41da95da2ba742a2bb6a
  Resolved https://github.com/ami-iit/lie-casadi.git to commit 4f538f354781243e600ce771c9236a43df83745d
  Installing build dependencies ... done
  Getting requirements to build wheel ... done
  Preparing metadata (pyproject.toml) ... done
Collecting numpy>=1.20
  Using cached numpy-1.22.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl (16.8 MB)
Collecting casadi
  Using cached casadi-3.5.5-cp38-none-manylinux1_x86_64.whl (34.2 MB)
Building wheels for collected packages: liecasadi
  Building wheel for liecasadi (pyproject.toml) ... done
  Created wheel for liecasadi: filename=liecasadi-0.1.dev39-py3-none-any.whl size=7882 sha256=078ab323d4479d39e5aa0a577a6150df54d9224d416b94c89d26adc5db0616be
  Stored in directory: /tmp/pip-ephem-wheel-cache-v0j9nv_a/wheels/7a/26/6a/62ad9ab4f348e178408f7d79d44555ed51514496949fcdb644
Successfully built liecasadi
Installing collected packages: casadi, numpy, liecasadi
Successfully installed casadi-3.5.5 liecasadi-0.1.dev39 numpy-1.22.1

You can see that casadi got installed via pip.

Then, I tried to run the example and the example fails with:

(castest) traversaro@IITICUBLAP257:~/liecasadi/examples$ python manifold_optimization.py
Traceback (most recent call last):
  File "manifold_optimization.py", line 4, in <module>
    import matplotlib.pyplot as plt
ModuleNotFoundError: No module named 'matplotlib'

This is due to matplotlib missing, and it make sense that this is not part of the dependencies of the library. If I then install matplot lib, then the example fails with:

(castest) traversaro@IITICUBLAP257:~/liecasadi/examples$ python manifold_optimization.py
Traceback (most recent call last):
  File "manifold_optimization.py", line 9, in <module>
    from liecasadi import SO3, SO3Tangent
  File "/home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages/liecasadi/__init__.py", line 3, in <module>
    from .so3 import SO3, SO3Tangent
  File "/home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages/liecasadi/so3.py", line 9, in <module>
    from attr import field
ModuleNotFoundError: No module named 'attr'

This is a bit more tricky, as it requires to install the attrs package (note the final s, as the attr package will not work).

After installing also attr, I get this error:

(castest) traversaro@IITICUBLAP257:~/liecasadi/examples$ python manifold_optimization.py
Traceback (most recent call last):
  File "manifold_optimization.py", line 21, in <module>
    opti.subject_to(quat[k + 1] == (vector_SO3 + rotation_SO3).as_quat())
  File "/home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages/casadi/casadi.py", line 28146, in subject_to
    ret = self._subject_to(*args)
  File "/home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages/casadi/casadi.py", line 27470, in _subject_to
    return _casadi.Opti__subject_to(self, *args)
RuntimeError: Error in Opti::subject_to [OptiNode] at .../casadi/core/optistack.cpp:96:
.../casadi/core/optistack_internal.cpp:905: Assertion "!g.is_constant()" failed:
You passed a constant to `subject_to`. You need a symbol to form a constraint.

Furthermore, even if I did not reached this point, I noticed that the example refers ma27 (

liecasadi/examples/manifold_optimization.py

Line 47 in c79d29e

"linear_solver": "ma27",

), but by following the instruction of the repo casadi gets installed by pip, and I do not think that that version of casadi has hsl support enabled.

Environments in which I run the tests:

(castest) traversaro@IITICUBLAP257:~/liecasadi/examples$ mamba list
# packages in environment at /home/traversaro/mambaforge/envs/castest:
#
# Name                    Version                   Build  Channel
_libgcc_mutex             0.1                 conda_forge    conda-forge
_openmp_mutex             4.5                       1_gnu    conda-forge
attrs                     21.4.0                   pypi_0    pypi
ca-certificates           2021.10.8            ha878542_0    conda-forge
casadi                    3.5.5                    pypi_0    pypi
cycler                    0.11.0                   pypi_0    pypi
fonttools                 4.29.1                   pypi_0    pypi
kiwisolver                1.3.2                    pypi_0    pypi
ld_impl_linux-64          2.36.1               hea4e1c9_2    conda-forge
libffi                    3.4.2                h7f98852_5    conda-forge
libgcc-ng                 11.2.0              h1d223b6_12    conda-forge
libgomp                   11.2.0              h1d223b6_12    conda-forge
libnsl                    2.0.0                h7f98852_0    conda-forge
libstdcxx-ng              11.2.0              he4da1e4_12    conda-forge
libzlib                   1.2.11            h36c2ea0_1013    conda-forge
liecasadi                 0.1.dev39                pypi_0    pypi
matplotlib                3.5.1                    pypi_0    pypi
ncurses                   6.3                  h9c3ff4c_0    conda-forge
numpy                     1.22.1                   pypi_0    pypi
openssl                   3.0.0                h7f98852_2    conda-forge
packaging                 21.3                     pypi_0    pypi
pillow                    9.0.0                    pypi_0    pypi
pip                       22.0.2             pyhd8ed1ab_0    conda-forge
pyparsing                 3.0.7                    pypi_0    pypi
python                    3.8.12          h0744224_3_cpython    conda-forge
python-dateutil           2.8.2                    pypi_0    pypi
python_abi                3.8                      2_cp38    conda-forge
readline                  8.1                  h46c0cb4_0    conda-forge
setuptools                60.6.0           py38h578d9bd_0    conda-forge
six                       1.16.0                   pypi_0    pypi
sqlite                    3.37.0               h9cd32fc_0    conda-forge
tk                        8.6.11               h27826a3_1    conda-forge
wheel                     0.37.1             pyhd8ed1ab_0    conda-forge
xz                        5.2.5                h516909a_1    conda-forge
zlib                      1.2.11            h36c2ea0_1013    conda-forge
(castest) traversaro@IITICUBLAP257:~/liecasadi/examples$ pip list --verbose
Package         Version   Location                                                             Installer
--------------- --------- -------------------------------------------------------------------- ---------
attrs           21.4.0    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
casadi          3.5.5     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
cycler          0.11.0    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
fonttools       4.29.1    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
kiwisolver      1.3.2     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
liecasadi       0.1.dev39 /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
matplotlib      3.5.1     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
numpy           1.22.1    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
packaging       21.3      /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
Pillow          9.0.0     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
pip             22.0.2    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages
pyparsing       3.0.7     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
python-dateutil 2.8.2     /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
setuptools      60.6.0    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages
six             1.16.0    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages pip
wheel           0.37.1    /home/traversaro/mambaforge/envs/castest/lib/python3.8/site-packages

Fix manifpy test dependency

In order to check the correctness of the computations, I wrote some tests against Manif.

It seems manifpy cannot be installed via PyPI. However, they provide installation instruction in https://github.com/artivis/manif/blob/devel/docs/pages/python/Quick-start.md.

Although I can install the library using conda on my system, I'm not able to move the same logic in a workflow.
manifpy seems to be installed but it is not found:

ModuleNotFoundError: No module named 'manifpy'

Am I doing something wrong?

@traversaro @GiulioRomualdi do you have any suggestions?

slerp_step returns NaN if the the angle between the two quaternions is zero

When dividing by cs.sin(angle) in

liecasadi/src/liecasadi/quaternion.py

Line 126 in ea55d1f

(cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle)

, there is a division by zero when the angle is zero.

One possibility could be to check if they are aligned before interpolating. Maybe using Casadi if_else https://github.com/casadi/casadi/wiki/L_113?

cc @Giulero

Recommend Projects

React

A declarative, efficient, and flexible JavaScript library for building user interfaces.
Vue.js

🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
Typescript

TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
TensorFlow

An Open Source Machine Learning Framework for Everyone
Django

The Web framework for perfectionists with deadlines.
Laravel

A PHP framework for web artisans
D3

Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

javascript

JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
web

Some thing interesting about web. New door for the world.
server

A server is a program made to process requests and deliver data to clients.
Machine learning

Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Visualization

Some thing interesting about visualization, use data art
Game

Some thing interesting about game, make everyone happy.

Recommend Org

Facebook

We are working to build community through open source technology. NB: members must have two-factor auth.
Microsoft

Open source projects and samples from Microsoft.
Google

Google ❤️ Open Source for everyone.
Alibaba

Alibaba Open Source for everyone
D3

Data-Driven Documents codes.
Tencent

China tencent open source team.