Module copp2_socp 

COPP2 SOCP backend (Clarabel).

Input: COPP2 problem + convex objective + SOCP/Clarabel options. Output: conic-optimization based solution and conversion helpers. Scenario: when conic formulation is preferred over DP-style solvers.

Example

//! This example uses [`copp2_socp`] to convert an analytic path into a
//! second-order convex-objective trajectory whose axial velocity and acceleration
//! both stay within `[-1, 1]`.

use copp::InterpolationMode;
use copp::diag::CoppError;
use copp::path::{Jet3, Path, sin};
use copp::robot::Robot;
use copp::solver::copp2_socp::{
    ClarabelOptionsBuilder, Copp2ProblemBuilder, CoppObjective, copp2_socp, s_to_t_topp2,
    t_to_s_topp2,
};
use std::f64::consts::PI;

fn main() -> Result<(), CoppError> {
    // 1) Deterministic 3-axis Lissajous path q(s), s in [0, 1]
    let path = Path::from_parametric(
        |s: Jet3| {
            vec![
                sin(2.0 * PI * s + 0.0),
                sin(3.0 * PI * s + 0.3),
                sin(5.0 * PI * s + 0.7),
            ]
        },
        0.0,
        1.0,
    )?;

    // `n` is the number of path samples (s_i) to build robot constraints on.
    let n = 1001;
    let s: Vec<f64> = (0..n).map(|j| j as f64 / (n - 1) as f64).collect();

    // 2) Build robot constraints (3-axis), then apply symmetric limits vel/acc = 1
    const DIM: usize = 3;
    let mut robot = Robot::with_capacity(DIM, n);
    // The axial velocity is -1 <= vel <= 1 for each axis in this example
    let vel_max = vec![1.0; DIM];
    let vel_min = vec![-1.0; DIM];
    // The axial acceleration is -1 <= acc <= 1 for each axis in this example.
    let acc_max = vec![1.0; DIM];
    let acc_min = vec![-1.0; DIM];
    robot
        .with_s(s.as_slice())?
        .with_q_from_path_2nd(&path, 0, n)?
        .with_axial_velocity((vel_max.as_slice(), n), (vel_min.as_slice(), n), 0)?
        .with_axial_acceleration((acc_max.as_slice(), n), (acc_min.as_slice(), n), 0)?;

    // 3) Build COPP2 problem and solve COPP2-SOCP (Clarabel backend)
    // Here we use a hybrid objective: 1.0 * time + 0.1 * thermal energy.
    // We use `usize` as a trivial point-mass robot model (inverse dynamics: `tau = ddq`) in this example.
    // The user should replace this with their real robot model where traits `RobotBasic` and `RobotTorque` are implemented.
    let objectives = [
        CoppObjective::Time(1.0),
        CoppObjective::ThermalEnergy(0.1, &[1.0; DIM]),
    ];
    let idx_s_interval = (0, n - 1); // 0 <= k <= n-1
    let a_boundary = (0.0, 0.0); // a(0) = 0, a(1) = 0
    let problem =
        Copp2ProblemBuilder::new(&robot, idx_s_interval, a_boundary, &objectives).build()?;
    let options = ClarabelOptionsBuilder::new()
        .allow_almost_solved(true)
        .build()?;

    let a_socp = copp2_socp(&problem, &options)?;

    // 4) Post-process COPP2-SOCP results: a(s) -> t(s) -> s(t)
    // t_final is the traversal time of the path.
    // t_s[i] is the time at which the path parameter s_i is reached.
    let (t_final, t_s) = s_to_t_topp2(&s, &a_socp, 0.0)?;
    // s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
    let dt = 1e-3;
    let s_t = t_to_s_topp2(
        &s,
        &a_socp,
        &t_s,
        InterpolationMode::UniformTimeGrid(0.0, dt, true),
    )?;

    // 5) Print some results. More detailed results and plots can be achieved by the user.
    println!("COPP2-SOCP done.");
    println!("dim = {DIM}, N = {n}");
    println!("t_final = {t_final:.6} s");
    println!("a_profile.len() = {}", a_socp.len());
    println!("s(t) samples = {}", s_t.len());

    Ok(())
}

Structs

ClarabelExpertInfor2nd

Clarabel expert result for second-order optimization backends.

ClarabelOptions

Shared options for Clarabel-based optimization routines.

ClarabelOptionsBuilder

Builder for ClarabelOptions.

Copp2Problem

Formulated COPP2 problem data.

Copp2ProblemBuilder

Builder for Copp2Problem.

Enums

CoppObjective

Objective terms for COPP optimization. Continuous formulation is shared by COPP2/COPP3; discrete form depends on how b and torque are sampled. Torque notation:

Functions

a_to_b_topp2

Compute segment profile b from node profile a.

clarabel_to_copp2_solution

Extract a nonnegative a profile from Clarabel solution vector with minimal copying.

copp2_socp

Strict COPP2-SOCP API for production use.

copp2_socp_expert

Expert COPP2-SOCP API with full Clarabel solution exposure.

copp2_socp_expert_with_info

Expert COPP2-SOCP API with Clarabel solution and linear-solver diagnostics.

s_to_t_topp2

Compute cumulative time profile t(s) from a(s).

t_to_s_topp2

Interpolate inverse mapping s(t) from a(s) and sampled t(s).