Rust, C++, Robotics, Kinematics, Dynamics, Trajectory Planning, Control, Linear Algebra

Author: Allen Liu

Rust Library on GitHub

Rust Documentation

C++ Library on GitHub

C++ Documentation

Project Overview

This project provides comprehensive implementations of fundamental robotics algorithms in both Rust and C++, based on the textbook Modern Robotics: Mechanics, Planning, and Control by Kevin M. Lynch and Frank C. Park (2017). These libraries enable roboticists and researchers to perform essential computations for robot kinematics, dynamics, trajectory generation, and control.

Reference: Official Modern Robotics GitHub Repository

The dual-language approach allows developers to choose the implementation that best fits their project requirements:

• Rust (modern-robotics-rs): Memory-safe, with zero-cost abstractions and modern package management via Cargo
• C++ (modern-robotics-cpp): High-performance with Armadillo linear algebra library, ideal for integration with existing robotics frameworks

Library Architecture

Both libraries follow the textbook’s chapter organization, providing a consistent API across implementations:

graph TB
    subgraph Core["Core Mathematical Foundations"]
        SO3["SO(3) Operations<br/>Rotation Matrices"]
        SE3["SE(3) Operations<br/>Homogeneous Transforms"]
        SCREW["Screw Theory<br/>Twists and Wrenches"]
    end

    subgraph Kinematics["Kinematics Module"]
        FK["Forward Kinematics<br/>Product of Exponentials"]
        IK["Inverse Kinematics<br/>Newton-Raphson"]
        JAC["Jacobian Computation<br/>Body and Space Frame"]
    end

    subgraph Dynamics["Dynamics Module"]
        FD["Forward Dynamics<br/>Joint Accelerations"]
        ID["Inverse Dynamics<br/>Joint Torques"]
        MASS["Mass Matrix<br/>Inertia Computation"]
    end

    subgraph Trajectory["Trajectory Generation"]
        POLY["Polynomial Scaling<br/>Cubic & Quintic"]
        JOINT["Joint Space<br/>Trajectories"]
        CART["Cartesian Space<br/>SE(3) Interpolation"]
    end

    subgraph Control["Control Module"]
        CT["Computed Torque<br/>Model-Based Control"]
        PID["PID Feedback<br/>Error Correction"]
    end

    SO3 --> SE3
    SE3 --> SCREW
    SCREW --> FK
    SCREW --> JAC
    FK --> IK
    JAC --> ID
    JAC --> FD
    ID --> MASS
    FD --> MASS
    POLY --> JOINT
    POLY --> CART
    MASS --> CT
    CT --> PID

    style Core fill:#e1f5ff
    style Kinematics fill:#fff4e1
    style Dynamics fill:#d4edda
    style Trajectory fill:#fce4ec
    style Control fill:#f3e5f5

Mathematical Foundations

Rigid Body Representations

The libraries implement the mathematical machinery for representing rigid body motions in 3D space:

flowchart LR
    subgraph so3["so(3) - Lie Algebra"]
        OMEGA["ω ∈ ℝ³<br/>Angular Velocity"]
        SKEW["[ω] ∈ so(3)<br/>Skew-Symmetric"]
    end

    subgraph SO3["SO(3) - Rotation Group"]
        R["R ∈ SO(3)<br/>Rotation Matrix"]
        AXIS["(ω̂, θ)<br/>Axis-Angle"]
    end

    subgraph se3["se(3) - Lie Algebra"]
        TWIST["𝒱 ∈ ℝ⁶<br/>Twist"]
        BRACKET["[𝒱] ∈ se(3)<br/>4×4 Matrix"]
    end

    subgraph SE3["SE(3) - Rigid Motion Group"]
        T["T ∈ SE(3)<br/>Transformation"]
        SCREW_AX["(𝒮, θ)<br/>Screw Axis"]
    end

    OMEGA -->|"VecToso3()"| SKEW
    SKEW -->|"so3ToVec()"| OMEGA
    SKEW -->|"MatrixExp3()"| R
    R -->|"MatrixLog3()"| SKEW
    R <-->|"AxisAng3()"| AXIS

    TWIST -->|"VecTose3()"| BRACKET
    BRACKET -->|"se3ToVec()"| TWIST
    BRACKET -->|"MatrixExp6()"| T
    T -->|"MatrixLog6()"| BRACKET
    T <-->|"AxisAng6()"| SCREW_AX

    style so3 fill:#e1f5ff
    style SO3 fill:#fff4e1
    style se3 fill:#d4edda
    style SE3 fill:#fce4ec

Key Mathematical Operations

Rotation Matrix Exponential (Rodrigues’ Formula):

Homogeneous Transformation Exponential:

Where:

Forward Kinematics

The libraries implement forward kinematics using the Product of Exponentials formula, supporting both body and space frame representations:

