adam

Automatic Differentiation for rigid-body-dynamics AlgorithMs

adam implements a collection of algorithms for calculating rigid-body dynamics for floating-base robots, in mixed and body fixed representations (see Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots) using:

• Jax
• CasADi
• PyTorch
• NumPy

adam employs the automatic differentiation capabilities of these frameworks to compute, if needed, gradients, Jacobian, Hessians of rigid-body dynamics quantities. This approach enables the design of optimal control and reinforcement learning strategies in robotics.

adam is based on Roy Featherstone's Rigid Body Dynamics Algorithms.

Table of contents

• 🐍 Dependencies
• 💾 Installation
  • 🐍 Installation with pip
  • 📦 Installation with conda
  • Installation from conda-forge package
  • 🔨 Installation from repo
• 🚀 Usage
  • Jax interface
  • CasADi interface
  • PyTorch interface
  • PyTorch Batched interface
• 🦸‍♂️ Contributing
• Todo

🐍 Dependencies

• python3

Other requisites are:

• urdfdom-py Python package, that exposes the urdf_parser_py Python module
• jax
• casadi
• pytorch
• numpy
• jax2torch

They will be installed in the installation step!

💾 Installation

The installation can be done either using the Python provided by apt (on Debian-based distros) or via conda (on Linux and macOS).

🐍 Installation with pip

Install python3, if not installed (in Ubuntu 20.04):

bash sudo apt install python3.8

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

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

Inside the virtual environment, install the library from pip:

• Install Jax interface:

bash pip install adam-robotics[jax]

• Install CasADi interface:

bash pip install adam-robotics[casadi]

• Install PyTorch interface:

bash pip install adam-robotics[pytorch]

• Install ALL interfaces:

bash pip install adam-robotics[all]

If you want the last version:

bash pip install adam-robotics[selected-interface]@git+https://github.com/ami-iit/ADAM

or clone the repo and install:

bash git clone https://github.com/ami-iit/adam.git cd adam pip install .[selected-interface]

📦 Installation with conda

Installation from conda-forge package

• Install CasADi interface:

bash conda create -n adamenv -c conda-forge adam-robotics-casadi

• Install Jax interface (warning: not available on Windows):

bash conda create -n adamenv -c conda-forge adam-robotics-jax

• Install PyTorch interface (warning: not available on Windows):

bash conda create -n adamenv -c conda-forge adam-robotics-pytorch

• Install ALL interfaces (warning: not available on Windows):

bash conda create -n adamenv -c conda-forge adam-robotics-all

[!NOTE] Check also the conda JAX installation guide here

🔨 Installation from repo

Install in a conda environment the required dependencies:

• Jax interface dependencies:

bash conda create -n adamenv -c conda-forge jax numpy lxml prettytable matplotlib urdfdom-py

• CasADi interface dependencies:

bash conda create -n adamenv -c conda-forge casadi numpy lxml prettytable matplotlib urdfdom-py

• PyTorch interface dependencies:

bash conda create -n adamenv -c conda-forge pytorch numpy lxml prettytable matplotlib urdfdom-py jax2torch

• ALL interfaces dependencies:

bash conda create -n adamenv -c conda-forge jax casadi pytorch numpy lxml prettytable matplotlib urdfdom-py jax2torch

Activate the environment, clone the repo and install the library:

bash conda activate adamenv git clone https://github.com/ami-iit/ADAM.git cd adam pip install --no-deps .

🚀 Usage

The following are small snippets of the use of adam. More examples are arriving! Have also a look at the tests folder.

Jax interface

[!NOTE] Check also the Jax installation guide here

