Two-Phase Robot Tracking#

Mathematical Formulation#

Problem Statement#

Find the optimal control forces \(u_1(t)\) and \(u_2(t)\) that minimize the tracking error across two phases:

\[J = \int_0^1 \left[ w_1(x_1 - x_{1,\text{ref}})^2 + w_2(x_2 - x_{2,\text{ref}})^2 + w_3(x_3 - x_{3,\text{ref}})^2 + w_4(x_4 - x_{4,\text{ref}})^2 \right] dt\]
\[+ \int_1^2 \left[ w_1(x_1 - x_{1,\text{ref}})^2 + w_2(x_2 - x_{2,\text{ref}})^2 + w_3(x_3 - x_{3,\text{ref}})^2 + w_4(x_4 - x_{4,\text{ref}})^2 \right] dt\]

Subject to the double integrator dynamics for both phases:

\[\frac{dx_1}{dt} = x_3\]
\[\frac{dx_2}{dt} = x_4\]
\[\frac{dx_3}{dt} = u_1\]
\[\frac{dx_4}{dt} = u_2\]

Reference Trajectories#

Phase 1 (\(t \in [0,1]\)):

  • \(x_{1,\text{ref}} = \frac{t}{2}\), \(x_{2,\text{ref}} = 0\), \(x_{3,\text{ref}} = 0.5\), \(x_{4,\text{ref}} = 0\)

Phase 2 (\(t \in [1,2]\)):

  • \(x_{1,\text{ref}} = 0.5\), \(x_{2,\text{ref}} = \frac{t-1}{2}\), \(x_{3,\text{ref}} = 0\), \(x_{4,\text{ref}} = 0.5\)

Boundary Conditions#

Phase 1:

  • Initial conditions: \(x_1(0) = 0\), \(x_2(0) = 0\), \(x_3(0) = 0.5\), \(x_4(0) = 0\)

  • Final conditions: Continuous with Phase 2

Phase 2:

  • Initial conditions: Continuous from Phase 1

  • Final conditions: \(x_1(2) = 0.5\), \(x_2(2) = 0.5\), \(x_3(2) = 0\), \(x_4(2) = 0.5\)

  • Control bounds: \(-10 \leq u_1, u_2 \leq 10\) for both phases

  • State bounds: \(-10 \leq x_i \leq 10\) for \(i = 1,2,3,4\)

Physical Parameters#

  • Tracking weights: \(w_1 = 100\), \(w_2 = 100\), \(w_3 = 500\), \(w_4 = 500\)

  • Phase 1 duration: \(t_1 = 1\) s

  • Phase 2 duration: \(t_2 = 1\) s

State Variables#

  • \(x_1(t)\): Position coordinate 1

  • \(x_2(t)\): Position coordinate 2

  • \(x_3(t)\): Velocity coordinate 1

  • \(x_4(t)\): Velocity coordinate 2

Control Variables#

  • \(u_1(t)\): Control force 1

  • \(u_2(t)\): Control force 2

Notes#

This problem demonstrates multiphase optimal control where a robot must track different reference trajectories in each phase with automatic continuity constraints between phases.

Running This Example#

cd examples/two_phase_robot
python two_phase_robot.py

Code Implementation#

