copp 0.2.2

Convex-objective path parameterization for robotic trajectory planning.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107

// Tutorial example: install a C++ inverse-dynamics callback.
//
// The callback is used by torque limits and torque objectives. This example
// keeps the dynamics intentionally small so the data flow is visible:
// tau = inertia * ddq + damping * dq + gravity * sin(q).

#include <cmath>
#include <iostream>
#include <vector>

#include <copp/copp.hpp>

struct ToyArmDynamics
{
    std::vector<double> inertia{1.2, 0.8};
    std::vector<double> damping{0.08, 0.05};
    std::vector<double> gravity{0.4, 0.25};

    void operator()(
        copp::Span<const double> q,
        copp::Span<const double> dq,
        copp::Span<const double> ddq,
        copp::Span<double> tau) const
    {
        // The callback receives one state at a time. All spans have length equal
        // to the robot dimension, and `tau` must be fully overwritten.
        for (std::size_t i = 0; i < tau.size(); ++i)
        {
            tau[i] = inertia[i] * ddq[i] + damping[i] * dq[i] + gravity[i] * std::sin(q[i]);
        }
    }
};

int main()
{
    // Step 1: Create stations and joint/torque limits. Torque limits will use
    // the callback below instead of the default point-mass model.
    std::vector<double> s{0.0, 0.25, 0.5, 0.75, 1.0};
    std::vector<double> upper{3.0, 3.0};
    std::vector<double> lower{-3.0, -3.0};
    std::vector<double> torque_upper{50.0, 50.0};
    std::vector<double> torque_lower{-50.0, -50.0};

    // Step 2: Build a path with derivatives available through Jet3.
    auto path = copp::Path::from_parametric(
        [](copp::Jet3 x)
        {
            return std::vector<copp::Jet3>{
                x,
                copp::sin(x),
            };
        },
        0.0,
        1.0);

    ToyArmDynamics dynamics;

    // Step 3: Install the callback. Captured objects must remain valid for the
    // period in which torque constraints/objectives are constructed.
    copp::Robot robot(2, s.size());
    robot.set_inverse_dynamics(
        [dynamics](
            copp::Span<const double> q,
            copp::Span<const double> dq,
            copp::Span<const double> ddq,
            copp::Span<double> tau)
        {
            dynamics(q, dq, ddq, tau);
        });

    // Step 4: Add velocity, acceleration, and torque constraints. The torque
    // call crosses the Rust/C++ bridge into `ToyArmDynamics`.
    robot.append_s(s)
        .set_q_from_path_2nd(path, 0, s.size())
        .add_velocity_limits(upper, lower, 0, s.size())
        .add_acceleration_limits(upper, lower, 0, s.size())
        .add_torque_limits(torque_upper, torque_lower, 0, s.size());

    namespace copp2 = copp::solver::copp2_socp;

    // Step 5: ThermalEnergy uses inverse dynamics as part of the objective.
    copp2::Problem problem{
        robot,
        {
            copp::objective::Time(1.0),
            copp::objective::ThermalEnergy(0.05, {1.0, 1.0}),
        },
        copp::IndexInterval{0, s.size() - 1},
        copp::Boundary2{0.0, 0.0},
    };

    // Step 6: Expert output exposes accepted profile and objective diagnostics.
    auto expert = copp2::solve_expert(problem);
    if (!expert.a)
    {
        std::cerr << "Clarabel did not return an accepted profile\n";
        return 1;
    }

    auto time = copp::interpolation::s_to_t_topp2(s, *expert.a, 0.0);
    std::cout << "has_inverse_dynamics = " << robot.has_inverse_dynamics() << "\n";
    std::cout << "accepted a length = " << expert.a->size() << "\n";
    std::cout << "t_final = " << time.t_final << "\n";
    std::cout << "objective = " << *expert.objective_value << "\n";

    return 0;
}