Metadata-Version: 2.4
Name: spartpy
Version: 1.1.3
Summary: Python port of SPART: dynamics and control toolkit for free-flyer space robots
License: ### GNU LESSER GENERAL PUBLIC LICENSE
        
        Version 3, 29 June 2007
        
        Copyright (C) 2007 Free Software Foundation, Inc.
        <https://fsf.org/>
        
        Everyone is permitted to copy and distribute verbatim copies of this
        license document, but changing it is not allowed.
        
        This version of the GNU Lesser General Public License incorporates the
        terms and conditions of version 3 of the GNU General Public License,
        supplemented by the additional permissions listed below.
        
        #### 0. Additional Definitions.
        
        As used herein, "this License" refers to version 3 of the GNU Lesser
        General Public License, and the "GNU GPL" refers to version 3 of the
        GNU General Public License.
        
        "The Library" refers to a covered work governed by this License, other
        than an Application or a Combined Work as defined below.
        
        An "Application" is any work that makes use of an interface provided
        by the Library, but which is not otherwise based on the Library.
        Defining a subclass of a class defined by the Library is deemed a mode
        of using an interface provided by the Library.
        
        A "Combined Work" is a work produced by combining or linking an
        Application with the Library. The particular version of the Library
        with which the Combined Work was made is also called the "Linked
        Version".
        
        The "Minimal Corresponding Source" for a Combined Work means the
        Corresponding Source for the Combined Work, excluding any source code
        for portions of the Combined Work that, considered in isolation, are
        based on the Application, and not on the Linked Version.
        
        The "Corresponding Application Code" for a Combined Work means the
        object code and/or source code for the Application, including any data
        and utility programs needed for reproducing the Combined Work from the
        Application, but excluding the System Libraries of the Combined Work.
        
        #### 1. Exception to Section 3 of the GNU GPL.
        
        You may convey a covered work under sections 3 and 4 of this License
        without being bound by section 3 of the GNU GPL.
        
        #### 2. Conveying Modified Versions.
        
        If you modify a copy of the Library, and, in your modifications, a
        facility refers to a function or data to be supplied by an Application
        that uses the facility (other than as an argument passed when the
        facility is invoked), then you may convey a copy of the modified
        version:
        
        -   a) under this License, provided that you make a good faith effort
            to ensure that, in the event an Application does not supply the
            function or data, the facility still operates, and performs
            whatever part of its purpose remains meaningful, or
        -   b) under the GNU GPL, with none of the additional permissions of
            this License applicable to that copy.
        
        #### 3. Object Code Incorporating Material from Library Header Files.
        
        The object code form of an Application may incorporate material from a
        header file that is part of the Library. You may convey such object
        code under terms of your choice, provided that, if the incorporated
        material is not limited to numerical parameters, data structure
        layouts and accessors, or small macros, inline functions and templates
        (ten or fewer lines in length), you do both of the following:
        
        -   a) Give prominent notice with each copy of the object code that
            the Library is used in it and that the Library and its use are
            covered by this License.
        -   b) Accompany the object code with a copy of the GNU GPL and this
            license document.
        
        #### 4. Combined Works.
        
        You may convey a Combined Work under terms of your choice that, taken
        together, effectively do not restrict modification of the portions of
        the Library contained in the Combined Work and reverse engineering for
        debugging such modifications, if you also do each of the following:
        
        -   a) Give prominent notice with each copy of the Combined Work that
            the Library is used in it and that the Library and its use are
            covered by this License.
        -   b) Accompany the Combined Work with a copy of the GNU GPL and this
            license document.
        -   c) For a Combined Work that displays copyright notices during
            execution, include the copyright notice for the Library among
            these notices, as well as a reference directing the user to the
            copies of the GNU GPL and this license document.
        -   d) Do one of the following:
            -   0) Convey the Minimal Corresponding Source under the terms of
                this License, and the Corresponding Application Code in a form
                suitable for, and under terms that permit, the user to
                recombine or relink the Application with a modified version of
                the Linked Version to produce a modified Combined Work, in the
                manner specified by section 6 of the GNU GPL for conveying
                Corresponding Source.
            -   1) Use a suitable shared library mechanism for linking with
                the Library. A suitable mechanism is one that (a) uses at run
                time a copy of the Library already present on the user's
                computer system, and (b) will operate properly with a modified
                version of the Library that is interface-compatible with the
                Linked Version.
        -   e) Provide Installation Information, but only if you would
            otherwise be required to provide such information under section 6
            of the GNU GPL, and only to the extent that such information is
            necessary to install and execute a modified version of the
            Combined Work produced by recombining or relinking the Application
            with a modified version of the Linked Version. (If you use option
            4d0, the Installation Information must accompany the Minimal
            Corresponding Source and Corresponding Application Code. If you
            use option 4d1, you must provide the Installation Information in
            the manner specified by section 6 of the GNU GPL for conveying
            Corresponding Source.)
        
        #### 5. Combined Libraries.
        
        You may place library facilities that are a work based on the Library
        side by side in a single library together with other library
        facilities that are not Applications and are not covered by this
        License, and convey such a combined library under terms of your
        choice, if you do both of the following:
        
        -   a) Accompany the combined library with a copy of the same work
            based on the Library, uncombined with any other library
            facilities, conveyed under the terms of this License.
        -   b) Give prominent notice with the combined library that part of it
            is a work based on the Library, and explaining where to find the
            accompanying uncombined form of the same work.
        
        #### 6. Revised Versions of the GNU Lesser General Public License.
        
        The Free Software Foundation may publish revised and/or new versions
        of the GNU Lesser General Public License from time to time. Such new
        versions will be similar in spirit to the present version, but may
        differ in detail to address new problems or concerns.
        
        Each version is given a distinguishing version number. If the Library
        as you received it specifies that a certain numbered version of the
        GNU Lesser General Public License "or any later version" applies to
        it, you have the option of following the terms and conditions either
        of that published version or of any later version published by the
        Free Software Foundation. If the Library as you received it does not
        specify a version number of the GNU Lesser General Public License, you
        may choose any version of the GNU Lesser General Public License ever
        published by the Free Software Foundation.
        
        If the Library as you received it specifies that a proxy can decide
        whether future versions of the GNU Lesser General Public License shall
        apply, that proxy's public statement of acceptance of any version is
        permanent authorization for you to choose that version for the
        Library.
        
