# SPART-2.0 / SPARTpy

![GitHub last commit](https://img.shields.io/github/last-commit/Matteodambr/SPART) ![GitHub forks](https://img.shields.io/github/forks/NPS-SRL/SPART?style=social) ![GitHub SPART stars](https://img.shields.io/github/stars/NPS-SRL/SPART?style=social) ![GitHub contributors](https://img.shields.io/github/contributors/Matteodambr/SPART) ![Open Issues](https://img.shields.io/github/issues-raw/NPS-SRL/SPART) ![Top Language](https://img.shields.io/github/languages/top/Matteodambr/SPART) ![Code Size](https://img.shields.io/github/languages/code-size/Matteodambr/SPART) ![GitHub issues](https://img.shields.io/github/issues/NPS-SRL/SPART) ![GitHub pull requests](https://img.shields.io/github/issues-pr/NPS-SRL/SPART)

**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.

This repository builds on the original SPART library ([link](https://github.com/NPS-SRL/SPART)) that is no longer maintained.  The main additions are:

1. A Python package (`SPARTpy`) that calls highly efficient MATLAB Coder-generated C functions directly from Python — faster than native MATLAB, even accounting for all Python overhead.
2. New space robot URDF models: a 3D system with a 7-DoF manipulator and a 2D system with a 4-DoF manipulator, both redundant and easily customizable.
3. ODE functions for integrating space robot dynamics, in both Simulink and MATLAB (Python ODE WIP).
4. Initial condition generation and postprocessing helpers for dynamics integration.
5. Homogenized function interfaces for consistent, user-friendly conventions.

---

## Installation

```bash
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)

---

## 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    = np.eye(3)                                          # base orientation (DCM)
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, r0, qm)

# Differential kinematics
Bij, Bi0, P0, pm = spart.diff_kinematics(R0, 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, 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]

_, y0, tau_ode = spart.space_robot_ode_input(
    0.0, R0, 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, r0, qm)` | Joint/link poses, joint axes and CoM positions |
| `diff_kinematics(R0, 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, 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).

---

<details>
<summary><b>Original SPART (MATLAB) README</b></summary>

[Read the Docs](https://spart.readthedocs.io/en/latest/) · [Original GitHub](https://github.com/NPS-SRL/SPART)

SPART is an open-source modeling and control toolkit for mobile-base robotic multibody systems with kinematic tree topologies (*i.e.*, open-loop multi-branched systems).
SPART is MATLAB-based and ROS-compatible, allowing to prototype in simulation and deploy to hardware controllers for robotic systems.

Given a URDF description of a multibody system, SPART computes the system's:

* Kinematics — pose of the links and joints (*i.e.*, rotation matrices and position vectors).
* Differential kinematics — operational space velocities and accelerations, as well as the geometric Jacobians and their time derivatives.
* Dynamics — generalized inertia and convective inertia matrices.
* Forward/Inverse dynamics — solves both problems, including the floating-base case.

SPART supports symbolic computation and analytic expressions for all kinematic and dynamic quantities.

### MATLAB Installation

Clone or download the toolkit and run the `SPART2path.m` script. This adds all SPART functions and the Simulink library to the MATLAB path and saves it.

To run an example go to `Examples/URDF_Tutorial` and run:

    URDF_Tutorial

### Documentation

Up to date documentation and tutorials: [spart.readthedocs.org](http://spart.readthedocs.org)

### Examples

Planar Desired-Reaction-Maneuver — kinematic redundancy exploited to keep the base pointing at the end-effector while it follows a prescribed path:

![DRM](docs/source/Figures/DRM.gif "Desired-Reaction-Maneuver")

Resolved motion-rate control of the R5D3 manipulator (end-effector tracing a triangle):

![R5D3](docs/source/Figures/R5D3.gif "R5D3 resolved motion-rate control")

</details>
