def test_integral(self): t0 = 1.2 T = 5.7 ocp = Ocp(t0=t0, T=T) x = ocp.state() u = ocp.control() ocp.set_der(x, u) ocp.subject_to(ocp.at_t0(x) == 0) ocp.subject_to(u <= 1) f = ocp.integral(x * ocp.t) ocp.add_objective(-f) # (t-t0)*t -> t^3/3-t^2/2*t0 ocp.solver('ipopt') opts = {"abstol": 1e-8, "reltol": 1e-8, "quad_err_con": True} for method in [ MultipleShooting(intg='rk'), MultipleShooting(intg='cvodes', intg_options=opts), #MultipleShooting(intg='idas',intg_options=opts), DirectCollocation() ]: ocp.method(method) sol = ocp.solve() ts, xs = sol.sample(f, grid='control') I = lambda t: t**3 / 3 - t**2 / 2 * t0 x_ref = I(t0 + T) - I(t0) assert_array_almost_equal(xs[-1], x_ref)
def test_time_dep_ode(self): t0 = 1.2 T = 5.7 ocp = Ocp(t0=t0, T=5.7) x = ocp.state() ocp.set_der(x, ocp.t**2) ocp.subject_to(ocp.at_t0(x) == 0) tf = t0 + T x_ref = tf**3 / 3 - t0**3 / 3 ocp.solver('ipopt') opts = {"abstol": 1e-9, "reltol": 1e-9} for method in [ MultipleShooting(intg='rk'), MultipleShooting(intg='cvodes', intg_options=opts), MultipleShooting(intg='idas', intg_options=opts), DirectCollocation() ]: ocp.method(method) sol = ocp.solve() ts, xs = sol.sample(x, grid='control') x_ref = ts**3 / 3 - t0**3 / 3 assert_array_almost_equal(xs, x_ref)
def test_variables(self): N = 10 ocp = Ocp(t0=2 * pi, T=10) p = ocp.parameter(grid='control') v = ocp.variable(grid='control') x = ocp.state() ocp.set_der(x, 0) ocp.subject_to(ocp.at_t0(x) == 0) ts = linspace(0, 10, N) ocp.add_objective(ocp.integral(sin(v - p)**2, grid='control')) ocp.method(MultipleShooting(N=N)) ocp.solver('ipopt') ocp.set_value(p, ts) ocp.set_initial(v, ts) sol = ocp.solve() _, xs = sol.sample(v, grid='control') assert_array_almost_equal(xs[:-1], ts) ocp.set_initial(v, 0.1 + 2 * pi + ts) sol = ocp.solve() _, xs = sol.sample(v, grid='control') assert_array_almost_equal(xs[:-1], 2 * pi + ts) ocp.set_initial(v, 0.1 + ocp.t) sol = ocp.solve() _, xs = sol.sample(v, grid='control') assert_array_almost_equal(xs[:-1], 2 * pi + ts) ocp.set_initial(v, 0.1 + 2 * pi) sol = ocp.solve() _, xs = sol.sample(v, grid='control') with self.assertRaises(AssertionError): assert_array_almost_equal(xs[:-1], 2 * pi + ts)
def test_der(self): T = 1 M = 1 b = 1 t0 = 0 x0 = 0 ocp = Ocp(t0=t0, T=T) x = ocp.state() u = ocp.control() ocp.set_der(x, u) y = 2 * x ocp.subject_to(ocp.der(y) <= 2 * b) ocp.subject_to(-2 * b <= ocp.der(y)) ocp.add_objective(ocp.at_tf(x)) ocp.subject_to(ocp.at_t0(x) == x0) ocp.solver('ipopt') ocp.method(MultipleShooting(N=4, M=M, intg='rk')) sol = ocp.solve() ts, xs = sol.sample(x, grid='control') self.assertAlmostEqual(xs[0], x0, places=6) self.assertAlmostEqual(xs[-1], x0 - b * T, places=6) self.assertAlmostEqual(ts[0], t0) self.assertAlmostEqual(ts[-1], t0 + T)
def test_param(self): ocp = Ocp(T=1) x = ocp.state() u = ocp.control() p = ocp.parameter() ocp.set_der(x, u) ocp.subject_to(u <= 1) ocp.subject_to(-1 <= u) ocp.add_objective(ocp.at_tf(x)) ocp.subject_to(ocp.at_t0(x) == p) ocp.solver('ipopt') ocp.method(MultipleShooting()) ocp.set_value(p, 0) sol = ocp.solve() ts, xs = sol.sample(x, grid='control') self.assertAlmostEqual(xs[0], 0) ocp.set_value(p, 1) sol = ocp.solve() ts, xs = sol.sample(x, grid='control') self.assertAlmostEqual(xs[0], 1)
def vdp(method,grid='control'): ocp = Ocp(T=10) # Define 2 states x1 = ocp.state() x2 = ocp.state() # Define 1 control u = ocp.control(order=0) # Specify ODE ocp.set_der(x1, (1 - x2**2) * x1 - x2 + u) ocp.set_der(x2, x1) # Lagrange objective ocp.add_objective(ocp.integral(x1**2 + x2**2 + u**2)) # Path constraints ocp.subject_to(-1 <= (u <= 1)) ocp.subject_to(x1 >= -0.25, grid=grid) # Initial constraints ocp.subject_to(ocp.at_t0(x1) == 0) ocp.subject_to(ocp.at_t0(x2) == 1) # Pick an NLP solver backend ocp.solver('ipopt') # Pick a solution method ocp.method(method) return (ocp, x1, x2, u)
def test_basic_t0_free(self): xf = 2 t0 = 0 for T in [2]: for x0 in [0, 1]: for b in [1, 2]: for method in [ MultipleShooting(N=4, intg='rk'), MultipleShooting(N=4, intg='cvodes'), MultipleShooting(N=4, intg='idas'), DirectCollocation(N=4) ]: ocp = Ocp(t0=FreeTime(2), T=T) x = ocp.state() u = ocp.control() ocp.set_der(x, u) ocp.subject_to(u <= b) ocp.subject_to(-b <= u) ocp.add_objective(ocp.tf) ocp.subject_to(ocp.at_t0(x) == x0) ocp.subject_to(ocp.at_tf(x) == xf) ocp.subject_to(ocp.t0 >= 0) ocp.solver('ipopt') ocp.method(method) sol = ocp.solve() ts, xs = sol.sample(x, grid='control') self.assertAlmostEqual(xs[0], x0, places=6) self.assertAlmostEqual(xs[-1], xf, places=6) self.assertAlmostEqual(ts[0], t0) self.assertAlmostEqual(ts[-1], t0 + T)
def bang_bang_problem(stage_method): ocp = Ocp(T=FreeTime(1)) p = ocp.state() v = ocp.state() u = ocp.control() ocp.set_der(p, v) ocp.set_der(v, u) ocp.subject_to(u <= 1) ocp.subject_to(-1 <= u) ocp.add_objective(ocp.T) ocp.subject_to(ocp.at_t0(p) == 0) ocp.subject_to(ocp.at_t0(v) == 0) ocp.subject_to(ocp.at_tf(p) == 1) ocp.subject_to(ocp.at_tf(v) == 0) ocp.solver('ipopt') ocp.method(stage_method) return (ocp, ocp.solve(), p, v, u)
def integrator_control_problem(T=1, u_max=1, x0=0, stage_method=None, t0=0): if stage_method is None: stage_method = MultipleShooting() ocp = Ocp(t0=t0, T=T) x = ocp.state() u = ocp.control() ocp.set_der(x, u) ocp.subject_to(u <= u_max) ocp.subject_to(-u_max <= u) ocp.add_objective(ocp.at_tf(x)) if x0 is not None: ocp.subject_to(ocp.at_t0(x) == x0) ocp.solver('ipopt') ocp.method(stage_method) return (ocp, x, u)
def test_dae_casadi(self): # cross check with dae_colloation xref = 0.1 # chariot reference l = 1. #- -> crane, + -> pendulum m = 1. M = 1. g = 9.81 ocp = Ocp(T=5) x = ocp.state() y = ocp.state() w = ocp.state() dx = ocp.state() dy = ocp.state() dw = ocp.state() xa = ocp.algebraic() u = ocp.control() ocp.set_der(x, dx) ocp.set_der(y, dy) ocp.set_der(w, dw) ddx = (w - x) * xa / m ddy = g - y * xa / m ddw = ((x - w) * xa - u) / M ocp.set_der(dx, ddx) ocp.set_der(dy, ddy) ocp.set_der(dw, ddw) ocp.add_alg((x - w) * (ddx - ddw) + y * ddy + dy * dy + (dx - dw)**2) ocp.add_objective( ocp.at_tf((x - xref) * (x - xref) + (w - xref) * (w - xref) + dx * dx + dy * dy)) ocp.add_objective( ocp.integral((x - xref) * (x - xref) + (w - xref) * (w - xref))) ocp.subject_to(-2 <= (u <= 2)) ocp.subject_to(ocp.at_t0(x) == 0) ocp.subject_to(ocp.at_t0(y) == l) ocp.subject_to(ocp.at_t0(w) == 0) ocp.subject_to(ocp.at_t0(dx) == 0) ocp.subject_to(ocp.at_t0(dy) == 0) ocp.subject_to(ocp.at_t0(dw) == 0) #ocp.subject_to(xa>=0,grid='integrator_roots') ocp.set_initial(y, l) ocp.set_initial(xa, 9.81) # Pick an NLP solver backend # NOTE: default scaling strategy of MUMPS leads to a singular matrix error ocp.solver( 'ipopt', { "ipopt.linear_solver": "mumps", "ipopt.mumps_scaling": 0, "ipopt.tol": 1e-12 }) # Pick a solution method method = DirectCollocation(N=50) ocp.method(method) # Solve sol = ocp.solve() assert_array_almost_equal( sol.sample(xa, grid='integrator', refine=1)[1][0], 9.81011622448889) assert_array_almost_equal( sol.sample(xa, grid='integrator', refine=1)[1][1], 9.865726317147214) assert_array_almost_equal( sol.sample(xa, grid='integrator')[1][0], 9.81011622448889) assert_array_almost_equal( sol.sample(xa, grid='integrator')[1][1], 9.865726317147214) assert_array_almost_equal( sol.sample(xa, grid='control')[1][0], 9.81011622448889) assert_array_almost_equal( sol.sample(xa, grid='control')[1][1], 9.865726317147214)
ocp = Ocp() stage = ocp.stage(t0=FreeTime(0), T=FreeTime(1)) p = stage.state() v = stage.state() stage.set_der(p, v) stage.set_der(v, -9.81) stage.subject_to(stage.at_t0(v) >= 0) stage.subject_to(p >= 0) stage.method(MultipleShooting(N=1, M=20, intg='rk')) stage.subject_to(stage.at_t0(p) == 0) ocp.subject_to(stage.t0 == 0) stage_prev = stage n_bounce = 2 for i in range(n_bounce): ocp.subject_to(stage.at_tf(p) == 0) stage = ocp.stage(stage_prev) ocp.subject_to(stage.at_t0(v) == -0.9 * stage_prev.at_tf(v)) ocp.subject_to(stage.t0 == stage_prev.tf) stage_prev = stage # Final bounce should reach 0.5 exactly ocp.subject_to(stage.at_tf(v) == 0) ocp.subject_to(stage.at_tf(p) == 0.5)
x0 = [0, 0] ocp = Ocp(t0=tgrid[0], T=tgrid[-1]) # Define states x = ocp.state(2) # Define controls u = ocp.control() # Specify ODE model_rhs = pendulum_ode(x, u, model_param) ocp.set_der(x, model_rhs) # Set initial conditions ocp.subject_to(ocp.at_t0(x) == x0) # Define reference y_ref = ocp.parameter(grid='control') ocp.set_value(y_ref, y_ref_val) # Define output correction beta = ocp.parameter(grid='control') # Define previous control u_prev = ocp.parameter(grid='control') # Set ILC objective ocp.add_objective( ocp.integral((y_ref - beta - x[0])**2, grid='control') + 1e-3 * ocp.integral((u - u_prev)**2, grid='control'))
ocp.set_der(x, v * cos(theta)) ocp.set_der(y, v * sin(theta)) ocp.set_der(theta, w) ocp.set_der(s_obs, sdot_obs) #-------------------------------------------------------------------------------# # Solve the first iteration # #-------------------------------------------------------------------------------# #------------------------- Constraints on initial point X_0 = ocp.parameter(4) X = vertcat(x, y, theta, s_obs) ocp.subject_to(ocp.at_t0(X) == X_0) current_X = vertcat(initial_pos_x, initial_pos_y, 0.0, 0.0) ocp.set_value(X_0, current_X) #------------------------- Constraints on Final point end_goal_x = ocp.parameter(1) end_goal_y = ocp.parameter(1) ocp.set_value(end_goal_x, global_end_goal_x) ocp.set_value(end_goal_y, global_end_goal_y) #----------------------------- constraints on controls ocp.subject_to(0 <= (v <= 1))
#-----------------------------ODEs ocp.set_der(x, v * cos(theta)) ocp.set_der(y, v * sin(theta)) ocp.set_der(theta, w) #-------------------------------------------------------------------------------# # Solve the first iteration # #-------------------------------------------------------------------------------# #------------------------- Constraints on initial point X_0 = ocp.parameter(3) X = vertcat(x, y, theta) ocp.subject_to(ocp.at_t0(X) == X_0) current_X = vertcat(initial_pos_x, initial_pos_y, 0.0) ocp.set_value(X_0, current_X) #------------------------- Constraints on Final point global_goal = vertcat(global_end_goal_x, global_end_goal_y) end_goal_x = ocp.parameter(1) end_goal_y = ocp.parameter(1) ocp.set_value(end_goal_x, global_end_goal_x) ocp.set_value(end_goal_y, global_end_goal_y) #----------------------------- constraints on controls
def test_stage_cloning_t0_T(self): for t0_stage, t0_sol_stage in [(None, 0), (-1, -1), (FreeTime(-1), -1)]: for T_stage, T_sol_stage in [(None, 2), (2, 2), (FreeTime(1), 2)]: kwargs = {} if t0_stage is not None: kwargs["t0"] = t0_stage if T_stage is not None: kwargs["T"] = T_stage stage = Stage(**kwargs) p = stage.state() v = stage.state() u = stage.control() stage.set_der(p, v) stage.set_der(v, u) stage.subject_to(u <= 1) stage.subject_to(-1 <= u) stage.add_objective(stage.tf) stage.subject_to(stage.at_t0(p) == 0) stage.subject_to(stage.at_t0(v) == 0) stage.subject_to(stage.at_tf(p) == 1) stage.subject_to(stage.at_tf(v) == 0) stage.method(MultipleShooting(N=2)) for t0, t0_sol in ([] if t0_stage is None else [ (None, t0_sol_stage) ]) + [(-1, -1), (FreeTime(-1), -1)]: for T, T_sol in ([] if T_stage is None else [ (None, T_sol_stage) ]) + [(2, 2), (FreeTime(1), 2)]: ocp = Ocp() kwargs = {} if t0 is not None: kwargs["t0"] = t0 if T is not None: kwargs["T"] = T mystage = ocp.stage(stage, **kwargs) if mystage.is_free_starttime(): ocp.subject_to(mystage.t0 >= t0_sol) ocp.solver('ipopt') sol = ocp.solve() tolerance = 1e-6 ts, ps = sol(mystage).sample(p, grid='integrator', refine=10) ps_ref = np.hstack( ((0.5 * np.linspace(0, 1, 10 + 1)**2)[:-1], np.linspace(0.5, 1.5, 10 + 1) - 0.5 * np.linspace(0, 1, 10 + 1)**2)) np.testing.assert_allclose(ps, ps_ref, atol=tolerance) ts_ref = t0_sol + np.linspace(0, 2, 10 * 2 + 1) ts, vs = sol(mystage).sample(v, grid='integrator', refine=10) np.testing.assert_allclose(ts, ts_ref, atol=tolerance) vs_ref = np.hstack((np.linspace(0, 1, 10 + 1)[:-1], np.linspace(1, 0, 10 + 1))) np.testing.assert_allclose(vs, vs_ref, atol=tolerance) u_ref = np.array([1.0] * 10 + [-1.0] * 11) ts, us = sol(mystage).sample(u, grid='integrator', refine=10) np.testing.assert_allclose(us, u_ref, atol=tolerance)
stage.set_der(p, v) stage.set_der(v, -9.81) stage.subject_to(stage.at_t0(v) >= 0) stage.subject_to(p >= 0) stage.method(MultipleShooting(N=1, M=20, intg='rk')) return stage, p, v ocp = Ocp() # Shoot up the ball stage1, p1, v1 = create_bouncing_ball_stage(ocp) ocp.subject_to(stage1.t0 == 0) # Stage starts at time 0 ocp.subject_to(stage1.at_t0(p1) == 0) ocp.subject_to(stage1.at_tf(p1) == 0) # After bounce 1 stage2, p2, v2 = create_bouncing_ball_stage(ocp) ocp.subject_to(stage2.at_t0(v2) == -0.9 * stage1.at_tf(v1)) ocp.subject_to(stage2.at_t0(p2) == stage1.at_tf(p1)) ocp.subject_to(stage2.t0 == stage1.tf) ocp.subject_to(stage2.at_tf(p2) == 0) # After bounce 2 stage3, p3, v3 = create_bouncing_ball_stage(ocp) ocp.subject_to(stage3.at_t0(v3) == -0.9 * stage2.at_tf(v2)) ocp.subject_to(stage3.at_t0(p3) == stage2.at_tf(p2)) ocp.subject_to(stage3.t0 == stage2.tf)
ocp.set_der(s_obs , sdot_obs) #-------------------------------------------------------------------------------# # Solve the first iteration # #-------------------------------------------------------------------------------# #------------------------- Constraints on initial point X_0 = ocp.parameter(4) X = vertcat(x, y, theta, s_obs) ocp.subject_to(ocp.at_t0(X) == X_0) current_X = vertcat(initial_pos_x,initial_pos_y,0.0,0.0) ocp.set_value(X_0, current_X) #------------------------- Constraints on Final point global_goal = vertcat(global_end_goal_x,global_end_goal_y) end_goal_x = ocp.parameter(1) end_goal_y = ocp.parameter(1) ocp.set_value( end_goal_x, global_end_goal_x)