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)