pinocchio-rust 0.1.0
A Python extension module written in Rust using PyO3
Requires Python: >=3.8
Keywords: python, rust, pyo3, extension-module

# Pinocchio Rust Model

A Rust reimplementation of Pinocchio's core `Model` data structure for representing robotic kinematic trees.

## Overview

This library provides a simplified but faithful Rust port of the C++ [Pinocchio](https://github.com/stack-of-tasks/pinocchio) library's model data structures. It's designed for robotics applications that need to represent kinematic trees with joints, links, frames, and physical properties.

## Features

- ✅ Core spatial algebra types: `SE3`, `Inertia`, `Motion`
- ✅ Kinematic tree representation with joints and frames
- ✅ Multiple joint types: Fixed, Revolute, Prismatic, Floating, etc.
- ✅ Frame management (Joint, Body, Sensor, FixedJoint, OpFrame)
- ✅ Physical parameters: inertia, friction, damping, limits
- ✅ Tree traversal support: parents, children, supports, subtrees
- ✅ Configuration space indexing (nq, nv)
- ✅ Built on [nalgebra](https://nalgebra.org/) for linear algebra

## Data Structure Comparison

### C++ Pinocchio Model → Rust Model

```cpp
// C++ Pinocchio
struct ModelTpl {
    int nq, nv, njoints;
    InertiaVector inertias;
    SE3Vector jointPlacements;
    JointModelVector joints;
    std::vector<std::string> names;
    std::vector<JointIndex> parents;
    FrameVector frames;
    // ... more fields
};
```

```rust
// Rust equivalent
pub struct Model {
    pub nq: usize, 
    pub nv: usize,
    pub njoints: usize,
    pub inertias: Vec<Inertia>,
    pub joint_placements: Vec<SE3>,
    pub joints: Vec<JointModel>,
    pub names: Vec<String>,
    pub parents: Vec<JointIndex>,
    pub frames: Vec<Frame>,
    // ... more fields
}
```

## Installation

Add this to your `Cargo.toml`:

```toml
[dependencies]
pinocchio_rust = { path = "path/to/pinocchio_rust" }
nalgebra = "0.33"
```

## Quick Start

```rust
use pinocchio_rust::{Model, JointModel, SE3, Inertia};
use nalgebra::Vector3;

fn main() {
    // Create a new model
    let mut model = Model::new();
    model.set_name("my_robot".to_string());

    // Add a revolute joint
    let joint_id = model.add_joint(
        0, // parent: universe
        JointModel::revolute(Vector3::new(0.0, 0.0, 1.0)), // Z-axis rotation
        SE3::identity(),
        "joint_1".to_string(),
    );

    // Add inertia to the joint
    let inertia = Inertia::new(
        1.0, // mass (kg)
        Vector3::new(0.0, 0.0, 0.1), // center of mass
        Symmetric3::zeros(), // rotational inertia
    );
    model.append_body_to_joint(joint_id, inertia);

    // Print model info
    model.print_info();
}
```

## Examples

### Basic Robot Arm

```bash
cargo run --example basic_model
```

This example creates a 3-DOF robotic arm and demonstrates:
- Creating joints with different types
- Adding inertia properties
- Managing frames
- Traversing the kinematic tree

Output:
```
╔════════════════════════════════════════════════╗
║   Pinocchio Rust Model Example                ║
╚════════════════════════════════════════════════╝

========== Model Information ==========
Model name: simple_robot
Number of joints: 4
Number of frames: 8
Number of DOF (nq): 3
Number of velocity DOF (nv): 3
...
```

## Core Types

### `SE3` - Rigid Body Transformation

```rust
pub struct SE3 {
    pub rotation: Matrix3<f64>,    // 3x3 rotation matrix
    pub translation: Vector3<f64>, // 3D translation vector
}
```

Represents rigid body transformations in SE(3). Supports:
- Composition: `a_M_b.compose(&b_M_c)` → `a_M_c`
- Inverse: `a_M_b.inverse()` → `b_M_a`
- Point transformation: `transform.transform_point(&p)`
- Homogeneous matrix conversion

### `Inertia` - Spatial Inertia

```rust
pub struct Inertia {
    pub mass: f64,           // Mass (kg)
    pub com: Vector3<f64>,   // Center of mass (m)
    pub inertia: Symmetric3, // Rotational inertia (kg⋅m²)
}
```

Represents the spatial inertia of a rigid body.

### `JointModel` - Joint Type and Configuration

```rust
pub enum JointType {
    Fixed,      // 0 DOF
    Revolute,   // 1 DOF rotation
    Prismatic,  // 1 DOF translation
    Planar,     // 3 DOF (2 trans + 1 rot)
    Floating,   // 6 DOF
    Continuous, // 1 DOF unbounded rotation
    Spherical,  // 3 DOF rotation
}
```

### `Frame` - Coordinate Frames

```rust
pub struct Frame {
    pub name: String,
    pub parent_joint: JointIndex,
    pub parent_frame: FrameIndex,
    pub placement: SE3,
    pub frame_type: FrameType,
    pub inertia: Inertia,
}

pub enum FrameType {
    OpFrame,    // Operational frame
    Joint,      // Joint frame
    FixedJoint, // Fixed joint
    Body,       // Body frame
    Sensor,     // Sensor frame
}
```

## Testing

Run tests:
```bash
cargo test
```

Run with output:
```bash
cargo test -- --nocapture
```

## use build_wheels.sh
Compilation depends on uv, please install: https://docs.astral.sh/uv/getting-started/installation/

Windows ARM64 build prepare:
```shell
sudo apt-get install llvm
```
Linux ARM64 build prepare:
```shell
sudo apt-get install gcc-aarch64-linux-gnu
```

## develop with maturin
Maturin is a tool for developing python package base on rust.
Requires:
1. cargo.toml
2. .venv (can use uv to crate)

Update rust code to python venv:
```shell
maturin develop --release
```

## References

- [Pinocchio C++ Library](https://github.com/stack-of-tasks/pinocchio)
- [Pinocchio Documentation](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/)