def loss_quadratic(m, z, v=None, W=None):
""" Quadratic cost function
Simple quadratic loss with W as weight matrix, ignoring variance
Parameters
----------
m : dx1 ndarray[float | casadi.Sym]
The mean of the input Gaussian
v : dxd ndarray[float | casadi.Sym]
z: dx1 ndarray[float | casadi.Sym]
The target-state [optional]
W: dxd ndarray[float | casadi.Sym]
The weighting matrix factor for the cost-function (scaling)
Returns
-------
L: float
The quadratic loss
"""
D = np.shape(m)[0]
if W is None:
W = SX.eye(D)
l_var = 0
if not v is None:
l_var = trace(mtimes(W, v))
return mtimes((m - z).T, mtimes(W, m - z)) + l_var
def matrix_norm_2_generalized(a, b_inv, x=None, n_iter=None):
""" Get largest generalized eigenvalue of the pair inv_a^{-1},b
get the largest eigenvalue of the generalized eigenvalue problem
a x = \lambda b x
<=> b x = (1/\lambda) a x
Let \omega := 1/lambda
We solve the problem
b x = \omega a x
using the inverse power iteration which converges to
the smallest generalized eigenvalue \omega_\min
Hence we can get \lambda_\max = 1/\omega_\min,
the largest eigenvalue of a x = \lambda b x
"""
n, _ = a.shape
if x is None:
x = np.eye(n, 1)
x /= norm_2(x)
if n_iter is None:
n_iter = 2 * n**2
y = mtimes(b_inv, mtimes(a, x))
for i in range(n_iter):
x = y / norm_2(y)
y = mtimes(b_inv, mtimes(a, x))
return mtimes(y.T, x)
def compute_remainder_overapproximations(q, k_fb, l_mu, l_sigma):
""" Compute symbolically the (hyper-)rectangle over-approximating the lagrangians of mu and sigma
Parameters
----------
q: n_s x n_s ndarray[casadi.SX.sym]
The shape matrix of the current state ellipsoid
k_fb: n_u x n_s ndarray[casadi.SX.sym]
The linear feedback term
l_mu: n x 0 numpy 1darray[float]
The lipschitz constants for the gradients of the predictive mean
l_sigma n x 0 numpy 1darray[float]
The lipschitz constans on the predictive variance
Returns
-------
u_mu: n_s x 0 numpy 1darray[casadi.SX.sym]
The upper bound of the over-approximation of the mean lagrangian remainder
u_sigma: n_s x 0 numpy 1darray[casadi.SX.sym]
The upper bound of the over-approximation of the variance lagrangian remainder
"""
n_u, n_s = np.shape(k_fb)
s = horzcat(np.eye(n_s), k_fb.T)
b = mtimes(s, s.T)
qb = mtimes(q, b)
evals = matrix_norm_2_generalized(b, q)
r_sqr = vec_max(evals)
u_mu = l_mu * r_sqr
u_sigma = l_sigma * sqrt(r_sqr)
return u_mu, u_sigma
def lin_ellipsoid_safety_distance(p_center,
q_shape,
h_mat,
h_vec,
c_safety=1.0):
"""Compute symbolically the distance between eLlipsoid and polytope in casadi.
Evaluate the distance of an ellipsoid E(p_center,q_shape), to a polytopic set
of the form:
h_mat * x <= h_vec.
Parameters
----------
p_center: n_s x 1 array
The center of the state ellipsoid
q_shape: n_s x n_s array
The shape matrix of the state ellipsoid
h_mat: m x n_s array:
The shape matrix of the safe polytope (see above)
h_vec: m x 1 array
The additive vector of the safe polytope (see above)
Returns
-------
d_safety: 1darray of length m
The distance of the ellipsoid to the polytope. If d < 0 (elementwise),
the ellipsoid is inside the poltyope (safe), otherwise safety is not guaranteed.
"""
d_center = mtimes(h_mat, p_center)
d_shape = c_safety * sqrt(sum1(mtimes(q_shape, h_mat.T) * h_mat.T)).T
d_safety = d_center + d_shape - h_vec
return d_safety
def gp_pred(x,
kern,
beta=None,
x_train=None,
k_inv_training=None,
pred_var=True):
"""
"""
n_pred, _ = np.shape(x)
if beta is None:
pred_mu = SX.zeros(n_pred, 1)
else:
k_star = kern(x, y=x_train)
pred_mu = mtimes(k_star, beta)
if pred_var:
pred_sigm = kern(x, diag_only=True)
if not beta is None:
pred_sigm = pred_sigm - sum2(
mtimes(k_star, k_inv_training) * k_star)
return pred_mu, pred_sigm
return pred_mu
def _include_statistics_of_expression(self, expr, name, exp_phi, ls_factor,
model, problem):
if self.variable_type == 'algebraic':
raise NotImplementedError(
"stochastic variables as algebraic variables are not implemented"
)
if self.variable_type == 'theta':
var_vector = self._get_expression_in_each_scenario(expr, model)
stochastic_var_mean = problem.create_optimization_theta(
name + '_mean', 1)
stochastic_var_var = problem.create_optimization_theta(
name + '_var', 1)
stochastic_var_par = problem.create_optimization_theta(
name + '_par', self.n_pol_parameters)
problem.include_time_equality(stochastic_var_par -
mtimes(ls_factor, var_vector),
when='end')
problem.include_time_equality(stochastic_var_mean -
stochastic_var_par[0],
when='end')
problem.include_time_equality(stochastic_var_var - mtimes(
(stochastic_var_par[1:]**2).T, exp_phi[1:]),
when='end')
return stochastic_var_mean, stochastic_var_var, stochastic_var_par
def __mul__(self, other):
if isinstance(other, Pose):
return Pose(casadi.mtimes(self.R, other.R), casadi.mtimes(self.R, other.t) + self.t)
elif isinstance(other, casadi.casadi.MX) and other.shape == (3, 1):
return casadi.mtimes(self.R, other) + self.t
else:
raise NotImplementedError
def test_matrixexpressions(self):
with open(os.path.join(TEST_DIR, 'MatrixExpressions.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'MatrixExpressions')
print(casadi_model)
ref_model = Model()
A = ca.MX.sym("A", 3, 3)
b = ca.MX.sym("b", 3)
c = ca.MX.sym("c", 3)
d = ca.MX.sym("d", 3)
C = ca.MX.sym("C", 2, 3)
D = ca.MX.sym("D", 3, 2)
E = ca.MX.sym("E", 2, 3)
I = ca.MX.sym("I", 5, 5)
F = ca.MX.sym("F", 3, 3)
ref_model.alg_states = list(map(Variable, [A, b, c, d]))
ref_model.constants = list(map(Variable, [C, D, E, I, F]))
constant_values = [
1.7 * ca.DM.ones(2, 3),
ca.DM.zeros(3, 2),
ca.DM.ones(2, 3),
ca.DM.eye(5),
ca.DM.triplet([0, 1, 2], [0, 1, 2], [1, 2, 3], 3, 3)
]
for const, val in zip(ref_model.constants, constant_values):
const.value = val
ref_model.equations = [
ca.mtimes(A, b) - c,
ca.mtimes(A.T, b) - d, F[1, 2]
]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def _relax_algebraic_equations(self):
# get the equations to relax
alg_relax = self.model.alg[self.relax_algebraic_index]
n_alg_relax = alg_relax.numel()
self.n_relax += n_alg_relax
# create a symbolic nu
nu_alg = SX.sym('AL_nu_alg', n_alg_relax)
self.nu_sym = vertcat(self.nu_sym, nu_alg)
# save the relaxed algebraic equations for computing the update later
self.relaxed_alg = vertcat(self.relaxed_alg, alg_relax)
# include the penalization term in the objective
self.problem.L += (mtimes(nu_alg.T, alg_relax) +
self.mu_sym / 2. * mtimes(alg_relax.T, alg_relax))
# include the relaxed y_sym as controls
u_guess = self.problem.y_guess[
self.
relax_algebraic_index] if self.problem.y_guess is not None else None
self.problem.include_control(
self.model.y[self.relax_algebraic_var_index],
u_max=self.problem.y_max[self.relax_algebraic_var_index],
u_min=self.problem.y_min[self.relax_algebraic_var_index],
u_guess=u_guess)
self.problem.remove_algebraic(
self.model.y[self.relax_algebraic_var_index], alg_relax)
def generate_cost_function(self, p_0, u_0, p_all, q_all, mu_perf, sigma_perf,
k_ff_safe, k_fb_safe, sigma_safe, k_fb_perf=None,
k_ff_perf=None, gp_pred_sigma_perf=None,
custom_cost_func=None, eps_noise=0.0):
# Generate cost function
if custom_cost_func is None:
cost = 0
if self.n_perf > 1:
n_cost_deviation = np.minimum(self.n_perf, self.n_safe)
for i in range(1, n_cost_deviation):
cost += mtimes(mu_perf[i, :] - p_all[i, :],
mtimes(.1 * self.wx_cost,
(mu_perf[i, :] - p_all[i, :]).T))
for i in range(self.n_perf):
cost -= sqrt(sum2(gp_pred_sigma_perf[i, :] + eps_noise))
else:
for i in range(self.n_safe):
cost -= sqrt(sum2(sigma_safe[i, :] + eps_noise))
else:
if self.n_perf > 1:
cost = custom_cost_func(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)
else:
cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe,
sigma_safe)
return cost
def get_gravity_rnea(self, root, tip, gravity):
"""Returns the gravitational term as a casadi function."""
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
n_joints = self.get_n_joints(root, tip)
q = cs.SX.sym("q", n_joints)
i_X_p, Si, Ic = self._model_calculation(root, tip, q)
v = []
a = []
ag = cs.SX([0., 0., 0., gravity[0], gravity[1], gravity[2]])
f = []
tau = cs.SX.zeros(n_joints)
for i in range(0, n_joints):
if i == 0:
a.append(cs.mtimes(i_X_p[i], -ag))
else:
a.append(cs.mtimes(i_X_p[i], a[i - 1]))
f.append(cs.mtimes(Ic[i], a[i]))
for i in range(n_joints - 1, -1, -1):
tau[i] = cs.mtimes(Si[i].T, f[i])
if i != 0:
f[i - 1] = f[i - 1] + cs.mtimes(i_X_p[i].T, f[i])
tau = cs.Function("C", [q], [tau], self.func_opts)
return tau
def addResidualCost(self, measurement_model, X, t_array, y_array, R, params=None):
N_measurements = t_array.shape[0]
if y_array is not None:
p = y_array.shape[0]
else:
p = params["p"]
# Define Casadi function
x = casadi.MX.sym('x', self.n)
y = casadi.MX.sym('y', p)
residual = casadi.Function('l', [x, y], [y - measurement_model(x, params)])
# Measurment parameters
Y = self.addParameter(N_measurements, p)
# Define stage cost
for (i, t) in enumerate(t_array):
# Build the expression for x at time t based on
phi_t = self.CPM.evaluateLagrangePolynomials(t)
X_t = 0
for j in range(self.N + 1):
X_t += X[j]*phi_t[j]
# Then add to the cost
r_t = residual(X_t, Y[i])
self.J += casadi.mtimes(r_t.T, casadi.mtimes(R, r_t))
if y_array is not None:
self.setMeasurement(Y, t_array, y_array)
return Y
def sqrt_correct(Rs, H, W):
"""
source: Fast Stable Kalman Filter Algorithms Utilising the Square Root, Steward 98
Rs: sqrt(R)
H: measurement matrix
W: sqrt(P)
@return:
Wp: sqrt(P+) = sqrt((I - KH)P)
K: Kalman gain
Ss: Innovation variance
"""
n_x = H.shape[1]
n_y = H.shape[0]
B = ca.sparsify(ca.blockcat(Rs, ca.mtimes(H, W), ca.SX.zeros(n_x, n_y), W))
# qr by default is upper triangular, so we transpose inputs and outputs
B_Q, B_R = ca.qr(B.T) # B_Q orthogonal, B_R, lower triangular
B_Q = B_Q.T
B_R = B_R.T
Wp = B_R[n_y:, n_y:]
Ss = B_R[:n_y, :n_y]
P_HT_SsInv = B_R[n_y:, :n_y]
K = ca.mtimes(P_HT_SsInv, ca.inv(Ss))
return Wp, K, Ss
def eval_ws_eq(self, z, p): t_ws = p[0:2] R_ws = np.array( [ [ca.cos(p[2]), -ca.sin(p[2])], [ca.sin(p[2]), ca.cos(p[2])] ] ) obca_d = [] obca = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ j : j + self.n_ineq[i] ] mu = z[ self.N_ineq + i*self.m_ineq : self.N_ineq + (i+1)*self.m_ineq ] d = z[ self.N_ineq + self.M_ineq + i ] obca_d.append( -ca.dot(self.g, mu) + ca.dot( ca.mtimes(A, t_ws)-b, Lambda ) - d) obca.append( ca.mtimes(self.G.T, mu) + ca.mtimes( ca.mtimes(A, R_ws).T, Lambda ) ) j += self.n_ineq[i] ws_eq = ca.vcat( [ca.vcat(obca_d), ca.vcat(obca)] ) return ws_eq
def code_generation():
x = ca.SX.sym('x', 14)
x_gz = ca.SX.sym('x_gz', 14)
p = ca.SX.sym('p', 16)
u = ca.SX.sym('u', 4)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
gz_eqs = gazebo_equations()
f_state = gz_eqs['state_from_gz']
eqs = rocket_equations()
C_FLT_FRB = gz_eqs['C_FLT_FRB']
F_FRB, M_FRB = eqs['force_moment'](x, u, p)
F_FLT = ca.mtimes(C_FLT_FRB, F_FRB)
M_FLT = ca.mtimes(C_FLT_FRB, M_FRB)
f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'],
['fin'])
f_force_moment = ca.Function('rocket_force_moment', [x, u, p],
[F_FLT, M_FLT], ['x', 'u', 'p'],
['F_FLT', 'M_FLT'])
u_control = eqs['control'](x, p, t, dt)
f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
gen = ca.CodeGenerator('casadi_gen.c', {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
})
gen.add(f_state)
gen.add(f_force_moment)
gen.add(f_control)
gen.add(f_u_to_fin)
gen.generate()
def _relax_time_equalities(self):
# get the equations to relax
eq_relax = self.problem.g_eq[self.relax_time_equality_index]
n_eq_relax = eq_relax.numel()
self.n_relax += n_eq_relax
# create a symbolic nu
nu_alg = SX.sym('AL_nu_eq', n_eq_relax)
self.nu_sym = vertcat(self.nu_sym, nu_alg)
# save the relaxed algebraic equations for computing the update later
self.relaxed_eq = vertcat(self.relaxed_eq, eq_relax)
# include the penalization term in the objective
self.problem.L = self.problem.L + (
mtimes(nu_alg.T, eq_relax) +
self.mu_sym / 2. * mtimes(eq_relax.T, eq_relax))
# Remove equality
self.problem.g_eq = remove_variables_from_vector_by_indices(
self.relax_time_equality_index, self.problem.g_eq)
for ind in sorted(self.relax_time_equality_index, reverse=True):
self.problem.time_g_eq.pop(ind)
def interpolate(ts, xs, t, equidistant, mode=0):
if interp1d is not None:
if mode == 0:
mode_str = 'linear'
elif mode == 1:
mode_str = 'floor'
else:
mode_str = 'ceil'
return interp1d(ts, xs, t, mode_str, equidistant)
else:
if mode == 1:
xs = xs[:-1] # block-forward
else:
xs = xs[1:] # block-backward
t = ca.MX(t)
if t.size1() > 1:
t_ = ca.MX.sym('t')
xs_ = ca.MX.sym('xs', xs.size1())
f = ca.Function('interpolant', [t_, xs_], [
ca.mtimes(ca.transpose((t_ >= ts[:-1]) * (t_ < ts[1:])), xs_)
])
f = f.map(t.size1(), 'serial')
return ca.transpose(f(ca.transpose(t), ca.repmat(xs, 1,
t.size1())))
else:
return ca.mtimes(ca.transpose((t >= ts[:-1]) * (t < ts[1:])), xs)
def _predict_sym(self, x_test, return_std=False, return_cov=False):
"""
Computes the GP posterior mean and covariance at a given a test sample using CasADi symbolics.
:param x_test: vector of size mx1, where m is the number of features used for the GP prediction
:return: the posterior mean (scalar) and covariance (scalar).
"""
k_s = self.kernel(self._x_train_cs, x_test.T)
# Posterior mean value
mu_s = cs.mtimes(k_s.T, self._K_inv_y_cs) + self.y_mean
if not return_std and not return_cov:
return {'mu': mu_s}
k_ss = self.kernel(x_test, x_test) + 1e-8 * cs.MX.eye(x_test.shape[1])
# Posterior covariance
cov_s = k_ss - cs.mtimes(cs.mtimes(k_s.T, self._K_inv_cs), k_s)
cov_s = cs.diag(cov_s)
if return_std:
return {'mu': mu_s, 'std': np.sqrt(cov_s)}
return {'mu': mu_s, 'cov': cov_s}
def estimate(self, t_k, y_k, u_k):
if not self._checked:
self._check()
self._checked = True
if not self._types_fixed:
self._fix_types()
self._types_fixed = True
x_mean = self.x_mean
x_cov = self.p_k
(x_hat_k_minus, p_k_minus, y_hat_k_minus, p_yk_yk,
k_gain) = self._priori_update(x_mean,
x_cov,
u=u_k,
p=self.p,
theta=self.theta)
x_hat_k = x_hat_k_minus + mtimes(k_gain, (y_k - y_hat_k_minus))
p_k = p_k_minus - mtimes(k_gain, mtimes(p_yk_yk, k_gain.T))
self.x_mean = x_hat_k
self.p_k = p_k
self.dataset.insert_data('x', t_k, self.x_mean)
self.dataset.insert_data('P', t_k, vec(self.p_k))
return x_hat_k, p_k
def diff(self, z, z_train):
"""
Computes the symbolic differentiation of the kernel function, evaluated at point z and using the training
dataset z_train. This function implements equation (80) from overleaf document, without the c^{v_x} vector,
and for all the partial derivatives possible (m), instead of just one.
:param z: evaluation point. Symbolic vector of length m
:param z_train: training dataset. Symbolic matrix of shape n x m
:return: an m x n matrix, which is the derivative of the exponential kernel function evaluated at point z
against the training dataset.
"""
if self.kernel_type != 'squared_exponential':
raise NotImplementedError
len_scale = self.params['l'] if len(
self.params['l']) > 0 else self.params['l'] * cs.MX.ones(
z_train.shape[1])
len_scale = np.atleast_2d(len_scale**2)
# Broadcast z vector to have the shape of z_train (tile z to to the number of training points n)
z_tile = cs.mtimes(cs.MX.ones(z_train.shape[0], 1), z.T)
# Compute k_zZ. Broadcast it to shape of z_tile and z_train, i.e. by the number of variables in z.
k_zZ = cs.mtimes(cs.MX.ones(z_train.shape[1], 1),
self.__call__(z_train, z.T).T)
return -k_zZ * (z_tile - z_train).T / cs.mtimes(
cs.MX.ones(z_train.shape[0], 1), len_scale).T
def solve_K(self, robot, X, U):
K = c.DM.zeros(robot.nu * robot.nx, self.T + 1)
_, A, B = robot.proc_model()
for i in range(self.T, 0, -1):
At = A(X[:, i], U[:, i - 1])
Bt = B(X[:, i], U[:, i - 1])
#print "At:", At
#print "Bt:", Bt
if i == self.T:
P = self.Wxf
else:
P = c.mtimes(c.mtimes(At.T, P), At) - c.mtimes(
c.mtimes(c.mtimes(At.T, P), Bt), K_mat) + self.Wx
K_mat = c.mtimes(c.inv(self.Wu + c.mtimes(c.mtimes(Bt.T, P), Bt)),
c.mtimes(c.mtimes(Bt.T, P), At))
K[:, i] = c.reshape(K_mat, robot.nu * robot.nx, 1)
return K
def Kalman_filter(self, robot, X_prev_est, X_prev_act, U, P_prev):
#Prediction
X_prior = robot.kinematics(X_prev_est, U[0], U[1], U[2], 0)
Sigma_w = (self.epsilon**2) * self.Sigma_w
Sigma_nu = (self.epsilon**2) * self.Sigma_nu
P_prior = c.mtimes(c.mtimes(robot.A, P_prev), robot.A.T) + c.mtimes(
c.mtimes(robot.G, Sigma_w), robot.G.T)
#Update
M = c.DM([[(X_prev_act[0] - 3)**2, 0, 0], [0, 1, 0], [0, 0, 1]])
S = P_prior + c.mtimes(c.mtimes(M, Sigma_nu), M.T)
K_gain = c.mtimes(P_prior, c.inv(S))
P_post = c.mtimes(c.DM.eye(robot.nx) - K_gain, P_prior)
X_act = robot.kinematics(X_prev_act, U[0], U[1], U[2], self.epsilon)
X_act = np.reshape(X_act, (robot.nx, )) #to suit the receiving array
nu0 = np.random.normal(0, np.sqrt(Sigma_nu[0, 0]))
nu1 = np.random.normal(0, np.sqrt(Sigma_nu[1, 1]))
nu2 = np.random.normal(0, np.sqrt(Sigma_nu[2, 2]))
nu = c.DM([[nu0], [nu1], [nu2]])
Y = X_act + c.mtimes(M, nu)
X_est = X_prior + c.mtimes(K_gain, Y - X_prior)
X_est = np.reshape(X_est, (robot.nx, ))
return X_est, X_act, P_post
def stage_cost(_x, _u, _x_ref=None, _u_ref=None):
if _x_ref is None:
_x_ref = cs.DM.zeros(_x.shape)
if _u_ref is None:
_u_ref = cs.DM.zeros(_u.shape)
dx = _x - _x_ref
du = _u - _u_ref
return cs.mtimes([dx.T, Q, dx]) + cs.mtimes([du.T, R, du])
def minimize_jerk(u):
xdd = u[0, :]
ydd = u[1, :]
xddd = casadi.diff(xdd)
yddd = casadi.diff(ydd)
xddd_squared = casadi.mtimes(xddd, casadi.transpose(xddd))
yddd_squared = casadi.mtimes(yddd, casadi.transpose(yddd))
return xddd_squared + yddd_squared
def get_regularised_cost_expr(self):
slack_var = self.skill_spec.slack_var
if slack_var is not None:
slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
else:
slack_cost = 0.0
return self.weight_shifter * self.cost_expression + slack_cost
def get_cost_integrand_function(self):
"""Returns a casadi function for the discretized integrand of
the cost expression integrated one timestep. For the rectangle
method, this just amounts to timing by the timestep.
As with the other controllers, the cost is affected by the
weight shifter, giving a regularised cost with the slack
variables.
"""
# Setup new symbols needed
dt = self.timestep
# Setup skill_spec symbols
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
cntrl_vars = [robot_vel_var]
cntrl_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
virtual_vel_var = self.skill_spec.virtual_vel_var
cntrl_vars += [virtual_vel_var]
cntrl_names += ["virtual_vel_var"]
slack_var = self.skill_spec.slack_var
if slack_var is not None:
list_vars += [slack_var]
list_names += ["slack_var"]
# Full symbol list same way as in other controllers
list_vars += cntrl_vars
list_names += cntrl_names
# Expression for the cost with regularisation:
if slack_var is not None:
slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
regularised_cost = self.weight_shifter*self.cost_expression
regularised_cost += slack_cost
else:
regularised_cost = self.cost_expression
# Choose integration method
if self.options["cost_integration_method"].lower() == "rectangle":
cost_integrand = regularised_cost*dt
elif self.options["cost_integration_method"].lower() == "trapezoidal":
# Trapezoidal rule
raise NotImplementedError("Trapezoidal rule integration not"
+ " implemented.")
elif self.options["cost_integration_method"].lower() == "simpson":
# Simpson rule
raise NotImplementedError("Simpson rule integration not"
+ " implemented.")
else:
raise NotImplementedError(self.options["cost_integration_method"]
+ " is not a known integration method.")
return cs.Function("fc_k", list_vars, [cost_integrand],
list_names, ["cost_integrand"])
def get_constraints_expr(self):
"""Returns a casadi expression describing all the constraints, and
expressions for their upper and lower bounds.
Return:
tuple: (A, Blb,Bub) for Blb<=A*x<Bub, where A*x, Blb and Bub are
casadi expressions.
"""
cnstr_expr_list = []
lb_cnstr_expr_list = [] # lower bound
ub_cnstr_expr_list = [] # upper bound
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
virtual_var = self.skill_spec.virtual_var
n_slack = self.skill_spec.n_slack_var
slack_ind = 0
for cnstr in self.skill_spec.constraints:
expr_size = cnstr.expression.size()
# What's A*opt_var?
cnstr_expr = cnstr.jacobian(robot_var)
if virtual_var is not None:
cnstr_expr = cs.horzcat(cnstr_expr,
cnstr.jacobian(virtual_var))
# Everyone wants a feedforward
lb_cnstr_expr = -cnstr.jacobian(time_var)
ub_cnstr_expr = -cnstr.jacobian(time_var)
# Setup bounds
if isinstance(cnstr, EqualityConstraint):
lb_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
ub_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
elif isinstance(cnstr, SetConstraint):
lb_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_min - cnstr.expression)
ub_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_max - cnstr.expression)
elif isinstance(cnstr, VelocityEqualityConstraint):
lb_cnstr_expr += cnstr.target
ub_cnstr_expr += cnstr.target
elif isinstance(cnstr, VelocitySetConstraint):
lb_cnstr_expr += cnstr.set_min
ub_cnstr_expr += cnstr.set_max
# Soft constraints have slack
if n_slack > 0:
slack_mat = cs.DM.zeros((expr_size[0], n_slack))
if cnstr.constraint_type == "soft":
slack_mat[:, slack_ind:slack_ind +
expr_size[0]] = -cs.DM.eye(expr_size[0])
slack_ind += expr_size[0]
cnstr_expr = cs.horzcat(cnstr_expr, slack_mat)
# Add to lists
cnstr_expr_list += [cnstr_expr]
lb_cnstr_expr_list += [lb_cnstr_expr]
ub_cnstr_expr_list += [ub_cnstr_expr]
cnstr_expr_full = cs.vertcat(*cnstr_expr_list)
lb_cnstr_expr_full = cs.vertcat(*lb_cnstr_expr_list)
ub_cnstr_expr_full = cs.vertcat(*ub_cnstr_expr_list)
return cnstr_expr_full, lb_cnstr_expr_full, ub_cnstr_expr_full
def buildDynamicalSystem(self):
# q1 is first link q2 is second link
q1 = ca.MX.sym('q1')
dq1 = ca.MX.sym('dq1')
q2 = ca.MX.sym('q2')
dq2 = ca.MX.sym('dq2')
u = ca.MX.sym('u')
theta = ca.MX.sym("theta", 7, 1)
if self.trainMotor:
Rm = ca.MX.sym("Rm")
km = ca.MX.sym("km")
params = ca.vertcat(ca.MX.sym("params", 7, 1), Rm, km)
else:
Rm = self.Rm
km = self.km
params = theta
states = ca.vertcat(q1, q2, dq1, dq2)
controls = u # km * (u - km * dq1) / Rm;
if self.hackOn:
# convert actions which are given as torques to voltage to be consistent with the controller inputs of Lukas
u = u * Rm / km
controls_torque = km * (u - km * dq1) / Rm
# build matrix for mass matrix
phi_1_1 = ca.MX(2, 7)
phi_1_1[0, 0] = ca.MX.ones(1)
phi_1_1[0, 1] = ca.sin(q2) * ca.sin(q2)
phi_1_1[1, 2] = ca.cos(q2)
phi_1_2 = ca.MX(2, 7)
phi_1_2[0, 2] = -ca.cos(q2)
phi_1_2[1, 3] = ca.MX.ones(1)
mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params),
ca.mtimes(phi_1_2, params))
phi_2 = ca.MX(2, 7)
phi_2[0, 1] = ca.sin(2 * q2) * dq1 * dq2
phi_2[0, 2] = ca.sin(q2) * dq2 * dq2
phi_2[0, 5] = dq1
phi_2[1, 1] = -1 / 2 * ca.sin(2 * q2) * dq1 * dq1
phi_2[1, 4] = self.g * ca.sin(q2)
phi_2[1, 6] = dq2
forces = ca.mtimes(phi_2, params)
actions = ca.vertcat(controls_torque, ca.MX.zeros(1))
b = actions - forces
states_dd = ca.solve(mass_matrix, b)
states_d = ca.vertcat(dq1, dq2, states_dd[0], states_dd[1])
return states, states_d, controls, params
def kinematics(cls, r, w):
assert r.shape == (4, 1) or r.shape == (4, )
assert w.shape == (3, 1) or w.shape == (3, )
a = r[:3]
n_sq = ca.dot(a, a)
X = wedge(a)
B = 0.25 * ((1 - n_sq) * ca.SX.eye(3) + 2 * X +
2 * ca.mtimes(a, ca.transpose(a)))
return ca.vertcat(ca.mtimes(B, w), 0)
def dynamics_rear(X_prev, S, v):
dt = 0.1
L = 2.33
X_new = X_prev + dt*casadi.blockcat([[casadi.mtimes(v/2.0,casadi.cos(X_prev[2] - S - 8*m.pi/180)+casadi.cos(X_prev[2] + (S + 8*m.pi/180)))],
[casadi.mtimes(v/2.0,casadi.sin(X_prev[2] - S - 8*m.pi/180) + casadi.sin(X_prev[2] + (S + 8*m.pi/180)))],
[casadi.mtimes(v,casadi.sin(-S - 8*m.pi/180)*1/L)]])
X_new[2] = casadi.atan2(casadi.sin(X_new[2]), casadi.cos(X_new[2]))
return X_new
def construct_upd_x(self, problem=None):
# construct optifather & give reference to problem
self.father_updx = OptiFather(self.group.values())
self.problem.father = self.father_updx
# define parameters
z_i = self.define_parameter('z_i', self.q_i_struct.shape[0])
z_ji = self.define_parameter('z_ji', self.q_ji_struct.shape[0])
l_i = self.define_parameter('l_i', self.q_i_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
rho = self.define_parameter('rho')
if problem is None:
# put z and l variables in the struct format
z_i = self.q_i_struct(z_i)
z_ji = self.q_ji_struct(z_ji)
l_i = self.q_i_struct(l_i)
l_ji = self.q_ji_struct(l_ji)
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t/T
# get (part of) variables
x_i = self._get_x_variables(symbolic=True)
# 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([z_ji, l_ji], tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mtimes(l.T, x-z)
if not self.options['AMA']:
obj += 0.5*rho*mtimes((x-z).T, (x-z))
for nghb in self.q_ji.keys():
z = z_ji[str(nghb), child.label, name]
l = l_ji[str(nghb), child.label, name]
obj += mtimes(l.T, x-z)
if not self.options['AMA']:
obj += 0.5*rho*mtimes((x-z).T, (x-z))
self.define_objective(obj)
# construct problem
prob, buildtime = self.father_updx.construct_problem(
self.options, str(self._index))
else:
prob, buildtime = self.father_updx.construct_problem(self.options, str(self._index), problem)
self.problem_upd_x = prob
self.father_updx.init_transformations(self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_admm()
return buildtime
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_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 MX 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.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho*x)
G = -(1/rho)*mtimes(A, A.T)
h = b + (1/rho)*mtimes(A, f)
mu = solve(G, h)
z = -(1/rho)*(mtimes(A.T, mu)+f)
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, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
self.problem_upd_z = prob
return buildtime
def evalspline(s, x):
# Evaluate spline with symbolic variable
# This is possible not the best way to implement this. The conditional node
# from casadi should be considered
Bl = s.basis
coeffs = s.coeffs
k = Bl.knots
basis = [[]]
for i in range(len(k) - 1):
if i < Bl.degree + 1 and Bl.knots[0] == Bl.knots[i]:
basis[-1].append((x >= Bl.knots[i])*(x <= Bl.knots[i + 1]))
else:
basis[-1].append((x > Bl.knots[i])*(x <= Bl.knots[i + 1]))
for d in range(1, Bl.degree + 1):
basis.append([])
for i in range(len(k) - d - 1):
b = 0 * x
bottom = k[i + d] - k[i]
if bottom != 0:
b = (x - k[i]) * basis[d - 1][i] / bottom
bottom = k[i + d + 1] - k[i + 1]
if bottom != 0:
b += (k[i + d + 1] - x) * basis[d - 1][i + 1] / bottom
basis[-1].append(b)
result = 0.
for l in range(len(Bl)):
result += mtimes(coeffs[l], basis[-1][l])
return result
def _get_symbolic_flux(self, finite_element, degree):
""" Get a symbolic expression for the boundary fluxes at the given
finite_element and polynomial degree """
return cs.mtimes(cs.DM(self.efms_object.T.values),
self.var.a_sx[finite_element, degree-1] *
self.var.v_sx[self._get_stage_index(finite_element)])
def crop_spline(spline, min_value, max_value):
T, knots2 = get_interval_T(spline.basis, min_value, max_value)
if isinstance(spline.coeffs, (SX, MX)):
coeffs2 = mtimes(T, spline.coeffs)
else:
coeffs2 = T.dot(spline.coeffs)
basis2 = BSplineBasis(knots2, spline.basis.degree)
return BSpline(basis2, coeffs2)
def get_derivative(self, s):
# Case 1: s is a constant, e.g. MX(5)
if ca.MX(s).is_constant():
return 0
# Case 2: s is a symbol, e.g. MX(x)
elif s.is_symbolic():
if s.name() not in self.derivative:
if len(self.for_loops) > 0 and s in self.for_loops[-1].indexed_symbols:
# Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol.
for_loop_symbol = self.for_loops[-1].indexed_symbols[s]
s_without_index = self.get_mx(ast.ComponentRef(name=for_loop_symbol.tree.name))
der_s_without_index = self.get_derivative(s_without_index)
if ca.MX(der_s_without_index).is_symbolic():
return self.get_indexed_symbol(ast.ComponentRef(name=der_s_without_index.name(), indices=for_loop_symbol.tree.indices), der_s_without_index)
else:
return 0
else:
der_s = _new_mx("der({})".format(s.name()), s.size())
# If the derivative contains an expression (e.g. der(x + y)) this method is
# called with MX variables that are the result of a ca.symvar call. This
# ca.symvar call strips the _modelica_shape field from the MX variable,
# therefore we need to find the original MX to get the modelica shape.
der_s._modelica_shape = \
self.nodes[self.current_class][s.name()]._modelica_shape
self.derivative[s.name()] = der_s
self.nodes[self.current_class][der_s.name()] = der_s
return der_s
else:
return self.derivative[s.name()]
# Case 3: s is an already indexed symbol, e.g. MX(x[1])
elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic():
slice_info = s.info()['slice']
dep = s.dep()
if dep.name() not in self.derivative:
der_dep = _new_mx("der({})".format(dep.name()), dep.size())
der_dep._modelica_shape = \
self.nodes[self.current_class][dep.name()]._modelica_shape
self.derivative[dep.name()] = der_dep
self.nodes[self.current_class][der_dep.name()] = der_dep
return der_dep[slice_info['start']:slice_info['stop']:slice_info['step']]
else:
return self.derivative[dep.name()][slice_info['start']:slice_info['stop']:slice_info['step']]
# Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2)
# Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2)
else:
# Differentiate expression using CasADi
orig_deps = ca.symvar(s)
deps = ca.vertcat(*orig_deps)
J = ca.Function('J', [deps], [ca.jacobian(s, deps)])
J_sparsity = J.sparsity_out(0)
der_deps = [self.get_derivative(dep) if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size()) for j, dep in enumerate(orig_deps)]
return ca.mtimes(J(deps), ca.vertcat(*der_deps))
def shift_knot1_fwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = MX.sym('cfs', cfs.shape)
t_shift_sym = MX.sym('t_shift')
T = shiftfirstknot_T(basis, t_shift_sym)
cfs2_sym = mtimes(T, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
T = shiftfirstknot_T(basis, t_shift)
return T.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')
_, 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 construct_upd_res(self, problem=None):
if problem is not None:
self.problem_upd_res = problem
return 0.
# create parameters
x_i = struct_symMX(self.q_i_struct)
z_i = struct_symMX(self.q_i_struct)
z_i_p = struct_symMX(self.q_i_struct)
z_ij = struct_symMX(self.q_ij_struct)
z_ij_p = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
t0 = t/T
rho = MX.sym('rho')
inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho]
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_i_p = self.q_i_struct(z_i_p)
z_ij = self.q_ij_struct(z_ij)
z_ij_p = self.q_ij_struct(z_ij_p)
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, z_i_p], tf, self.q_i)
self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij)
# compute residuals
pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat))
pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat))
dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat))
dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat))
cr = rho*pr + dr
out = [pr, dr, cr]
# create problem
prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options)
self.problem_upd_res = prob
return buildtime
def _initialize_polynomial_constraints(self):
""" Add constraints to the model to account for system dynamics and
continuity constraints """
# All collocation time points
T = np.zeros((self.nk, self.d+1), dtype=object)
for k in range(self.nk):
for j in range(self.d+1):
T[k,j] = (self.var.h_sx[self._get_stage_index(k)] *
(k + self.col_vars['tau_root'][j]))
# For all finite elements
for k in range(self.nk):
# For all collocation points
for j in range(1, self.d+1):
# Get an expression for the state derivative at the collocation
# point
xp_jk = cs.mtimes(cs.DM(self.col_vars['C'][:,j]).T, self.var.x_sx[k]).T
# Add collocation equations to the NLP.
# Boundary fluxes are calculated by multiplying the EFM
# coefficients in V by the efm matrix
[fk] = self.dxdt.call(
[T[k,j], cs.SX(self.var.x_sx[k,j]),
self._get_symbolic_flux(k, j)])
self.add_constraint(
self.var.h_sx[self._get_stage_index(k)] * fk - xp_jk,
msg='DXDT collocation - FE {0}, degree {1}'.format(k,j))
# Add continuity equation to NLP
if k+1 != self.nk:
# Get an expression for the state at the end of the finite
# element
self.add_constraint(
cs.SX(self.var.x_sx[k+1,0]) -
self._get_endpoint_expr(self.var.x_sx[k]),
msg='Continuity - FE {0}'.format(k))
# Get an expression for the endpoint for objective purposes
xf = self._get_endpoint_expr(self.var.x_sx[-1])
self.xf = {met : x_sx for met, x_sx in zip(self.boundary_species, xf)}
# Similarly, get an expression for the beginning point
x0 = self.var.x_sx[0,0,:]
self.x0 = {met : x_sx for met, x_sx in zip(self.boundary_species, x0)}
def dot(self, other):
if isinstance(other, (cas.MX, cas.SX)):
# compatible with casadi 3.0 -- added by ruben
return cas.mtimes(cas.DM(csr_matrix(self)), other)
# NOT COMPATIBLE WITH CASADI 2.4
# return cas.DMatrix(csr_matrix(self)).mul(other)
elif get_module(other) in ['cvxpy', 'cvxopt']:
return cvxopt.sparse(cvxopt.matrix(self.toarray())) * other
# A = self.tocoo()
# B = cvxopt.spmatrix(
# A.data, A.row.tolist(), A.col.tolist(), A.shape
# )
# return B * other
else:
try: # Scipy sparse matrix
return super(csr_matrix_alt, self).dot(other)
except: # Regular numpy matrix
return np.dot(self.toarray(), other)
def shiftfirstknot_T(basis, t_shift, inverse=False):
# Create transformation matrix that shifts the first (degree+1) knots over
# t_shift. With inverse = True, the inverse transformation is also
# computed.
knots, deg = basis.knots, basis.degree
N = len(basis)
if isinstance(t_shift, SX):
typ, sym = SX, True
elif isinstance(t_shift, MX):
typ, sym = MX, True
else:
typ, sym = np, False
_T = typ.eye(deg+1)
for k in range(deg+1):
_t = typ.zeros((deg+1+k+1, deg+1+k))
for j in range(deg+1+k+1):
if j >= deg+1:
_t[j, j-1] = 1.
elif j <= k:
_t[j, j] = 1.
else:
_t[j, j-1] = (knots[j+deg-k]-t_shift)/(knots[j+deg-k]-knots[j])
_t[j, j] = (t_shift-knots[j])/(knots[j+deg-k]-knots[j])
_T = mtimes(_t, _T) if sym else _t.dot(_T)
T = typ.eye(N)
T[:deg+1, :deg+1] = _T[deg+1:, :]
if inverse: # T is upper triangular: easy inverse
Tinv = typ.eye(len(basis))
for i in range(deg, -1, -1):
Tinv[i, i] = 1./T[i, i]
for j in range(deg, i, -1):
Tinv[i, j] = (-1./T[i, i])*sum([T[i, k]*Tinv[k, j]
for k in range(i+1, deg+2)])
return T, Tinv
else:
return T
def _get_endpoint_expr(self, state_sx):
"""Use the variables in self.col_vars['D'] for find an expression for
the end of the finite element """
return cs.mtimes(cs.DM(self.col_vars['D']).T, state_sx).T
def __init__(self,objective,*args):
"""
optisolve(objective)
optisolve(objective,constraints)
"""
if len(args)>=1:
constraints = args[0]
else:
constraints = []
options = dict()
if len(args)>=2:
options = args[1]
if not isinstance(constraints,list):
raise Exception("Constraints must be given as list: [x>=0,y<=0]")
[ gl_pure, gl_equality] = sort_constraints( constraints )
symbols = OptimizationObject.get_primitives([objective]+gl_pure)
# helper functions for 'x'
X = C.veccat(*symbols["x"])
helper = C.Function('helper',[X],symbols["x"])
helper_inv = C.Function('helper_inv',symbols["x"],[X])
# helper functions for 'p' if applicable
if 'p' in symbols:
P = C.veccat(*symbols["p"])
self.Phelper_inv = C.Function('Phelper_inv',symbols["p"],[P])
else:
P = C.MX.sym('p',0,1)
if len(gl_pure)>0:
g_helpers = [];
for p in gl_pure:
g_helpers.append(C.MX.sym('g',p.sparsity()))
G_helpers = C.veccat(*g_helpers)
self.Ghelper = C.Function('Ghelper',[G_helpers],g_helpers)
self.Ghelper_inv = C.Function('Ghelper_inv',g_helpers,[G_helpers])
codegen = False;
if 'codegen' in options:
codegen = options["codegen"]
del options["codegen"]
opt = {}
if codegen:
options["jit"] = True
opt["jit"] = True
gl_pure_v = C.MX()
if len(gl_pure)>0:
gl_pure_v = C.veccat(*gl_pure)
if objective.is_vector() and objective.numel()>1:
F = C.vec(objective)
objective = 0.5*C.dot(F,F)
FF = C.Function('nlp',[X,P], [F])
JF = FF.jacobian()
J_out = JF.call([X,P])
J = J_out[0].T;
H = C.mtimes(J,J.T)
sigma = C.MX.sym('sigma')
Hf = C.Function('H',dict(x=X,p=P,lam_f=sigma,hess_gamma_x_x=sigma*C.triu(H)),['x', 'p', 'lam_f', 'lam_g'],['hess_gamma_x_x'],opt)
if "expand" in options and options["expand"]:
Hf = Hf.expand()
options["hess_lag"] = Hf
nlp = {"x":X,"p":P,"f":objective,"g":gl_pure_v}
self.solver = C.nlpsol('solver','ipopt', nlp, options)
# Save to class properties
self.symbols = symbols
self.helper = helper
self.helper_inv = helper_inv
self.gl_equality = gl_equality
self.resolve()
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = - cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = - cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = - casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array([[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
[0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
[0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
[0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal,
self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
arg = self.get_mx(tree.operands[2])
if n_operands == 4:
assert isinstance(tree.operands[3], ast.Primary)
mode = tree.operands[3].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp], yp)
src = func(arg)
elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
if isinstance(tree.operands[2], ast.ComponentRef):
zp = self.get_mx(entered_class.symbols[tree.operands[2].name].value)
else:
zp = self.get_mx(tree.operands[2])
arg_1 = self.get_mx(tree.operands[3])
arg_2 = self.get_mx(tree.operands[4])
if n_operands == 6:
assert isinstance(tree.operands[5], ast.Primary)
mode = tree.operands[5].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F'))
src = func(ca.vertcat(arg_1, arg_2))
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
func = self.get_function(op)
src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def construct_upd_xz(self, problem=None):
# construct optifather & give reference to problem
self.father_updx = OptiFather(self.group.values())
self.problem.father = self.father_updx
# define z_ij variables
init = self.q_ij_struct(0)
for nghb, q_ij in self.q_ij.items():
for child, q_j in q_ij.items():
for name, ind in q_j.items():
var = np.array(child._values[name])
v = var.T.flatten()[ind]
init[nghb.label, child.label, name, ind] = v
z_ij = self.define_variable(
'z_ij', self.q_ij_struct.shape[0], value=np.array(init.cat))
# define parameters
l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
# put them in the struct format
z_ij = self.q_ij_struct(z_ij)
l_ij = self.q_ij_struct(l_ij)
l_ji = self.q_ji_struct(l_ji)
# get (part of) variables
x_i = self._get_x_variables(symbolic=True)
# construct local copies of parameters
par = {}
for name, s in self.par_global.items():
par[name] = self.define_parameter(name, s.shape[0], s.shape[1])
if problem is None:
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline(x_i, tf, self.q_i)
self._transform_spline([z_ij, l_ij], tf, self.q_ij)
self._transform_spline(l_ji, tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
for nghb in self.q_ji.keys():
l = l_ji[str(nghb), child.label, name]
obj += mtimes(l.T, x)
for nghb, q_j in self.q_ij.items():
for child in q_j.keys():
for name in q_j[child].keys():
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj -= mtimes(l.T, z)
self.define_objective(obj)
# construct constraints
for con in self.global_constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = x_i[label][name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_global.items():
if s.name() == sym.name():
c = substitute(c, sym, par[name])
lb, ub = con[1], con[2]
self.define_constraint(c, lb, ub)
# construct problem
prob, buildtime = self.father_updx.construct_problem(
self.options, str(self._index), problem)
self.problem_upd_xz = prob
self.father_updx.init_transformations(self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_dd()
return buildtime