examples/two_phase_robot/two_phase_robot.py#
  1import numpy as np
  2
  3import maptor as mtor
  4
  5
  6# Problem setup
  7problem = mtor.Problem("Two Phase Path Tracking Robot")
  8
  9# Phase 1: First tracking phase (0 to 1 second)
 10phase1 = problem.set_phase(1)
 11t1 = phase1.time(initial=0.0, final=1.0)
 12x1_1 = phase1.state("x1", initial=0.0, boundary=(-10.0, 10.0))
 13x2_1 = phase1.state("x2", initial=0.0, boundary=(-10.0, 10.0))
 14x3_1 = phase1.state("x3", initial=0.5, boundary=(-10.0, 10.0))
 15x4_1 = phase1.state("x4", initial=0.0, boundary=(-10.0, 10.0))
 16u1_1 = phase1.control("u1", boundary=(-10.0, 10.0))
 17u2_1 = phase1.control("u2", boundary=(-10.0, 10.0))
 18
 19# Phase 1 dynamics: Simple integrator
 20phase1.dynamics({x1_1: x3_1, x2_1: x4_1, x3_1: u1_1, x4_1: u2_1})
 21
 22# Phase 1 reference tracking objective
 23w1, w2, w3, w4 = 100.0, 100.0, 500.0, 500.0
 24x1ref_1 = t1 / 2.0
 25x2ref_1 = 0.0
 26x3ref_1 = 0.5
 27x4ref_1 = 0.0
 28
 29integrand_1 = (
 30    w1 * (x1_1 - x1ref_1) ** 2
 31    + w2 * (x2_1 - x2ref_1) ** 2
 32    + w3 * (x3_1 - x3ref_1) ** 2
 33    + w4 * (x4_1 - x4ref_1) ** 2
 34)
 35integral_1 = phase1.add_integral(integrand_1)
 36
 37# Phase 2: Second tracking phase (1 to 2 seconds) with automatic continuity
 38phase2 = problem.set_phase(2)
 39t2 = phase2.time(initial=1.0, final=2.0)
 40x1_2 = phase2.state("x1", initial=x1_1.final, final=0.5, boundary=(-10.0, 10.0))
 41x2_2 = phase2.state("x2", initial=x2_1.final, final=0.5, boundary=(-10.0, 10.0))
 42x3_2 = phase2.state("x3", initial=x3_1.final, final=0.0, boundary=(-10.0, 10.0))
 43x4_2 = phase2.state("x4", initial=x4_1.final, final=0.5, boundary=(-10.0, 10.0))
 44u1_2 = phase2.control("u1", boundary=(-10.0, 10.0))
 45u2_2 = phase2.control("u2", boundary=(-10.0, 10.0))
 46
 47# Phase 2 dynamics: Same integrator
 48phase2.dynamics({x1_2: x3_2, x2_2: x4_2, x3_2: u1_2, x4_2: u2_2})
 49
 50# Phase 2 reference tracking objective
 51x1ref_2 = 0.5
 52x2ref_2 = (t2 - 1.0) / 2.0
 53x3ref_2 = 0.0
 54x4ref_2 = 0.5
 55
 56integrand_2 = (
 57    w1 * (x1_2 - x1ref_2) ** 2
 58    + w2 * (x2_2 - x2ref_2) ** 2
 59    + w3 * (x3_2 - x3ref_2) ** 2
 60    + w4 * (x4_2 - x4ref_2) ** 2
 61)
 62integral_2 = phase2.add_integral(integrand_2)
 63
 64# Total objective: Sum of both phase costs
 65problem.minimize(integral_1 + integral_2)
 66
 67# Mesh configuration
 68phase1.mesh([6, 6], [-1.0, 0.95, 1.0])
 69phase2.mesh([6, 6], [-1.0, -0.95, 1.0])
 70
 71
 72# Initial guess following PSOPT pattern
 73def _generate_phase_guess(N_intervals, t_start, t_end, x_start, x_end):
 74    """Generate initial guess for a phase matching C++ PSOPT format."""
 75    states_guess = []
 76    controls_guess = []
 77
 78    for N in N_intervals:
 79        # State points (N+1 points for N degree polynomial)
 80        N_state_points = N + 1
 81        states = np.zeros((4, N_state_points))
 82
 83        # Linear interpolation for states
 84        for i in range(4):
 85            states[i, :] = np.linspace(x_start[i], x_end[i], N_state_points)
 86
 87        states_guess.append(states)
 88
 89        # Control points (N points)
 90        controls = np.zeros((2, N))
 91        controls_guess.append(controls)
 92
 93    return states_guess, controls_guess
 94
 95
 96# Phase 1 initial guess: from initial to midpoint
 97x_initial = [0.0, 0.0, 0.5, 0.0]
 98x_midpoint = [0.25, 0.25, 0.25, 0.25]  # (initial + final) / 2
 99states_p1, controls_p1 = _generate_phase_guess([6, 6], 0.0, 1.0, x_initial, x_midpoint)
100
101
102# Phase 2 initial guess: from midpoint to final
103x_final = [0.5, 0.5, 0.0, 0.5]
104states_p2, controls_p2 = _generate_phase_guess([6, 6], 1.0, 2.0, x_midpoint, x_final)
105
106phase1.guess(
107    states=states_p1,
108    controls=controls_p1,
109    integrals=10.0,
110)
111
112phase2.guess(
113    states=states_p2,
114    controls=controls_p2,
115    integrals=10.0,
116)
117
118# Solve
119solution = mtor.solve_adaptive(
120    problem,
121    error_tolerance=1e-5,
122    max_iterations=25,
123    min_polynomial_degree=4,
124    max_polynomial_degree=8,
125    nlp_options={
126        "ipopt.max_iter": 500,
127        "ipopt.linear_solver": "mumps",
128        "ipopt.constr_viol_tol": 1e-10,
129        "ipopt.print_level": 0,
130        "ipopt.nlp_scaling_method": "gradient-based",
131        "ipopt.mu_strategy": "adaptive",
132        "ipopt.tol": 1e-10,
133    },
134)
135
136# Results
137if solution.status["success"]:
138    total_cost = solution.status["objective"]
139    total_time = solution.status["total_mission_time"]
140
141    print(f"Total tracking cost: {total_cost:.6f}")
142    print(f"Total mission time: {total_time:.1f} seconds")
143
144    # Final state verification
145    print("\nFinal states:")
146    print(f"  x1: {solution[(2, 'x1')][-1]:.6f} (target: 0.5)")
147    print(f"  x2: {solution[(2, 'x2')][-1]:.6f} (target: 0.5)")
148    print(f"  x3: {solution[(2, 'x3')][-1]:.6f} (target: 0.0)")
149    print(f"  x4: {solution[(2, 'x4')][-1]:.6f} (target: 0.5)")
150
151    # Show phase information
152    print("\nPhase Information:")
153    for phase_id, phase_data in solution.phases.items():
154        times = phase_data["times"]
155        variables = phase_data["variables"]
156        print(f"  Phase {phase_id}:")
157        print(f"    Duration: {times['duration']:.3f} seconds")
158        print(f"    States: {variables['state_names']}")
159        print(f"    Controls: {variables['control_names']}")
160
161    solution.plot(show_phase_boundaries=True)
162
163else:
164    print(f"Failed: {solution.status['message']}")