Project-URL: Homepage, https://github.com/Matteodambr/SPART
Project-URL: Documentation, https://spart.readthedocs.io
Project-URL: Issues, https://github.com/Matteodambr/SPART/issues
Keywords: multi body systems,space robotics,dynamics,kinematics,free-flying system
Classifier: Development Status :: 4 - Beta
Classifier: Intended Audience :: Science/Research
Classifier: License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3.8
Classifier: Programming Language :: Python :: 3.9
Classifier: Programming Language :: Python :: 3.10
Classifier: Programming Language :: Python :: 3.11
Classifier: Programming Language :: Python :: 3.12
Classifier: Topic :: Scientific/Engineering
Classifier: Topic :: Scientific/Engineering :: Physics
Requires-Python: >=3.8
Description-Content-Type: text/markdown
License-File: LICENSE.md
License-File: AUTHORS.md
Requires-Dist: numpy
Dynamic: license-file

# 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<sub>I</sub> = R<sub>body2I</sub> · V<sub>B</sub>

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<sub>B</sub> = R<sub>I2body</sub> · V<sub>I</sub>

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

Ṙ<sub>I2body</sub> = −[ω<sub>B</sub>]<sub>×</sub> R<sub>I2body</sub>

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<sub>I</sub> = R · V<sub>B</sub> | `kinematics`, `diff_kinematics`, `i_i` |
| `R0_I2body` | I → B | V<sub>B</sub> = R · V<sub>I</sub> | 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

GNU Lesser General Public License v3 (LGPLv3).  See [LICENSE.md](https://github.com/Matteodambr/SPART/blob/master/LICENSE.md).