```python import adam from adam.jax import KinDynComputations import icub_models import numpy as np import jax.numpy as jnp from jax import jit, vmap

if you want to icub-models https://github.com/robotology/icub-models to retrieve the urdf

modelpath = icubmodels.getmodelfile("iCubGazeboV2_5")

The joint list

jointsnamelist = [ 'torsopitch', 'torsoroll', 'torsoyaw', 'lshoulderpitch', 'lshoulderroll', 'lshoulderyaw', 'lelbow', 'rshoulderpitch', 'rshoulderroll', 'rshoulderyaw', 'relbow', 'lhippitch', 'lhiproll', 'lhipyaw', 'lknee', 'lanklepitch', 'lankleroll', 'rhippitch', 'rhiproll', 'rhipyaw', 'rknee', 'ranklepitch', 'rankle_roll' ]

kinDyn = KinDynComputations(modelpath, jointsname_list)

choose the representation, if you want to use the body fixed representation

kinDyn.setframevelocityrepresentation(adam.Representations.BODYFIXED_REPRESENTATION)

or, if you want to use the mixed representation (that is the default)

kinDyn.setframevelocityrepresentation(adam.Representations.MIXEDREPRESENTATION) wHb = np.eye(4) joints = np.ones(len(jointsnamelist)) M = kinDyn.massmatrix(wHb, joints) print(M) wHf = kinDyn.forwardkinematics('framename', wH_b, joints)

IMPORTANT! The Jax Interface function execution can be slow! We suggest to jit them.

For example:

def frameforwardkinematics(wHb, joints): # This is needed since str is not a valid JAX type return kinDyn.forwardkinematics('framename', wHb, joints)

jittedframefk = jit(frameforwardkinematics) wHf = jittedframefk(wHb, joints)

In the same way, the functions can be also vmapped

vmappedframefk = vmap(frameforwardkinematics, in_axes=(0, 0))

which can be also jitted

jittedvmappedframefk = jit(vmappedframe_fk)

and called on a batch of data

jointsbatch = jnp.tile(joints, (1024, 1)) wHbbatch = jnp.tile(wHb, (1024, 1, 1)) wHfbatch = jittedvmappedframefk(wHbbatch, jointsbatch)

```

[!NOTE] The first call of the jitted function can be slow, since JAX needs to compile the function. Then it will be faster!

CasADi interface

```python import casadi as cs import adam from adam.casadi import KinDynComputations import icub_models import numpy as np

if you want to icub-models https://github.com/robotology/icub-models to retrieve the urdf

modelpath = icubmodels.getmodelfile("iCubGazeboV2_5")

The joint list

jointsnamelist = [ 'torsopitch', 'torsoroll', 'torsoyaw', 'lshoulderpitch', 'lshoulderroll', 'lshoulderyaw', 'lelbow', 'rshoulderpitch', 'rshoulderroll', 'rshoulderyaw', 'relbow', 'lhippitch', 'lhiproll', 'lhipyaw', 'lknee', 'lanklepitch', 'lankleroll', 'rhippitch', 'rhiproll', 'rhipyaw', 'rknee', 'ranklepitch', 'rankle_roll' ]

kinDyn = KinDynComputations(modelpath, jointsname_list)

choose the representation you want to use the body fixed representation

kinDyn.setframevelocityrepresentation(adam.Representations.BODYFIXED_REPRESENTATION)

or, if you want to use the mixed representation (that is the default)

kinDyn.setframevelocityrepresentation(adam.Representations.MIXEDREPRESENTATION) wHb = np.eye(4) joints = np.ones(len(jointsnamelist)) M = kinDyn.massmatrixfun() print(M(wHb, joints))

If you want to use the symbolic version

wHb = cs.SX.eye(4) joints = cs.SX.sym('joints', len(jointsnamelist)) M = kinDyn.massmatrixfun() print(M(wHb, joints))

This is usable also with casadi.MX

wHb = cs.MX.eye(4) joints = cs.MX.sym('joints', len(jointsnamelist)) M = kinDyn.massmatrixfun() print(M(wHb, joints))

```

PyTorch interface