flowchart TD
    subgraph Input["Input Parameters"]
        M["M ∈ SE(3)<br/>Home Configuration"]
        SLIST["Screw Axes<br/>{𝒮₁, 𝒮₂, ..., 𝒮ₙ}"]
        THETA["Joint Angles<br/>{θ₁, θ₂, ..., θₙ}"]
    end

    subgraph SpaceFrame["Space Frame FK"]
        EXP_S1["e^{[𝒮₁]θ₁}"]
        EXP_S2["e^{[𝒮₂]θ₂}"]
        EXP_SN["e^{[𝒮ₙ]θₙ}"]
        PROD_S["T = e^{[𝒮₁]θ₁}...e^{[𝒮ₙ]θₙ}M"]
    end

    subgraph BodyFrame["Body Frame FK"]
        EXP_B1["e^{[ℬ₁]θ₁}"]
        EXP_B2["e^{[ℬ₂]θ₂}"]
        EXP_BN["e^{[ℬₙ]θₙ}"]
        PROD_B["T = Me^{[ℬ₁]θ₁}...e^{[ℬₙ]θₙ}"]
    end

    subgraph Output["Output"]
        T_END["T ∈ SE(3)<br/>End-Effector Pose"]
    end

    M --> PROD_S
    M --> PROD_B
    SLIST --> EXP_S1
    SLIST --> EXP_S2
    SLIST --> EXP_SN
    THETA --> EXP_S1
    THETA --> EXP_S2
    THETA --> EXP_SN
    THETA --> EXP_B1
    THETA --> EXP_B2
    THETA --> EXP_BN

    EXP_S1 --> PROD_S
    EXP_S2 --> PROD_S
    EXP_SN --> PROD_S

    EXP_B1 --> PROD_B
    EXP_B2 --> PROD_B
    EXP_BN --> PROD_B

    PROD_S --> T_END
    PROD_B --> T_END

    style Input fill:#e1f5ff
    style SpaceFrame fill:#fff4e1
    style BodyFrame fill:#d4edda
    style Output fill:#fce4ec

Product of Exponentials Formula:

Space Frame: \(T(\theta) = e^{[\mathcal{S}_1]\theta_1} e^{[\mathcal{S}_2]\theta_2} \cdots e^{[\mathcal{S}_n]\theta_n} M\)

Body Frame: \(T(\theta) = M e^{[\mathcal{B}_1]\theta_1} e^{[\mathcal{B}_2]\theta_2} \cdots e^{[\mathcal{B}_n]\theta_n}\)

Inverse Kinematics

The inverse kinematics solver uses the Newton-Raphson iterative method to find joint angles for a desired end-effector pose:

flowchart TD
    START[Start] --> INIT["Initialize θ₀<br/>Initial Guess"]
    INIT --> FK["Compute FK<br/>T(θₖ)"]
    FK --> ERROR["Compute Error<br/>𝒱ᵦ = log(T⁻¹(θₖ)T_d)"]
    ERROR --> CHECK{"||ωᵦ|| < εω<br/>AND<br/>||vᵦ|| < εv ?"}
    CHECK -->|Yes| SUCCESS["Return θₖ<br/>Solution Found"]
    CHECK -->|No| JACOBIAN["Compute Jacobian<br/>Jᵦ(θₖ)"]
    JACOBIAN --> UPDATE["Update θ<br/>θₖ₊₁ = θₖ + J⁺ᵦ𝒱ᵦ"]
    UPDATE --> ITER{"k < max_iter ?"}
    ITER -->|Yes| FK
    ITER -->|No| FAIL["Return Failure<br/>No Convergence"]

    style START fill:#e1f5ff
    style SUCCESS fill:#d4edda
    style FAIL fill:#ffcccc
    style CHECK fill:#fff4e1

Newton-Raphson Update:

Where $ J_b^+ $ is the Moore-Penrose pseudoinverse of the body Jacobian.

Dynamics

The dynamics module implements both forward and inverse dynamics for open-chain robots:

flowchart LR
    subgraph Inverse["Inverse Dynamics"]
        direction TB
        ID_IN["θ, θ̇, θ̈"]
        ID_PROC["Newton-Euler<br/>Algorithm"]
        ID_OUT["τ (Joint Torques)"]
        ID_IN --> ID_PROC --> ID_OUT
    end

    subgraph Forward["Forward Dynamics"]
        direction TB
        FD_IN["θ, θ̇, τ"]
        FD_PROC["Articulated Body<br/>Algorithm"]
        FD_OUT["θ̈ (Accelerations)"]
        FD_IN --> FD_PROC --> FD_OUT
    end

    subgraph MassMatrix["Mass Matrix"]
        direction TB
        MM_IN["θ"]
        MM_PROC["Column-by-Column<br/>Computation"]
        MM_OUT["M(θ)"]
        MM_IN --> MM_PROC --> MM_OUT
    end

    style Inverse fill:#e1f5ff
    style Forward fill:#fff4e1
    style MassMatrix fill:#d4edda

