def run_ipopt_ssmevaluator(ssm, n_s, n_u, linearize_mean):
casadi_ssm = CasadiSSMEvaluator(ssm, linearize_mean)
x = cas.MX.sym("x", (n_s, 1))
y = cas.MX.sym("y", (n_u, 1))
if linearize_mean:
mu, sigma, mu_jac = casadi_ssm(x, y)
f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) + cas.sum1(
cas.sum2(mu_jac))
else:
mu, sigma = casadi_ssm(x, y)
f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma))
x = cas.vertcat(x, y)
options = {
"ipopt": {
"hessian_approximation": "limited-memory",
"max_iter": 2,
"derivative_test": "first-order"
}
}
solver = cas.nlpsol("solver", "ipopt", {"x": x, "f": f}, options)
with capture_stdout() as out:
res = solver(x0=np.random.randn(5, 1))
return str(type(ssm)), linearize_mean, out[0]
def assertOutput(self,included,excluded):
with capture_stdout() as result:
yield
if not(isinstance(included,list)):
included = [included]
if not(isinstance(excluded,list)):
excluded = [excluded]
for e in included:
self.assertTrue(e in result[0],msg=result[0] + "<->" + e)
for e in excluded:
self.assertFalse(e in result[0],msg=result[0] + "<->" + e)
def test_output(self):
with capture_stdout() as result:
DM([1, 2]).print_dense()
assert "2" in result[0]
x = SX.sym("x")
f = {'x': x, 'f': x**2}
solver = nlpsol("solver", "ipopt", f)
with capture_stdout() as result:
solver_out = solver(x0=0)
assert "Number of nonzeros in equality constraint" in result[0]
assert "iter objective inf_pr" in result[0]
with capture_stdout() as result:
try:
solver = nlpsol("solver", "foo", f)
except:
pass
assert "casadi_nlpsol_foo" in result[1]
def test_output(self):
with capture_stdout() as result:
DMatrix([1, 2]).printDense()
assert "2" in result[0]
x = SX.sym("x")
f = SXFunction('f', nlpIn(x=x), nlpOut(f=x**2))
solver = NlpSolver("solver", "ipopt", f)
with capture_stdout() as result:
solver.evaluate()
assert "Number of nonzeros in equality constraint" in result[0]
assert "iter objective inf_pr" in result[0]
assert "main loop" in result[0]
with capture_stdout() as result:
try:
solver = NlpSolver("solver", "foo", f)
except:
pass
assert "casadi_nlpsolver_foo" in result[1]
def test_output(self):
with capture_stdout() as result:
DM([1,2]).print_dense()
assert "2" in result[0]
x=SX.sym("x")
f = {'x':x, 'f':x**2}
solver = nlpsol("solver", "ipopt",f)
with capture_stdout() as result:
solver_out = solver(solver_in)
assert "Number of nonzeros in equality constraint" in result[0]
assert "iter objective inf_pr" in result[0]
assert "main loop" in result[0]
with capture_stdout() as result:
try:
solver = nlpsol("solver","foo",f)
except:
pass
assert "casadi_nlpsol_foo" in result[1]
def test_output(self):
with capture_stdout() as result:
DMatrix([1, 2]).printDense()
assert "2" in result[0]
x = SX.sym("x")
f = SXFunction("f", nlpIn(x=x), nlpOut(f=x ** 2))
solver = NlpSolver("solver", "ipopt", f)
with capture_stdout() as result:
solver.evaluate()
assert "Number of nonzeros in equality constraint" in result[0]
assert "iter objective inf_pr" in result[0]
assert "main loop" in result[0]
with capture_stdout() as result:
try:
solver = NlpSolver("solver", "foo", f)
except:
pass
assert "casadi_nlpsolver_foo" in result[1]
def test_ipopt_ssmevaluator_multistep_ahead(before_test_casadissm):
p_0, q_0, ssm, k_fb, k_ff, L_mu, L_sigm, c_safety, a, b = before_test_casadissm
T = 3
n_u, n_s = np.shape(k_fb)
u_0 = .2 * np.random.randn(n_u, 1)
k_fb_0 = np.random.randn(
T - 1,
n_s * n_u) # np.zeros((T-1,n_s*n_u))# np.random.randn(T-1,n_s*n_u)
k_ff = np.random.randn(T - 1, n_u)
# k_fb_ctrl = np.zeros((n_u,n_s))#np.random.randn(n_u,n_s)
u_0_cas = MX.sym("u_0", (n_u, 1))
k_fb_cas_0 = MX.sym("k_fb", (T - 1, n_u * n_s))
k_ff_cas = MX.sym("k_ff", (T - 1, n_u))
ssm_forward = ssm.get_forward_model_casadi(True)
p_new_cas, q_new_cas, pred_sigm_all = reach_cas.multi_step_reachability(
p_0, u_0, k_fb_cas_0, k_ff_cas, ssm_forward, L_mu, L_sigm, c_safety, a,
b)
h_mat_safe = np.hstack((np.eye(n_s, 1), -np.eye(n_s, 1))).T
h_safe = np.array([300, 300]).reshape((2, 1))
h_mat_obs = np.copy(h_mat_safe)
h_obs = np.array([300, 300]).reshape((2, 1))
g = []
lbg = []
ubg = []
for i in range(T):
p_i = p_new_cas[i, :].T
q_i = q_new_cas[i, :].reshape((n_s, n_s))
g_state = lin_ellipsoid_safety_distance(p_i,
q_i,
h_mat_obs,
h_obs,
c_safety=2.0)
g = vertcat(g, g_state)
lbg += [-cas.inf] * 2
ubg += [0] * 2
x_safe = vertcat(u_0_cas, k_ff_cas.reshape((-1, 1)))
params_safe = vertcat(k_fb_cas_0.reshape((-1, 1)))
f_safe = sum1(sum2(p_new_cas)) + sum1(sum2(q_new_cas)) + sum1(
sum2(pred_sigm_all))
k_ff_cas_all = MX.sym("k_ff_single", (T, n_u))
k_fb_cas_all = MX.sym("k_fb_all", (T - 1, n_s * n_u))
k_fb_cas_all_inp = [
k_fb_cas_all[i, :].reshape((n_u, n_s)) for i in range(T - 1)
]
ssm_forward1 = ssm.get_forward_model_casadi(True)
mu_multistep, sigma_multistep, sigma_pred_perf = prop_casadi.multi_step_taylor_symbolic(
p_0, ssm_forward1, k_ff_cas_all, k_fb_cas_all_inp, a=a, b=b)
x_perf = vertcat(k_ff_cas_all.reshape((-1, 1)))
params_perf = vertcat(k_fb_cas_all.reshape((-1, 1)))
f_perf = sum1(sum2(mu_multistep)) + sum1(sum2(sigma_multistep)) + sum1(
sum2(sigma_pred_perf))
f_both = f_perf + f_safe
x_both = vertcat(x_safe, x_perf)
params_both = vertcat(params_safe, params_perf)
options = {
"ipopt": {
"hessian_approximation": "limited-memory",
"max_iter": 1
},
'error_on_fail': False
}
#raise NotImplementedError("""Need to parse output to get fail/pass signal!
# Either 'Maximun Number of Iterations..' or 'Optimal solution found' are result of
# a successful run""")
#safe only
n_x = np.shape(x_safe)[0]
n_p = np.shape(params_safe)[0]
solver = cas.nlpsol("solver", "ipopt", {
"x": x_safe,
"f": f_safe,
"p": params_safe,
"g": g
}, options)
with capture_stdout() as out:
solver(x0=np.random.randn(n_x, 1),
p=np.random.randn(n_p, 1),
lbg=lbg,
ubg=ubg)
opt_sol_found = solver.stats()
if not opt_sol_found:
max_numb_exceeded = parse_solver_output_pass(out)
if not max_numb_exceeded:
pytest.fail(
"Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong"
)
n_x = np.shape(x_perf)[0]
n_p = np.shape(params_perf)[0]
solver = cas.nlpsol("solver", "ipopt", {
"x": x_perf,
"f": f_perf,
"p": params_perf
}, options)
with capture_stdout() as out:
solver(x0=np.random.randn(n_x, 1), p=np.random.randn(n_p, 1))
opt_sol_found = solver.stats()
if not opt_sol_found:
max_numb_exceeded = parse_solver_output_pass(out)
if not max_numb_exceeded:
pytest.fail(
"Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong"
)
#both
n_x = np.shape(x_both)[0]
n_p = np.shape(params_both)[0]
solver = cas.nlpsol("solver", "ipopt", {
"x": x_both,
"f": f_both,
"p": params_both
}, options)
with capture_stdout() as out:
solver(x0=np.random.randn(n_x, 1), p=np.random.randn(n_p, 1))
opt_sol_found = solver.stats()
if not opt_sol_found:
max_numb_exceeded = parse_solver_output_pass(out)
if not max_numb_exceeded:
pytest.fail(
"Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong"
)