```python import adam from adam.pytorch import KinDynComputations import icub_models import numpy as np

if you want to icub-models https://github.com/robotology/icub-models to retrieve the urdf

modelpath = icubmodels.getmodelfile("iCubGazeboV2_5")

The joint list

jointsnamelist = [ 'torsopitch', 'torsoroll', 'torsoyaw', 'lshoulderpitch', 'lshoulderroll', 'lshoulderyaw', 'lelbow', 'rshoulderpitch', 'rshoulderroll', 'rshoulderyaw', 'relbow', 'lhippitch', 'lhiproll', 'lhipyaw', 'lknee', 'lanklepitch', 'lankleroll', 'rhippitch', 'rhiproll', 'rhipyaw', 'rknee', 'ranklepitch', 'rankle_roll' ]

kinDyn = KinDynComputations(modelpath, jointsname_list)

choose the representation you want to use the body fixed representation

kinDyn.setframevelocityrepresentation(adam.Representations.BODYFIXED_REPRESENTATION)

or, if you want to use the mixed representation (that is the default)

kinDyn.setframevelocityrepresentation(adam.Representations.MIXEDREPRESENTATION) wHb = np.eye(4) joints = np.ones(len(jointsnamelist)) M = kinDyn.massmatrix(wH_b, joints) print(M) ```

PyTorch Batched interface

[!NOTE] When using this interface, note that the first call of the jitted function can be slow, since JAX needs to compile the function. Then it will be faster!

```python import adam from adam.pytorch import KinDynComputationsBatch import icub_models

if you want to icub-models

modelpath = icubmodels.getmodelfile("iCubGazeboV2_5")

The joint list

jointsnamelist = [ 'torsopitch', 'torsoroll', 'torsoyaw', 'lshoulderpitch', 'lshoulderroll', 'lshoulderyaw', 'lelbow', 'rshoulderpitch', 'rshoulderroll', 'rshoulderyaw', 'relbow', 'lhippitch', 'lhiproll', 'lhipyaw', 'lknee', 'lanklepitch', 'lankleroll', 'rhippitch', 'rhiproll', 'rhipyaw', 'rknee', 'ranklepitch', 'rankle_roll' ]

kinDyn = KinDynComputationsBatch(modelpath, jointsname_list)

choose the representation you want to use the body fixed representation

kinDyn.setframevelocityrepresentation(adam.Representations.BODYFIXED_REPRESENTATION)

or, if you want to use the mixed representation (that is the default)

kinDyn.setframevelocityrepresentation(adam.Representations.MIXEDREPRESENTATION) wHb = np.eye(4) joints = np.ones(len(jointsnamelist))

numsamples = 1024 wHbbatch = torch.tensor(np.tile(wHb, (numsamples, 1, 1)), dtype=torch.float32) jointsbatch = torch.tensor(np.tile(joints, (num_samples, 1)), dtype=torch.float32)

M = kinDyn.massmatrix(wHbbatch, jointsbatch) wHf = kinDyn.forwardkinematics('framename', wHbbatch, joints_batch) ```

Inverse Kinematics

adam provides an interface for solving inverse kinematics problems using CasADi. The solver supports

• position, orientation, and full pose constraints
• frame-to-frame constraints (ball, fixed)
• optional joint limit constraints

```python import casadi as cs import numpy as np import adam from adam.casadi import KinDynComputations from adam.casadi.inverse_kinematics import InverseKinematics, TargetType

Load your robot model

import icubmodels modelpath = icubmodels.getmodelfile("iCubGazeboV25")

The joint list

jointsnamelist = ...

Create IK solver

ik = InverseKinematics(model_path, joints)

Add a pose target on a frame (e.g., the left sole)

ik.addtarget("lsole", targettype=TargetType.POSE, assoftconstraint=True, weight=1.0) ik.addballconstraint(frame1, frame2, assoft_constraint=True)

Update the target to a desired pose

desiredposition = np.array([0.3, 0.2, 1.0]) desiredorientation = np.eye(3) ik.updatetarget("lsole", (desiredposition, desiredorientation))

Solve

ik.solve()

Retrieve solution

wHbsol, qsol = ik.getsolution() print("Base pose:\n", wHbsol) print("Joint values:\n", q_sol) ```

🦸‍♂️ Contributing

adam 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:

[!WARNING] REPOSITORY UNDER DEVELOPMENT! We cannot guarantee stable API

Todo

• [x] Center of Mass position
• [x] Jacobians
• [x] Forward kinematics
• [x] Mass Matrix via CRBA
• [x] Centroidal Momentum Matrix via CRBA
• [x] Recursive Newton-Euler algorithm (still no acceleration in the algorithm, since it is used only for the computation of the bias force)
• [ ] Articulated Body algorithm