Equations of Motion:

Where:

• $ M(\theta) $ is the mass matrix
• $ c(\theta, \dot{\theta}) $ represents Coriolis and centrifugal terms
• $ g(\theta) $ is the gravity vector

Trajectory Generation

The libraries support multiple trajectory generation methods:

flowchart TB
    subgraph TimeScaling["Time Scaling Functions"]
        CUBIC["Cubic Polynomial<br/>s(t) = 3t² - 2t³"]
        QUINTIC["Quintic Polynomial<br/>s(t) = 10t³ - 15t⁴ + 6t⁵"]
    end

    subgraph JointSpace["Joint Space Trajectory"]
        JS_START["θ_start"]
        JS_END["θ_end"]
        JS_INTERP["θ(s) = θ_start + s(θ_end - θ_start)"]
    end

    subgraph CartesianSpace["Cartesian Space Trajectory"]
        SCREW_TRAJ["Screw Trajectory<br/>SE(3) Interpolation"]
        DECOUPLED["Decoupled Trajectory<br/>R(s), p(s) separate"]
    end

    CUBIC --> JS_INTERP
    QUINTIC --> JS_INTERP
    CUBIC --> SCREW_TRAJ
    QUINTIC --> SCREW_TRAJ
    CUBIC --> DECOUPLED
    QUINTIC --> DECOUPLED

    JS_START --> JS_INTERP
    JS_END --> JS_INTERP

    style TimeScaling fill:#e1f5ff
    style JointSpace fill:#fff4e1
    style CartesianSpace fill:#d4edda

Quintic Time Scaling (Zero Velocity and Acceleration at Endpoints):

Computed Torque Control

The control module implements computed torque control with PID feedback:

flowchart LR
    subgraph Reference["Reference"]
        TRAJ_REF["θ_d, θ̇_d, θ̈_d"]
    end

    subgraph Feedback["Feedback Controller"]
        ERROR_CALC["Error Calculation<br/>e = θ_d - θ"]
        PID_CTRL["PID Controller<br/>K_p·e + K_i∫e + K_d·ė"]
    end

    subgraph Feedforward["Feedforward"]
        DYNAMICS["Inverse Dynamics<br/>M(θ)θ̈_d + c + g"]
    end

    subgraph Plant["Robot"]
        ROBOT["Robot Dynamics<br/>τ → θ̈"]
    end

    TRAJ_REF --> ERROR_CALC
    TRAJ_REF --> DYNAMICS

    ERROR_CALC --> PID_CTRL
    PID_CTRL --> SUM((+))
    DYNAMICS --> SUM

    SUM -->|"τ"| ROBOT
    ROBOT -->|"θ, θ̇"| ERROR_CALC

    style Reference fill:#e1f5ff
    style Feedback fill:#fff4e1
    style Feedforward fill:#d4edda
    style Plant fill:#fce4ec

Computed Torque Control Law:

API Comparison

Code Examples

Rust Implementation

use modern_robotics::{FKinSpace, JacobianSpace};
use nalgebra::{DMatrix, DVector};

// Define screw axes and home configuration
let s_list = DMatrix::from_row_slice(6, 3, &[
    0.0, 0.0, 0.0,  // S1
    0.0, 1.0, 1.0,  // S2
    1.0, 0.0, 0.0,  // S3
    0.0, 0.0, 0.0,  // S4
    0.0, 0.0, 1.0,  // S5
    0.0, 0.0, 0.0,  // S6
]);

let theta = DVector::from_vec(vec![0.1, 0.2, 0.3]);

// Compute forward kinematics
let t_end = FKinSpace(&m, &s_list, &theta);

// Compute Jacobian
let jacobian = JacobianSpace(&s_list, &theta);

C++ Implementation

#include <modern_robotics/modern_robotics.hpp>

// Define screw axes and home configuration
arma::mat Slist = {{0, 0, 0, 0, 0, 0},
                   {0, 1, 1, 0, 0, 0},
                   {1, 0, 0, 0, 0, 1}};
Slist = Slist.t();

arma::vec theta = {0.1, 0.2, 0.3};

// Compute forward kinematics
arma::mat T_end = mr::FKinSpace(M, Slist, theta);

// Compute Jacobian
arma::mat Jacobian = mr::JacobianSpace(Slist, theta);

Technical Requirements

Rust Library

C++ Library

Function Reference

Rigid-Body Motions (Chapter 3)

Kinematics (Chapters 4-6)

Dynamics (Chapter 8)

Trajectory (Chapter 9)

Control (Chapter 11)

License

Both libraries are released under the MIT License.