TOPP3-SOCP

TOPP3-SOCP solves the third-order time-optimal problem with Clarabel. It is the recommended open-source third-order method when solution quality matters more than raw solve speed.

Like other TOPP3/COPP3 wrappers, the problem descriptor requires an a_linearization profile. In practice, a TOPP2-RA seed is usually good enough for the first iteration, and the previous TOPP3-SOCP solution is a natural seed for the second iteration.

The strict API returns a Profile3rd. Use socp_expert when you need raw Clarabel status, residuals, primal/dual vectors, or accepted-profile checks.

Runnable Example

  1"""Use TOPP3-SOCP to convert a JAX-defined path into a jerk-limited time-optimal trajectory.
  2
  3The example builds a TOPP2-RA seed, solves a third-order ``(a,b)`` profile with
  4Clarabel, and performs one refinement around the first solution.
  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/jerk = [-1, 1].
 38    robot = copp.Robot(dim, capacity=n)
 39    robot.append_s(s)
 40    robot.set_q_from_path_3rd(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    robot.add_jerk_limits(upper, lower, start_idx_s=0, length=n)
 47
 48    # 3) Use TOPP2-RA as the initial a(s) linearization for the third-order model.
 49    topp2_problem = copp.solver.topp2_ra.Problem(
 50        robot.constraints,
 51        idx_s_interval=(0, n - 1),
 52        a_boundary=(0.0, 0.0),
 53    )
 54    a_ra0 = copp.solver.topp2_ra.solve(topp2_problem, copp.solver.topp2_ra.Options())
 55    robot.constraints.amax_substitute(a_ra0, 0)
 56
 57    # 4) Solve TOPP3-SOCP twice, refreshing the a(s) linearization once.
 58    options = copp.solver.topp3_socp.Options(allow_almost_solved=True)
 59
 60    problem1 = copp.solver.topp3_socp.Problem(
 61        robot.constraints,
 62        a_ra0,
 63        idx_s_start=0,
 64        a_boundary=(0.0, 0.0),
 65        b_boundary=(0.0, 0.0),
 66        num_stationary_max=1,
 67    )
 68    profile1 = copp.solver.topp3_socp.solve(problem1, options)
 69    t_final1, t_s1 = copp.interpolation.s_to_t_topp3(s, profile1, 0.0)
 70    s_t1 = copp.interpolation.t_to_s_topp3_uniform(
 71        s,
 72        profile1,
 73        t_s1,
 74        dt,
 75        t0=0.0,
 76        include_final=True,
 77    )
 78
 79    problem2 = copp.solver.topp3_socp.Problem(
 80        robot.constraints,
 81        profile1.a,
 82        idx_s_start=0,
 83        a_boundary=(0.0, 0.0),
 84        b_boundary=(0.0, 0.0),
 85        num_stationary_max=1,
 86    )
 87    profile2 = copp.solver.topp3_socp.solve(problem2, options)
 88    t_final2, t_s2 = copp.interpolation.s_to_t_topp3(s, profile2, 0.0)
 89    s_t2 = copp.interpolation.t_to_s_topp3_uniform(
 90        s,
 91        profile2,
 92        t_s2,
 93        dt,
 94        t0=0.0,
 95        include_final=True,
 96    )
 97
 98    # 5) Print the tutorial summary.
 99    print("TOPP3-SOCP done. (The first iteration)")
100    print(f"dim = {dim}, N = {n}")
101    print(f"t_final = {t_final1:.6f} s")
102    print(f"a_profile.len() = {len(profile1.a)}")
103    print(f"b_profile.len() = {len(profile1.b)}")
104    print(f"s(t) samples = {len(s_t1)}")
105    print("---------")
106    print("TOPP3-SOCP done. (The second iteration)")
107    print(f"t_final = {t_final2:.6f} s <= {t_final1:.6f} s")
108    print(f"a_profile.len() = {len(profile2.a)}")
109    print(f"b_profile.len() = {len(profile2.b)}")
110    print(f"s(t) samples = {len(s_t2)}")
111
112
113if __name__ == "__main__":
114    main()