COPP2-SOCP

COPP2-SOCP solves a second-order convex-objective path-parameterization problem with Clarabel. It uses the same path and constraint construction as TOPP2, but the problem descriptor also owns an objective list:

objectives = [
    copp.objective.Time(1.0),
    copp.objective.ThermalEnergy(0.1, np.ones(dim)),
]

With no inverse-dynamics callback installed, Robot uses point-mass dynamics for torque-related objective terms and constraints:

\[\tau = \ddot{q}.\]

For real robots, pass a callable or object with inverse_dynamics(q, dq, ddq) when constructing Robot.

Runnable Example

 1"""Use COPP2-SOCP to solve a second-order convex-objective timing problem.
 2
 3The example combines traversal-time and thermal-energy objectives under
 4velocity and acceleration limits, then samples the resulting trajectory in time.
 5"""
 6
 7import numpy as np
 8
 9import copp_py as copp
10
11
12def main() -> None:
13    try:
14        import jax
15        import jax.numpy as jnp
16    except ImportError as exc:
17        raise SystemExit(
18            'Install JAX to run this example: python -m pip install "copp-py[jax]"'
19        ) from exc
20
21    jax.config.update("jax_enable_x64", True)
22
23    dim = 3
24    n = 1001
25    dt = 1.0e-3
26
27    # 1) Define q(s). Path.from_jax differentiates it up to third order.
28    def q_fn(s):
29        freq = jnp.array([2.0 * jnp.pi, 3.0 * jnp.pi, 5.0 * jnp.pi], dtype=jnp.float64)
30        phase = jnp.array([0.0, 0.3, 0.7], dtype=jnp.float64)
31        return jnp.sin(freq * s + phase)
32
33    path = copp.Path.from_jax(q_fn, 0.0, 1.0)
34    s = np.linspace(0.0, 1.0, n, dtype=np.float64)
35
36    # 2) Build robot constraints (3-axis), then apply symmetric limits
37    # velocity/acceleration = [-1, 1].
38    robot = copp.Robot(dim, capacity=n)
39    robot.append_s(s)
40    robot.set_q_from_path_2nd(path, 0, n)
41
42    upper = np.ones(dim, dtype=np.float64)
43    lower = -upper
44    robot.add_velocity_limits(upper, lower, start_idx_s=0, length=n)
45    robot.add_acceleration_limits(upper, lower, start_idx_s=0, length=n)
46
47    # 3) Build COPP2 problem and solve COPP2-SOCP with Clarabel.
48    # Here we use a hybrid objective: 1.0 * time + 0.1 * thermal energy.
49    # With no inverse-dynamics callback installed, Robot uses point dynamics
50    # (`tau = ddq`), matching the reference tutorial setup.
51    objectives = [
52        copp.objective.Time(1.0),
53        copp.objective.ThermalEnergy(0.1, np.ones(dim, dtype=np.float64)),
54    ]
55    problem = copp.solver.copp2_socp.Problem(
56        robot,
57        objectives,
58        idx_s_interval=(0, n - 1),
59        a_boundary=(0.0, 0.0),
60    )
61    options = copp.solver.copp2_socp.Options(
62        allow_almost_solved=True,
63        allow_insufficient_progress=True,
64    )
65    a_socp = copp.solver.copp2_socp.solve(problem, options)
66
67    # 4) Post-process COPP2-SOCP results: a(s) -> t(s) -> s(t).
68    t_final, t_s = copp.interpolation.s_to_t_topp2(s, a_socp, 0.0)
69    s_t = copp.interpolation.t_to_s_topp2_uniform(
70        s,
71        a_socp,
72        t_s,
73        dt,
74        t0=0.0,
75        include_final=True,
76    )
77
78    # 5) Print the tutorial summary.
79    print("COPP2-SOCP done.")
80    print(f"dim = {dim}, N = {n}")
81    print(f"t_final = {t_final:.6f} s")
82    print(f"a_profile.len() = {len(a_socp)}")
83    print(f"s(t) samples = {len(s_t)}")
84
85
86if __name__ == "__main__":
87    main()