COPP3-SOCP

COPP3-SOCP extends the third-order conic formulation to convex objectives. The path and robot setup is the same as TOPP3-SOCP, but the problem descriptor stores a Robot and an objective list instead of only a raw constraint proxy.

This distinction matters for objective terms such as thermal energy and torque variation. Those objectives need robot derivative data and, when available, an inverse-dynamics callback.

Runnable Example

  1"""Use COPP3-SOCP to solve a jerk-limited convex-objective timing problem.
  2
  3The example combines traversal-time and thermal-energy objectives, seeds the
  4third-order model with TOPP2-RA, and performs one refinement around the first
  5SOCP solution.
  6"""
  7
  8import numpy as np
  9
 10import copp_py as copp
 11
 12
 13def main() -> None:
 14    try:
 15        import jax
 16        import jax.numpy as jnp
 17    except ImportError as exc:
 18        raise SystemExit(
 19            'Install JAX to run this example: python -m pip install "copp-py[jax]"'
 20        ) from exc
 21
 22    jax.config.update("jax_enable_x64", True)
 23
 24    dim = 3
 25    n = 1001
 26    dt = 1.0e-3
 27
 28    # 1) Define q(s). Path.from_jax differentiates it up to third order.
 29    def q_fn(s):
 30        freq = jnp.array([2.0 * jnp.pi, 3.0 * jnp.pi, 5.0 * jnp.pi], dtype=jnp.float64)
 31        phase = jnp.array([0.0, 0.3, 0.7], dtype=jnp.float64)
 32        return jnp.sin(freq * s + phase)
 33
 34    path = copp.Path.from_jax(q_fn, 0.0, 1.0)
 35    s = np.linspace(0.0, 1.0, n, dtype=np.float64)
 36
 37    # 2) Build robot constraints (3-axis), then apply symmetric limits
 38    # velocity/acceleration/jerk = [-1, 1].
 39    robot = copp.Robot(dim, capacity=n)
 40    robot.append_s(s)
 41    robot.set_q_from_path_3rd(path, 0, n)
 42
 43    upper = np.ones(dim, dtype=np.float64)
 44    lower = -upper
 45    robot.add_velocity_limits(upper, lower, start_idx_s=0, length=n)
 46    robot.add_acceleration_limits(upper, lower, start_idx_s=0, length=n)
 47    robot.add_jerk_limits(upper, lower, start_idx_s=0, length=n)
 48
 49    # 3) Use TOPP2-RA as the initial a(s) linearization for the third-order model.
 50    topp2_problem = copp.solver.topp2_ra.Problem(
 51        robot.constraints,
 52        idx_s_interval=(0, n - 1),
 53        a_boundary=(0.0, 0.0),
 54    )
 55    a_ra0 = copp.solver.topp2_ra.solve(topp2_problem, copp.solver.topp2_ra.Options())
 56    robot.constraints.amax_substitute(a_ra0, 0)
 57
 58    # 4) Build COPP3 objective terms and solve COPP3-SOCP twice.
 59    objectives = [
 60        copp.objective.Time(1.0),
 61        copp.objective.ThermalEnergy(0.1, np.ones(dim, dtype=np.float64)),
 62    ]
 63    options = copp.solver.copp3_socp.Options(allow_almost_solved=True)
 64
 65    problem1 = copp.solver.copp3_socp.Problem(
 66        robot,
 67        objectives,
 68        a_ra0,
 69        idx_s_start=0,
 70        a_boundary=(0.0, 0.0),
 71        b_boundary=(0.0, 0.0),
 72        num_stationary_max=1,
 73    )
 74    profile1 = copp.solver.copp3_socp.solve(problem1, options)
 75    t_final1, t_s1 = copp.interpolation.s_to_t_topp3(s, profile1, 0.0)
 76    s_t1 = copp.interpolation.t_to_s_topp3_uniform(
 77        s,
 78        profile1,
 79        t_s1,
 80        dt,
 81        t0=0.0,
 82        include_final=True,
 83    )
 84
 85    problem2 = copp.solver.copp3_socp.Problem(
 86        robot,
 87        objectives,
 88        profile1.a,
 89        idx_s_start=0,
 90        a_boundary=(0.0, 0.0),
 91        b_boundary=(0.0, 0.0),
 92        num_stationary_max=1,
 93    )
 94    profile2 = copp.solver.copp3_socp.solve(problem2, options)
 95    t_final2, t_s2 = copp.interpolation.s_to_t_topp3(s, profile2, 0.0)
 96    s_t2 = copp.interpolation.t_to_s_topp3_uniform(
 97        s,
 98        profile2,
 99        t_s2,
100        dt,
101        t0=0.0,
102        include_final=True,
103    )
104
105    # 5) Print the tutorial summary.
106    print("COPP3-SOCP done. (The first iteration)")
107    print(f"dim = {dim}, N = {n}")
108    print(f"t_final = {t_final1:.6f} s")
109    print(f"a_profile.len() = {len(profile1.a)}")
110    print(f"b_profile.len() = {len(profile1.b)}")
111    print(f"s(t) samples = {len(s_t1)}")
112    print("---------")
113    print("COPP3-SOCP done. (The second iteration)")
114    print(f"t_final = {t_final2:.6f} s <= {t_final1:.6f} s")
115    print(f"a_profile.len() = {len(profile2.a)}")
116    print(f"b_profile.len() = {len(profile2.b)}")
117    print(f"s(t) samples = {len(s_t2)}")
118
119
120if __name__ == "__main__":
121    main()