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_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)
#----------------------------- constraints on controls ocp.subject_to(0 <= (v <= 1)) ocp.subject_to(-1 <= (w <= 1)) ocp.subject_to(sdot_obs >= 0) # ------------------------ Initial guess ------------------------ # Initial guess of ACADOS and IPOPT are made the same # path guess = global path path_guess_x = np.linspace(0, global_end_goal_x, N + 1) path_guess_y = np.linspace(0, global_end_goal_y, N + 1) ocp.set_initial(x, path_guess_x) ocp.set_initial(y, path_guess_y) # path parameters s_guess = np.linspace(0, 1, N + 1) sdot_guess = (s_guess[1] - s_guess[0]) / dt sdot_guess = sdot_guess * np.ones(N) #controls guess are N ocp.set_initial(sdot_obs, s_guess) ocp.set_initial(sdot_obs, sdot_guess) #controls v,w guess + theta v_guess = np.zeros(N) #controls are N not N+1 w_guess = np.zeros(N)
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 )) ocp.subject_to( -pi <= ( w <= pi )) ocp.subject_to( sdot_obs >= 0) # ------------------------ Initial guess ------------------------ ocp.set_initial(x, np.zeros(N+1)) ocp.set_initial(y, np.zeros(N+1)) ocp.set_initial(theta, np.zeros(N+1)) ocp.set_initial(sdot_obs, np.zeros(N+1)) ocp.set_initial(sdot_obs, np.zeros(N+1)) ocp.set_initial(v , np.zeros(N+1)) ocp.set_initial(w , np.zeros(N+1)) #--------------- Bubbles with s bubbles_x = ocp.parameter(N+1) bubbles_y = ocp.parameter(N+1) bubbles_radii = ocp.parameter(N+1)