TOPP2 Reach Sets

Reach-set construction exposes the feasible range of the second-order state a along the path. For station k, the result stores a lower and upper bound:

\[a_{\min,k} \le a_k \le a_{\max,k}.\]

The backward-only reach set enforces the terminal boundary. The bidirectional reach set enforces both the start and terminal boundaries, so it is the better debugging view when a TOPP2 problem appears infeasible.

Runnable Example

 1"""Compute second-order reachable sets for a JAX-defined path.
 2
 3The example applies velocity and acceleration limits in ``[-1, 1]``, then
 4compares backward-only and bidirectional bounds on ``a(s) = dot(s)^2``.
 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
26    # 1) Define q(s). Path.from_jax differentiates it up to third order.
27    def q_fn(s):
28        freq = jnp.array([2.0 * jnp.pi, 3.0 * jnp.pi, 5.0 * jnp.pi], dtype=jnp.float64)
29        phase = jnp.array([0.0, 0.3, 0.7], dtype=jnp.float64)
30        return jnp.sin(freq * s + phase)
31
32    path = copp.Path.from_jax(q_fn, 0.0, 1.0)
33    s = np.linspace(0.0, 1.0, n, dtype=np.float64)
34
35    # 2) Build robot constraints (3-axis), then apply symmetric limits
36    # velocity/acceleration = [-1, 1].
37    robot = copp.Robot(dim, capacity=n)
38    robot.append_s(s)
39    robot.set_q_from_path_2nd(path, 0, n)
40
41    upper = np.ones(dim, dtype=np.float64)
42    lower = -upper
43    robot.add_velocity_limits(upper, lower, start_idx_s=0, length=n)
44    robot.add_acceleration_limits(upper, lower, start_idx_s=0, length=n)
45
46    # 3) Build TOPP2 problem and compute reachable sets.
47    problem = copp.solver.reach_set2.Problem(
48        robot.constraints,
49        idx_s_interval=(0, n - 1),
50        a_boundary=(0.0, 0.0),
51    )
52    options = copp.solver.reach_set2.Options()
53
54    # Backward-only reachability constrains the terminal boundary.
55    reach_back = copp.solver.reach_set2.backward(problem, options)
56    # Bidirectional reachability constrains both start and terminal boundaries.
57    reach_bidir = copp.solver.reach_set2.bidirectional(problem, options)
58
59    # 4) Print the tutorial summary.
60    print("reach_set2 done.")
61    print(f"dim = {dim}, N = {n}")
62    print(
63        "backward-only: "
64        f"a_max.len() = {len(reach_back.a_max)}, "
65        f"a_min.len() = {len(reach_back.a_min)}"
66    )
67    print(
68        "bidirectional: "
69        f"a_max.len() = {len(reach_bidir.a_max)}, "
70        f"a_min.len() = {len(reach_bidir.a_min)}"
71    )
72
73    k0 = 0
74    km = n // 2
75    k1 = n - 1
76    print(
77        "bidirectional bounds @k=0/mid/end: "
78        f"[{reach_bidir.a_min[k0]:.6f}, {reach_bidir.a_max[k0]:.6f}], "
79        f"[{reach_bidir.a_min[km]:.6f}, {reach_bidir.a_max[km]:.6f}], "
80        f"[{reach_bidir.a_min[k1]:.6f}, {reach_bidir.a_max[k1]:.6f}]"
81    )
82
83
84if __name__ == "__main__":
85    main()