def gen_long_model():
model = AcadosModel()
model.name = 'long'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_obstacle = SX.sym('x_obstacle')
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
model.p = vertcat(a_min, a_max, x_obstacle)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def J_qi(T, Q, R, P_f, N_pred, states, controls, rhs, func_type=1):
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
# Eingangsanzahl
n_controls = controls.shape[0]
# nonlinear mapping function f(x,u)
f = Function('f', [states, controls], [rhs])
# Decision variables (controls) u_pwm
U = SX.sym('U', n_controls, N_pred)
# parameters (which include the !!!initial and the !!!reference state of
# the robot!!!reference input)
P = SX.sym('P', n_states + n_states + n_controls)
# number of x is always N+1 -> 0..N
X = SX.sym('X', n_states, (N_pred + 1))
X[:, 0] = P[0:n_states]
obj = 0
# compute predicted states and cost function
for k in range(N_pred):
st = X[:, k]
con = U[:, k]
f_value = f(st, con)
# print(f_value)
st_next = st + (T * f_value)
X[:, k + 1] = st_next
obj = obj + (st - P[n_states:2 * n_states]).T @ Q @ (
st - P[n_states:2 * n_states]) + (
con - P[2 * n_states:2 * n_states + n_controls]).T @ R @ (
con - P[2 * n_states:2 * n_states + n_controls])
# print(obj)
st = X[:, N_pred]
obj = obj + (st - P[n_states:2 * n_states]).T @ P_f @ (
st - P[n_states:2 * n_states])
# create a dense matrix of state constraints. Size of g: n_states * (N_pred+1) +1
g = SX.zeros(n_states * (N_pred + 1) + 1, 1)
# Constrainsvariable spezifizieren
for i in range(N_pred + 1):
for j in range(n_states):
g[n_states * i + j] = X[j, i]
g[n_states *
(N_pred + 1)] = (X[:, N_pred] - P[n_states:2 * n_states]).T @ P_f @ (
X[:, N_pred] - P[n_states:2 * n_states])
OPT_variables = reshape(U, N_pred, 1)
nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g, 'p': P}
opts = {}
opts['print_time'] = False
opts['ipopt'] = {
'max_iter': 100,
'print_level': 0,
'acceptable_tol': 1e-8,
'acceptable_obj_change_tol': 1e-6
}
# print(opts)
return f, nlpsol("solver", "ipopt", nlp_prob, opts)
def make_model_consistent(model):
x = model.x
xdot = model.xdot
u = model.u
z = model.z
p = model.p
if isinstance(x, SX):
is_SX = True
elif isinstance(x, MX):
is_SX = False
else:
raise Exception(
"model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
if is_empty(p):
if is_SX:
model.p = SX.sym('p', 0, 0)
else:
model.p = MX.sym('p', 0, 0)
if is_empty(z):
if is_SX:
model.z = SX.sym('z', 0, 0)
else:
model.z = MX.sym('z', 0, 0)
return model
def create_nlp(self, var, par, obj, con, options):
jit = options['codegen']
if options['verbose'] >= 2:
print 'Building nlp ... ',
if jit['jit']:
print('[jit compilation with flags %s]' %
(','.join(jit['jit_options']['flags']))),
t0 = time.time()
nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con))
solver = NlpSolver('solver', 'ipopt', nlp, options['solver'])
grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g')
hess_lag = solver.hessLag()
var, par = struct_symSX(var), struct_symSX(par)
nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit)
grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit)
jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit)
lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size)
hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g],
hess_lag([var, par, lam_f, lam_g]), jit)
options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g,
'hess_lag': hess_lag})
problem = NlpSolver('solver', 'ipopt', nlp, options['solver'])
t1 = time.time()
if options['verbose'] >= 2:
print 'in %5f s' % (t1-t0)
return problem, (t1-t0)
def construct_upd_l(self):
# create parameters
x_i = struct_symSX(self.q_i_struct)
z_i = struct_symSX(self.q_i_struct)
z_ij = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
x_j = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
t0 = t/T
inp = [x_i, z_i, z_ij, l_i, l_ij, x_j, t, T, rho]
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_ij = self.q_ij_struct(z_ij)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
x_j = self.q_ij_struct(x_j)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# update lambda
l_i_new = self.q_i_struct(l_i.cat + rho*(x_i.cat - z_i.cat))
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(l_i_new, tf, self.q_i)
self._transform_spline(l_ij_new, tf, self.q_ij)
out = [l_i_new, l_ij_new]
# create problem
prob, compile_time = self.father.create_function(
'upd_l', inp, out, self.options)
self.problem_upd_l = prob
def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(
v, omega,
(-l * m * sin(theta) * omega**2 + F + g * m * cos(theta) * sin(theta))
/ (M + m - m * cos(theta)**2),
(-l * m * cos(theta) * sin(theta) * omega**2 + F * cos(theta) +
g * m * sin(theta) + M * g * sin(theta)) /
(l * (M + m - m * cos(theta)**2)))
return (
Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs]),
4, # number of states
1) # number of controls
def test_remainder_overapproximation(before_test_remainder_overapproximation):
""" Do we get the same results for python and casadi? """
q_num, k_fb_num, n_s, n_u, l_mu, l_sigma = before_test_remainder_overapproximation
q_cas = SX.sym("q", (n_s, n_s))
# test with numeric k_fb
u_mu_cas, u_sigma_cas = utils_cas.compute_remainder_overapproximations(
q_cas, k_fb_num, l_mu, l_sigma)
f = Function("f", [q_cas], [u_mu_cas, u_sigma_cas])
f_out_cas = f(q_num)
f_out_py = utils.compute_remainder_overapproximations(
q_num, k_fb_num, l_mu, l_sigma)
assert np.allclose(
f_out_cas[0], f_out_py[0]), "are the overapproximations of mu the same"
assert np.allclose(
f_out_cas[1],
f_out_py[1]), "are the overapproximations of sigma the same"
# test with symbolic k_fb
k_fb_cas = SX.sym("k_fb", (n_u, n_s))
u_mu_cas_sym_kfb, u_sigma_cas_sym_kfb = utils_cas.compute_remainder_overapproximations(
q_cas, k_fb_cas, l_mu, l_sigma)
f_sym_k = Function("f", [q_cas, k_fb_cas],
[u_mu_cas_sym_kfb, u_sigma_cas_sym_kfb])
f_out_cas_sym_k = f_sym_k(q_num, k_fb_num)
assert np.allclose(
f_out_cas_sym_k[0],
f_out_py[0]), "are the overapproximations of mu the same"
assert np.allclose(
f_out_cas_sym_k[1],
f_out_py[1]), "are the overapproximations of sigma the same"
def test_blockdiag(self):
# Test blockdiag with DM
correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1],
[0, 0, 1, 1, 1], [0, 0, 1, 1, 1]])
a = DM.ones(2, 2)
b = DM.ones(3, 3)
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res))
# MX and DM mix
a = MX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = MX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and DM mix
a = SX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = SX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and MX
a = SX.sym('a', 2, 2)
b = MX.sym('b', 2, 2)
self.assertRaises(ValueError, blockdiag, a, b)
def chen_model():
""" The following ODE model comes from Chen1998. """
nx, nu = (2, 1)
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1]))
return Function('chen', [x, u], [rhs], ['x', 'u'], ['xdot']), nx, nu
def chen_model():
""" The following ODE model comes from Chen1998. """
nx, nu = (2, 1)
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1]))
return Function('chen', [x, u], [rhs]), nx, nu
def test_is_dae_false(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
ode = -2 * x
sys = DAESystem(x=x, y=y, ode=ode, t=t)
self.assertFalse(sys.is_dae)
def example_model():
""" The following ODE model comes from Chen1998. """
nx = 2
nu = 1
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1]))
return nx, nu, Function('ode_fun', [x, u], [rhs])
def export_chain_mass_model(n_mass, m, D, L):
model_name = 'chain_mass_' + str(n_mass)
x0 = np.array([0, 0, 0]) # fix mass (at wall)
M = n_mass - 2 # number of intermediate masses
nx = (2 * M + 1) * 3 # differential states
nu = 3 # control inputs
xpos = SX.sym('xpos', (M + 1) * 3, 1) # position of fix mass eliminated
xvel = SX.sym('xvel', M * 3, 1)
u = SX.sym('u', nu, 1)
xdot = SX.sym('xdot', nx, 1)
f = SX.zeros(3 * M, 1) # force on intermediate masses
for i in range(M):
f[3 * i + 2] = -9.81
for i in range(M + 1):
if i == 0:
dist = xpos[i * 3:(i + 1) * 3] - x0
else:
dist = xpos[i * 3:(i + 1) * 3] - xpos[(i - 1) * 3:i * 3]
scale = D / m * (1 - L / norm_2(dist))
F = scale * dist
# mass on the right
if i < M:
f[i * 3:(i + 1) * 3] -= F
# mass on the left
if i > 0:
f[(i - 1) * 3:i * 3] += F
# parameters
p = []
x = vertcat(xpos, xvel)
# dynamics
f_expl = vertcat(xvel, u, f)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.name = model_name
return model
def test_convert_expr_from_tau_to_time(self):
t = SX.sym('t')
tau = SX.sym('tau')
expr = tau
t_0 = 5
t_f = 15
correct_res = (t - t_0) / (t_f - t_0)
res = convert_expr_from_tau_to_time(expr, t, tau, t_0, t_f)
self.assertTrue(is_equal(res, correct_res, 10))
def test_type_ode(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
ode = -2 * x
sys = DAESystem(x=x, y=y, ode=ode, t=t)
self.assertEqual(sys.type, 'ode')
def test_is_ode_true(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
p = SX.sym('p', 3)
t = SX.sym('t')
ode = -2 * x
sys = DAESystem(x=x, y=y, p=p, ode=ode, t=t)
self.assertTrue(sys.is_ode)
def gen_lead_model():
model = AcadosModel()
model.name = 'lead'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_lead = SX.sym('x_lead')
v_lead = SX.sym('v_lead')
model.p = vertcat(x_lead, v_lead)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def test_type_dae(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
ode = -2 * x
alg = y - x[0] + x[1]**2
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, t=t)
self.assertEqual(sys.type, 'dae')
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
_, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mtimes(Tinv, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
_, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def test_is_ode_false(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
ode = -2 * x
alg = y - x[0] + x[1]**2
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, t=t)
self.assertFalse(sys.is_ode)
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
_, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mtimes(Tinv, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
_, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
T, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mul(Tinv, cfs_sym)
fun = SXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
return fun([cfs, t_shift])[0]
else:
T, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def test_dae_system_dict_ode_wo_p(self):
x = SX.sym('x', 2)
t = SX.sym('t')
ode = -2 * x
sys = DAESystem(x=x, ode=ode, t=t)
res = {'x': x, 'ode': ode, 't': t}
self.assertEqual(set(res.keys()), set(sys.dae_system_dict.keys()))
for key in res:
self.assertTrue(is_equal(res[key], sys.dae_system_dict[key]))
def export_pendulum_ode_model():
model_name = 'pendulum_ode'
# constants
M = 1. # mass of the cart [kg] -> now estimated
m = 0.1 # mass of the ball [kg]
g = 9.81 # length of the rod [m]
l = 0.8 # gravity constant [m/s^2]
# set up states & controls
x1 = SX.sym('x1')
theta = SX.sym('theta')
v1 = SX.sym('v1')
dtheta = SX.sym('dtheta')
x = vertcat(x1, theta, v1, dtheta)
# controls
F = SX.sym('F')
u = vertcat(F)
# xdot
x1_dot = SX.sym('x1_dot')
theta_dot = SX.sym('theta_dot')
v1_dot = SX.sym('v1_dot')
dtheta_dot = SX.sym('dtheta_dot')
xdot = vertcat(x1_dot, theta_dot, v1_dot, dtheta_dot)
# algebraic variables
# z = None
# parameters
p = []
# dynamics
denominator = M + m - m * cos(theta) * cos(theta)
f_expl = vertcat(
v1, dtheta, (-m * l * sin(theta) * dtheta * dtheta +
m * g * cos(theta) * sin(theta) + F) / denominator,
(-m * l * cos(theta) * sin(theta) * dtheta * dtheta + F * cos(theta) +
(M + m) * g * sin(theta)) / (l * denominator))
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def test__create_integrator_explicit(self):
# Make system 1
x = SX.sym('x')
t = SX.sym('t')
tau = SX.sym('tau')
ode = SX(1)
sys = DAESystem(x=x, ode=ode, tau=tau, t=t)
# Test
sys._create_integrator(integrator_type='explicit')
def test_has_parameters_true(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
p = SX.sym('p')
ode = -2 * x
alg = y - x[0] + x[1]**2 + p
sys = DAESystem(x=x, y=y, p=p, ode=ode, alg=alg, t=t)
self.assertTrue(sys.has_parameters)
def chen_model(implicit=False):
""" The following ODE model comes from Chen1998. """
nx, nu = (2, 1)
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u * (mu + (1. - mu) * x[0]),
x[0] + u * (mu - 4. * (1. - mu) * x[1]))
if implicit:
xdot = SX.sym('x', nx)
return Function('chen', [x, u, xdot], [xdot - rhs]), nx, nu
return Function('chen', [x, u], [rhs]), nx, nu
def test__create_integrator_alg_implicit(self):
x = SX.sym('x')
y = SX.sym('y')
t = SX.sym('t')
tau = SX.sym('tau')
ode = SX(1)
alg = y - (5 - t)
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, tau=tau, t=t)
# Test
sys._create_integrator(integrator_type='implicit')
def test_matrix_norm_2_generalized(before_test_matrix_norm_2_generalized):
""" Do we get the same results as numpy """
a, inv_b, n_s = before_test_matrix_norm_2_generalized
a_cas = SX.sym("a", (n_s, n_s))
inv_b_cas = SX.sym("inv_b", (n_s, n_s))
f = Function("f", [a_cas, inv_b_cas],
[utils_cas.matrix_norm_2_generalized(a_cas, inv_b_cas)])
f_out = f(a, inv_b)
assert np.allclose(f_out, np.max(np.linalg.eig(np.dot(inv_b, a))[0])), \
""" Do we get the largest eigenvalue of the generalized eigenvalue problem
def test_dae_system_dict_dae_wo_p(self):
x = SX.sym('x', 2)
y = SX.sym('y', 1)
t = SX.sym('t')
ode = -2 * x
alg = y - x[0] + x[1]**2
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, t=t)
res = {'x': x, 'z': y, 'ode': ode, 'alg': alg, 't': t}
self.assertEqual(set(res.keys()), set(sys.dae_system_dict.keys()))
for key in res:
self.assertTrue(is_equal(res[key], sys.dae_system_dict[key]))
def test_convert_from_tau_to_time_missing_tau(self):
# Make system
x = SX.sym('x', 2)
y = SX.sym('y', 1)
p = SX.sym('p')
tau = SX.sym('tau')
ode = -2 * x
alg = y - x[0] + x[1]**2
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, tau=tau, p=p)
# Test
self.assertRaises(AttributeError, sys.convert_from_tau_to_time, 0, 5)
def test__create_integrator_w_name(self):
# Make system 1
x = SX.sym('x')
y = SX.sym('y')
t = SX.sym('t')
tau = SX.sym('tau')
ode = SX(1)
alg = y - (5 - t)
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, tau=tau, t=t)
# Test
sys._create_integrator(options={"name": "integrator"})
def test__create_integrator_collocation(self):
# Make system 1
x = SX.sym('x')
y = SX.sym('y')
t = SX.sym('t')
tau = SX.sym('tau')
ode = SX(1)
alg = y - (5 - t)
sys = DAESystem(x=x, y=y, ode=ode, alg=alg, tau=tau, t=t)
# Test
sys._create_integrator(integrator_type='collocation')
def test__create_integrator_invalid_type(self):
# Make system 1
x = SX.sym('x')
t = SX.sym('t')
tau = SX.sym('tau')
ode = SX(1)
sys = DAESystem(x=x, ode=ode, tau=tau, t=t)
# Test
self.assertRaises(ValueError,
sys._create_integrator,
integrator_type='foo')
def _generate_cost(self):
idx_angle = 2
l = 0.5
self.env_options["l"] = l
x_target = 2.5
z_target = np.array(
[x_target, 0.0, 0.0, 0.0,
l])[:, None] # cart-pos,cart-vel,angle-vel,x_angle,y_angle
W_target = np.diag([20.0, 0.0, 0.0, 0.0, 5.0]) / 100.
trigon_augm = lambda m, v: trig_aug(m, v, idx_angle)
## Saturating cost functions (PILCO)
m_cas = SX.sym("mean", (5, 1))
v_cas = SX.sym("var", (5, 5))
loss_sat_func = loss_sat(m_cas, v_cas, z_target, W_target)
cost_stage = lambda m, v, u: loss_sat_func(m, v)
terminal_cost = lambda m, v: loss_sat_func(m, v)
## Quadratic cost functions (standard)
#cost_stage = lambda m,v,u: loss_quadratic(m,z_target,v,W_target) + 10*mtimes(u.T, u)
#terminal_cost = lambda m,v: loss_quadratic(m,z_target,v,W_target)
# cost = lambda p_0,u_0,p_all,q_all,mu_perf,sigma_perf,k_ff_all,k_fb_ctrl,k_fb_perf,k_ff_perf: \
# generic_cost(mu_perf,sigma_perf,k_ff_perf,cost_stage,terminal_cost,state_trafo = trigon_augm)
w_rl_cost = 100.0
self.rl_immediate_cost = lambda state: w_rl_cost * (state[0] - x_target
)**2
if self.n_perf > 0:
cost = lambda p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, \
sigma_safe, mu_perf, sigma_perf, \
gp_pred_sigma_perf, k_fb_perf, k_ff_perf: \
generic_cost(mu_perf, sigma_perf, k_ff_perf, cost_stage, terminal_cost, state_trafo=trigon_augm) # + \
# cost_dev_safe_perf(p_all,mu_perf)
else:
cost_stage = lambda m, v, u: loss_sat_func(m, v)
terminal_cost = lambda m, v: loss_sat_func(m, v)
cost = lambda p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe: \
generic_cost(p_all, q_all, k_ff_safe, cost_stage, terminal_cost, state_trafo=trigon_augm)
return cost
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A([par])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
import matplotlib.pyplot as plt
from numpy import array, diag, eye, zeros
from scipy.linalg import block_diag
from acados import ocp_nlp
from casadi import SX, Function, vertcat
nx, nu = 2, 1
x = SX.sym('x', nx)
u = SX.sym('u', nu)
ode_fun = Function('ode_fun', [x, u], [vertcat(x[1], u)], ['x', 'u'], ['rhs'])
N = 15
nlp = ocp_nlp(N, nx, nu)
nlp.set_dynamics(ode_fun, {'integrator': 'rk4', 'step': 0.1})
q, r = 1, 1
P = eye(nx)
nlp.set_stage_cost(eye(nx+nu), zeros(nx+nu), diag([q, q, r]))
nlp.set_terminal_cost(eye(nx), zeros(nx), P)
x0 = array([1, 1])
nlp.set_field("lbx", 0, x0)
nlp.set_field("ubx", 0, x0)
nlp.initialize_solver("sqp", {"qp_solver": "qpoases"})
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model()
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N*[1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N*[f]+[f_N])
ls_cost.ls_cost_matrix = N*[block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
nlp.set_path_constraints(N*[g] + [g_N])
for i in range(N):
nlp.lg[i] = -0.5
nlp.ug[i] = +0.5
def deform(
vertices_val,
adjacency,
target_to_base_indices,
targets_val,
):
n_vertices = vertices_val.shape[1]
n_targets = targets_val.shape[1]
assert vertices_val.shape[0] == 3
assert targets_val.shape[0] == 3
assert len(adjacency) == n_vertices
assert len(target_to_base_indices) == n_targets
start = time.time()
old_vertices = SX.sym("old_vertices", 3, n_vertices)
targets = SX.sym("targets", 3, n_targets)
vertices = SX.sym("vertices", 3, n_vertices)
rotations = SX.sym("rotations", 3, 3 * n_vertices)
vertices_to_fit = vertices[:, target_to_base_indices]
start_data = time.time()
data = (vertices_to_fit - targets).reshape((-1, 1))
print(f"data elapsed {time.time() - start_data}")
start_arap = time.time()
arap_residual = arap.make_rot_arap_residuals(adjacency, old_vertices, rotations, vertices)
print(f"arap elapsed {time.time() - start_arap}")
start_rigid = time.time()
rigid = arap.make_rigid_residuals(rotations)
print(f"rigid elapsed {time.time() - start_rigid}")
residuals = casadi.vertcat(
data,
arap_residual,
10.0 * rigid
)
variables = casadi.vertcat(
rotations.reshape((-1, 1)),
vertices.reshape((-1, 1))
)
start_jac = time.time()
jac = casadi.jacobian(residuals, variables)
print(f"jac elapsed {time.time() - start_jac}")
print("jac.shape", jac.shape, "jac.nnz()", jac.nnz())
fixed_values = [
old_vertices,
targets,
]
start_res = time.time()
residual_func = Function("res_func", [variables] + fixed_values, [residuals])
print(f"res elapsed {time.time() - start_res}")
start_jac_func = time.time()
jac_func = Function("jac_func", [variables] + fixed_values, [jac])
print(f"jac_func elapsed {time.time() - start_jac_func}")
print(f"construction elapsed {time.time() - start}")
exit(0)
def compute_residuals_and_jac(x):
start = time.time()
residuals_val = residual_func(x, vertices_val, targets_val).toarray()
print(f"Residual elapsed {time.time() - start}")
start = time.time()
jacobian_val = jac_func(x, vertices_val, targets_val)
print(f"Jacobian elapsed {time.time() - start}")
jacobian_val = jacobian_val.sparse()
return residuals_val, jacobian_val
init_rot = np.hstack([np.eye(3)] * n_vertices)
init_vertices = vertices_val
init_vars = np.hstack((
init_rot.T.reshape(-1),
init_vertices.reshape(-1)
))
res = perform_gauss_newton(init_vars, compute_residuals_and_jac, 50, dumping_factor=0.5)
new_vertices = res[9 * n_vertices:].reshape(-1, 3).T
return new_vertices
