# SPARTpy

**SPARTpy** is the Python interface to [SPART](https://github.com/Matteodambr/SPART), an open-source toolkit for modeling and controlling floating-base space robots.  It wraps MATLAB Coder-generated C functions via `ctypes`, delivering kinematics, differential kinematics, velocities, accelerations, and forward/inverse dynamics at speeds faster than native MATLAB execution, for use within Robotics/AI projects within Python.

## Installation

```
pip install spartpy
```

The package ships the MATLAB Coder-generated C sources.  On the first import on a new machine, `gcc` is invoked automatically to compile them into a shared library (`SPART_C.so`).  Requires `gcc` with OpenMP support:

```bash
sudo apt install gcc   # Debian / Ubuntu
```

## Dependencies

- Python ≥ 3.8
- NumPy
- SciPy *(optional — needed for trajectory integration)*
- yourdfpy / matplotlib *(optional — needed for visualisation)*

## Building / Rebuilding the C Back-end

SPARTpy calls MATLAB Coder-generated C code through `ctypes`.  The compiled shared library (`SPART_C.so`) is rebuilt **automatically on first import** whenever it is missing or out of date.  Two system libraries must be present for this to work:

### 1. `gcc` with OpenMP

Used to compile `SPART_C.so` from the bundled `.c` sources:

```bash
# Debian / Ubuntu
sudo apt install gcc

# Conda (if preferred)
conda install -c conda-forge gcc
```

### 2. Intel OpenMP runtime (`libiomp5.so`)

The `.so` was generated by MATLAB Coder, which links against Intel OpenMP.  The library is searched automatically in the following order:

1. Any MATLAB installation under `/usr/local/MATLAB/R*/sys/os/glnxa64/`
2. System paths: `/usr/lib/x86_64-linux-gnu/libiomp5.so`, `/usr/lib/libiomp5.so`, `/usr/local/lib/libiomp5.so`
3. `$MATLAB_ROOT/lib/libiomp5.so`

If none of those exist, install it via conda or the Intel package:

```bash
conda install -c intel openmp
# or
sudo apt install intel-mkl          # ships libiomp5
```

Alternatively, add the directory containing `libiomp5.so` to `LD_LIBRARY_PATH`:

```bash
export LD_LIBRARY_PATH=/path/to/dir/containing/libiomp5:$LD_LIBRARY_PATH
```

### 3. `tmwtypes.h` (build-time only)

The C sources include `tmwtypes.h`, a MATLAB Coder header.  A copy is shipped alongside the `.c` files in the package, so no MATLAB installation is required at build time.  If for any reason it is missing, the build logic searches for a MATLAB installation and copies it automatically.  You can also force it by setting:

```bash
export MATLAB_ROOT=/usr/local/MATLAB/R2024b
```

### When is a rebuild triggered?

The build is triggered automatically if:
- `SPART_C.so` does not exist (e.g. first install on the machine)
- `SPART_C.so` is older than any `.c` source file (sources were updated)
- Loading the existing `.so` raises an `OSError` (e.g. wrong architecture)

## Rotation Convention

SPARTpy (and SPART) use **two rotation matrices** with clearly distinct roles.  Understanding which one is expected by each function is important.

### `R0_body2I` — body-to-inertial (active, B → I)

> Maps a vector expressed in the **body frame** into the **inertial frame**:
>
> $$V_I = R_{body2I} \, V_B$$

This is the matrix accepted by all kinematics and dynamics functions:

```
kinematics(R0_body2I, ...)
diff_kinematics(R0_body2I, ...)
i_i(R0_body2I, ...)
```

It is the standard DCM used in SPART: the columns of `R0_body2I` are the body-frame basis vectors expressed in the inertial frame.

### `R0_I2body` — inertial-to-body (active, I → B)

> Maps a vector expressed in the **inertial frame** into the **body frame**:
>
> $$V_B = R_{I2body} \, V_I$$

This is the matrix stored inside the **ODE state vector** `y`, and the one accepted by `space_robot_ode_input`:

```
space_robot_ode_input(t, R0_I2body, ...)
```

`R0_I2body` is simply the transpose of `R0_body2I`:

```python
R0_I2body = R0_body2I.T
```

The ODE integrates the DCM kinematic equation

$$\dot{R}_{I2body} = -[\omega_B]_\times \, R_{I2body}$$

which keeps the state matrix as the inertial-to-body form.  Internally, `space_robot_ode` transposes it before calling kinematics:

```python
R0_body2I = R0_I2body.T   # done internally — callers supply R0_I2body
```

### Summary table

| Variable | Direction | Semantics | Used in |
|---|---|---|---|
| `R0_body2I` | B → I | $V_I = R \, V_B$ | `kinematics`, `diff_kinematics`, `i_i` |
| `R0_I2body` | I → B | $V_B = R \, V_I$ | ODE state `y`, `space_robot_ode_input` |

## Quick Start

```python
import os
import numpy as np
from SPARTpy import SPART

# Load a URDF model
urdf = os.path.join('URDF_models', 'floating_7dof_manipulator.urdf')
spart = SPART(urdf)

# Robot state
# R0_body2I is the active rotation from the base-link body CCS to the inertial CCS:
#   V_I = R0_body2I @ V_B  (maps body-frame vectors to the inertial frame)
# For ODE integration, R0_I2body = R0_body2I.T is stored in the state vector.
R0_body2I = np.eye(3)                                          # base orientation (body -> inertial)
r0    = np.array([0., 0., 0.])                            # base position [m]
qm    = np.deg2rad([30., 20., 30., 20., 30., 20., 30.])  # joint angles [rad]
u0    = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])        # base twist [m/s, rad/s]
um    = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])   # joint rates [rad/s]
u0dot = np.zeros(6)   # base acceleration
umdot = np.zeros(7)   # joint acceleration
wF0   = np.zeros(6)   # external wrench on base
wFm   = np.zeros((6, spart.robot.n_links_joints))  # external wrenches on links

# Kinematics
RJ, RL, rJ, rL, e, g = spart.kinematics(R0_body2I, r0, qm)

# Differential kinematics
Bij, Bi0, P0, pm = spart.diff_kinematics(R0_body2I, r0, rL, e, g)

# Velocities
t0, tL = spart.velocities(Bij, Bi0, P0, pm, u0, um)

# Inertias in inertial frame
I0, Im = spart.i_i(R0_body2I, RL)

# Accelerations
t0dot, tLdot = spart.accelerations(t0, tL, P0, pm, Bi0, Bij, u0, um, u0dot, umdot)

# Inverse dynamics
tau0, taum = spart.inverse_dynamics(wF0, wFm, t0, tL, t0dot, tLdot, P0, pm, I0, Im, Bij, Bi0)

# Forward dynamics
u0dot_out, umdot_out = spart.forward_dynamics(
    tau0, taum, wF0, wFm, t0, tL, P0, pm, I0, Im, Bij, Bi0, u0, um)

# Trajectory integration (requires scipy)
from scipy.integrate import solve_ivp

tau0_traj = np.zeros((6, 1))                         # no base torque
taum_traj = np.array([2., 1., 0.5, 0., 0., 0., 0.]) # joint torques [Nm]

# space_robot_ode_input expects R0_I2body = R0_body2I.T
_, y0, tau_ode = spart.space_robot_ode_input(
    0.0, R0_body2I.T, r0, np.zeros(6), qm, np.zeros(7), tau0_traj, taum_traj)

sol = solve_ivp(
    fun=lambda t, y: spart.space_robot_ode(t, y, tau_ode),
    t_span=(0.0, 10.0), y0=y0, method='RK45', max_step=0.05)

# Animate result (requires yourdfpy or matplotlib)
spart.animate_trajectory(sol.t, sol.y.T, fps=30, backend='matplotlib')
```

## Available Functions

| Method | Description |
|--------|-------------|
| `kinematics(R0_body2I, r0, qm)` | Joint/link poses, joint axes and CoM positions |
| `diff_kinematics(R0_body2I, r0, rL, e, g)` | Jacobian-related matrices `Bij`, `Bi0`, `P0`, `pm` |
| `velocities(Bij, Bi0, P0, pm, u0, um)` | Spatial velocities of base and links |
| `i_i(R0_body2I, RL)` | Inertia tensors expressed in the inertial frame |
| `accelerations(t0, tL, P0, pm, Bi0, Bij, u0, um, u0dot, umdot)` | Spatial accelerations |
| `inverse_dynamics(wF0, wFm, t0, tL, t0dot, tLdot, P0, pm, I0, Im, Bij, Bi0)` | Generalised forces from motion |
| `forward_dynamics(tau0, taum, wF0, wFm, t0, tL, P0, pm, I0, Im, Bij, Bi0, u0, um)` | Accelerations from forces |
| `space_robot_ode(t, y, tau)` | ODE right-hand side for `scipy.integrate.solve_ivp` |
| `animate_trajectory(t, Y, fps, backend)` | Visualise a trajectory (matplotlib or yourdfpy) |
| `benchmark(n_runs)` | Time all functions and print a summary table |

## License

BSD 3-Clause.  See [LICENSE.md](../LICENSE.md).