Optimal Transfer With State Trigger

from time import perf_counter

import matplotlib.pyplot as plt
import numpy as np
from _sgm_test_util import LTI_plot

import condor as co


class DblInt(co.ODESystem):
    A = np.array([[0, 1], [0, 0]])
    B = np.array([[0], [1]])

    x = state(shape=A.shape[0])
    mode = state()

    p1 = parameter()
    p2 = parameter()

    u = modal()

    dot[x] = A @ x + B * u


class Accel(DblInt.Mode):
    condition = mode == 0.0
    action[u] = 1.0


class Switch1(DblInt.Event):
    function = x[0] - p1
    update[mode] = 1.0


class Decel(DblInt.Mode):
    condition = mode == 1.0
    action[u] = -1.0


class Switch2(DblInt.Event):
    function = x[0] - p2
    # update[mode] = 2.
    terminate = True


class Transfer(DblInt.TrajectoryAnalysis):
    initial[x] = [-9.0, 0.0]
    xd = [1.0, 2.0]
    Q = np.eye(2)
    cost = trajectory_output(((x - xd).T @ (x - xd)) / 2)
    tf = 20.0

    class Options:
        state_max_step_size = 0.25
        state_atol = 1e-15
        state_rtol = 1e-12
        adjoint_atol = 1e-15
        adjoint_rtol = 1e-12
        # state_solver = co.backend.implementations.TrajectoryAnalysis.Solver.CVODE
        # adjoint_solver = co.backend.implementations.TrajectoryAnalysis.Solver.CVODE


p0 = -4.0, -1.0
sim = Transfer(*p0)
# sim.implementation.callback.jac_callback(sim.implementation.callback.p, [])

class MinimumTime(co.OptimizationProblem):
    p1 = variable()
    p2 = variable()
    sim = Transfer(p1, p2)
    objective = sim.cost

    class Options:
        # exact_hessian = False
        __implementation__ = co.implementations.ScipyCG


MinimumTime.set_initial(p1=p0[0], p2=p0[1])

t_start = perf_counter()
opt = MinimumTime()
t_stop = perf_counter()

print("time to run:", t_stop - t_start)
print(opt.p1, opt.p2)
print(opt._stats)

/opt/hostedtoolcache/Python/3.12.11/x64/lib/python3.12/site-packages/condor/implementations/iterative.py:481: RuntimeWarning: Method CG cannot handle bounds.
  min_out = minimize(
time to run: 0.4649003979999975
[-3.00000028] [1.00000242]
 message: Optimization terminated successfully.
 success: True
  status: 0
     fun: 4.044697431832216e-12
       x: [-3.000e+00  1.000e+00]
     nit: 15
     jac: [-1.494e-06  3.167e-06]
    nfev: 22
    njev: 22

LTI_plot(opt.sim)

• 
• 

plt.show()