MAGMA: Multiplier-Augmented Geometric Motion Algorithm

A trajectory optimization framework for constrained robotic systems using Pseudo-Spectral methods and Augmented Lagrangian optimization.

Overview

PS-AGHF solves optimal control problems for articulated robots by computing trajectories that:

• Minimize control effort (torque squared)
• Satisfy obstacle avoidance constraints
• Respect joint position, velocity, and torque limits
• Maintain dynamic feasibility

The solver uses Chebyshev pseudospectral discretization combined with Augmented Lagrangian Methods (ALM) for handling constraints.

Project Structure

ps_aghf/
├── src/ps_aghf/          # Core Python package
│   ├── aghf.py           # Main AGHF solver
│   ├── experiment.py     # Experiment orchestration (Phase 1/2 solving)
│   ├── parameter_set.py  # Solver configuration and constraint definitions
│   ├── post_processing.py# Result analysis and validation
│   ├── cheb.py           # Chebyshev polynomial utilities
│   └── ...
├── cpp/                  # C++ backend (pybind11 bindings)
│   ├── include/          # Header files
│   └── src/              # Source files
├── timing_scripts/       # Benchmarking and experiment scripts
│   └── constrained/      # Constrained optimization experiments

└── urdfs/                # Robot URDF model files

Running Other Methods

• BLAZE_PHLAME
• BLAZE_PHLAME_exp2
• aligator

Each of these methods have their own, self-contained, subdirectories in /comparisons.

Installation

Prerequisites

• Python 3.11+
• Conda (recommended for environment management)
• C++ compiler with C++14 support

Environment Setup

1. Create the conda environment:

cd ps_aghf
conda env create -f new_ps_aghf_v1_env.yml
conda activate ps_aghf_v2

2. Build the C++ extension:

cd cpp
make

3. Export Python paths:

export PYTHONPATH="/path/to/your/local/repo/AGHF/ps_aghf/src:/path/to/your/local/repo/AGHF/ps_aghf/src/ps_aghf"

Usage

Running Experiments

Two main experiment scripts are provided in timing_scripts/constrained/:

1. augLag_testing.py - Scenarios with box constraints, Experiment 3

python timing_scripts/constrained/augLag_testing.py

2. augLag_exp2.py - Scenarios with sphere obstacles, Experiment 2

python timing_scripts/constrained/augLag_exp2.py

Basic Usage Example

from ps_aghf.experiment import Experiment
from ps_aghf.parameter_set import ParameterSetActivatedCompositeALM
import numpy as np

# Define start and end states
X0 = np.array([q_start, qd_start]).flatten()
Xf = np.array([q_end, qd_end]).flatten()

# Define constraints
constraint_list = [
    {
        "type": "obstacles",
        "obstacles_info": obs_spheres,  # (4, n_obs) array: x, y, z, radius
        "c_cons_obs": 200,
        "k_cons_obs": 1e5
    },
    {
        "type": "state",
        "x_lower": x_min.reshape(-1, 1),
        "x_upper": x_max.reshape(-1, 1),
        "c_cons_state": 200,
        "k_cons_state": 1e5
    },
    {
        "type": "input",
        "u_lower": u_min.reshape(-1, 1),
        "u_upper": u_max.reshape(-1, 1),
        "c_cons_input": 200,
        "k_cons_input": 1e5
    }
]

# Create parameter set
pset = ParameterSetActivatedCompositeALM(
    p=7,                    # Chebyshev polynomial order
    N=7,                    # Number of joints
    X0=X0, Xf=Xf,
    name="my_experiment",
    s_max=500,              # Max homotopy evolution
    k=1e5,                  # Dynamic feasibilty
    abs_tol=1e-4, rel_tol=1e-4,
    method_name="cvode",
    max_steps=int(1e8),
    j_type=j_type,          # Joint types
    fp_urdf=fp_urdf,        # Path to URDF
    constraint_list=constraint_list
)

# Run two-phase optimization
experiment = Experiment(
    parameter_sets_ph2=[pset],
    timeout=60,
    folder_store="./results"
)

result_states, result_multipliers = experiment.run_ALM_two_phase(
    pset,
    pset_ph1=pset_ph1,      # Phase 1 parameter set
    mode="ALM",
    skip_ph1=False,
    force_ph2=True
)

Constraint Types

Type	Description	Key Parameters
obstacles	Sphere obstacle avoidance	obstacles_info, c_cons_obs, k_cons_obs
boxes	Box obstacle avoidance	boxes_data, joint_radii, c_cons_boxes, k_cons_boxes
state	Joint position/velocity limits	x_lower, x_upper, c_cons_state, k_cons_state
input	Torque limits	u_lower, u_upper, c_cons_input, k_cons_input
Future editions of our solver will feature dynamic feasibility as an equality constraint!

Two-Phase Optimization

The solver uses a two-phase approach:

1. Phase 1: Find a feasible trajectory without minimizing u^2
2. Phase 2: Refine the solution with u^2 minimization

This approach improves convergence for difficult problems with tight constraints.

Configuration Parameters

Solver Parameters

• p: Chebyshev polynomial order (typically 5-9)
• s_max: Max homotopy duration for heat flow evolution
• k: Controls the level of dynamic feasibility enforcement(usually 1e5)
• abs_tol, rel_tol: ODE solver tolerances

Constraint Parameters

• c_cons_*: Controls constraint activation sharpness
• k_cons_*: Constraint penalty weight, can be tuned to some extent

Output

Results are saved as pickle files containing:

• Optimized trajectory (sol)
• Constraint violation metrics
• Timing information
• Post-processing data (forward simulation validation)