def inv_T_matrix(T):
T_inv = cs.horzcat(
cs.horzcat(T[0:3, 0:3].T, cs.mtimes(-T[0:3, 0:3].T, T[0:3, 3])).T,
[0, 0, 0, 1]).T
return T_inv
def stack(arrays: Tuple, axis: int = 0):
"""
Join a sequence of arrays along a new axis. Returns a NumPy array if possible; if not, returns a CasADi array.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.stack.html
"""
if not is_casadi_type(arrays, recursive=True):
return _onp.stack(arrays, axis=axis)
else:
### Validate stackability
for array in arrays:
if is_casadi_type(array, recursive=False):
if not array.shape[1] == 1:
raise ValueError("Can only stack Nx1 CasADi arrays!")
else:
if not len(array.shape) == 1:
raise ValueError(
"Can only stack 1D NumPy ndarrays alongside CasADi arrays!"
)
if axis == 0 or axis == -2:
return _cas.transpose(_cas.horzcat(*arrays))
elif axis == 1 or axis == -1:
return _cas.horzcat(*arrays)
else:
raise ValueError(
"CasADi-backend arrays can only be 1D or 2D, so `axis` must be 0 or 1."
)
def linearized_quad_dynamics(self):
"""
Jacobian J matrix of the linearized dynamics specified in the function quad_dynamics. J[i, j] corresponds to
the partial derivative of f_i(x) wrt x(j).
:return: a CasADi symbolic function that calculates the 13 x 13 Jacobian matrix of the linearized simplified
quadrotor dynamics
"""
jac = cs.MX(self.state_dim, self.state_dim)
# Position derivatives
jac[0:3, 7:10] = cs.diag(cs.MX.ones(3))
# Angle derivatives
jac[3:7, 3:7] = skew_symmetric(self.r) / 2
jac[3, 10:] = 1 / 2 * cs.horzcat(-self.q[1], -self.q[2], -self.q[3])
jac[4, 10:] = 1 / 2 * cs.horzcat(self.q[0], -self.q[3], self.q[2])
jac[5, 10:] = 1 / 2 * cs.horzcat(self.q[3], self.q[0], -self.q[1])
jac[6, 10:] = 1 / 2 * cs.horzcat(-self.q[2], self.q[1], self.q[0])
# Velocity derivatives
a_u = (self.u[0] + self.u[1] + self.u[2] + self.u[3]) * self.quad.max_thrust / self.quad.mass
jac[7, 3:7] = 2 * cs.horzcat(a_u * self.q[2], a_u * self.q[3], a_u * self.q[0], a_u * self.q[1])
jac[8, 3:7] = 2 * cs.horzcat(-a_u * self.q[1], -a_u * self.q[0], a_u * self.q[3], a_u * self.q[2])
jac[9, 3:7] = 2 * cs.horzcat(0, -2 * a_u * self.q[1], -2 * a_u * self.q[1], 0)
# Rate derivatives
jac[10, 10:] = (self.quad.J[1] - self.quad.J[2]) / self.quad.J[0] * cs.horzcat(0, self.r[2], self.r[1])
jac[11, 10:] = (self.quad.J[2] - self.quad.J[0]) / self.quad.J[1] * cs.horzcat(self.r[2], 0, self.r[0])
jac[12, 10:] = (self.quad.J[0] - self.quad.J[1]) / self.quad.J[2] * cs.horzcat(self.r[1], self.r[0], 0)
return cs.Function('J', [self.x, self.u], [jac])
def getFourierDcm(vars):
return C.vertcat(
[
C.horzcat([vars["e11"], vars["e12"], vars["e13"]]),
C.horzcat([vars["e21"], vars["e22"], vars["e23"]]),
C.horzcat([vars["e31"], vars["e32"], vars["e33"]]),
]
)
def to_dataset(self):
"""
Return a dataset with the data of x, y, and u
:rtype: DataSet
"""
dataset = DataSet(name=self.problem_name + '_dataset')
dataset.max_delta_t = (self.t_f - self.t_0) / self.finite_elements
dataset.plot_style = 'plot'
dataset.create_entry('x', size=len(self.x_names), names=self.x_names)
dataset.create_entry('y', size=len(self.y_names), names=self.y_names)
dataset.create_entry('u', size=len(self.u_names), names=self.u_names)
dataset.create_entry('theta_opt',
size=len(self.theta_opt_names),
names=self.theta_opt_names,
plot_style='step')
for entry in self.other_data:
size = self.other_data[entry]['values'][0][0].shape[0]
dataset.create_entry(
entry,
size=size,
names=[entry + '_' + str(i) for i in range(size)])
if self.degree_control > 1:
dataset.data['u']['plot_style'] = 'plot'
else:
dataset.data['u']['plot_style'] = 'step'
x_times = self.x_data['time'] + [[self.t_f]]
for el in range(self.finite_elements + 1):
time = horzcat(*x_times[el])
values = horzcat(*self.x_data['values'][el])
dataset.insert_data('x', time, values)
for el in range(self.finite_elements):
time_y = horzcat(*self.y_data['time'][el])
values_y = horzcat(*self.y_data['values'][el])
dataset.insert_data('y', time_y, values_y)
time_u = horzcat(*self.u_data['time'][el])
values_u = horzcat(*self.u_data['values'][el])
dataset.insert_data('u', time_u, values_u)
dataset.insert_data('theta_opt',
time=horzcat(*self.time_breakpoints[:-1]),
value=horzcat(*self.theta_opt))
for entry in self.other_data:
for el in range(self.finite_elements):
dataset.insert_data(
entry,
time=horzcat(*self.other_data[entry]['time'][el]),
value=horzcat(*self.other_data[entry]['values'][el]))
return dataset
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 rotation_quaternion(quat):
"""Returns a rotation matrix from a quaternion."""
#Rotation Matrix
rowX_vb = casadi.horzcat(1-2*(quat[2]**2+quat[3]**2),2*(quat[1]*quat[2]-quat[3]*quat[0]),2*(quat[1]*quat[3]+quat[2]*quat[0]))
rowY_vb = casadi.horzcat(2*(quat[1]*quat[2]+quat[3]*quat[0]),1-2*(quat[1]**2+quat[3]**2),2*(quat[2]*quat[3]-quat[1]*quat[0]))
rowZ_vb = casadi.horzcat(2*(quat[1]*quat[3]-quat[2]*quat[0]),2*(quat[2]*quat[3]+quat[1]*quat[0]),1-2*(quat[1]**2+quat[2]**2))
return casadi.vertcat(rowX_vb, \
rowY_vb, \
rowZ_vb)
def torque_derivative_driven(states: MX.sym, controls: MX.sym,
parameters: MX.sym, nlp,
implicit_dynamics: bool,
with_contact: bool) -> MX:
"""
Forward dynamics driven by joint torques, optional external forces can be declared.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
implicit_dynamics: bool
If the implicit dynamics should be used
with_contact: bool
If the dynamic with contact should be used
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
tau = DynamicsFunctions.get(nlp.states["tau"], states)
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls)
if implicit_dynamics:
ddq = DynamicsFunctions.get(nlp.states["qddot"], states)
dddq = DynamicsFunctions.get(nlp.controls["qdddot"], controls)
dxdt = MX(nlp.states.shape, 1)
dxdt[nlp.states["q"].index, :] = dq
dxdt[nlp.states["qdot"].index, :] = ddq
dxdt[nlp.states["qddot"].index, :] = dddq
dxdt[nlp.states["tau"].index, :] = dtau
else:
ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau,
with_contact)
dxdt = MX(nlp.states.shape, ddq.shape[1])
dxdt[nlp.states["q"].index, :] = horzcat(
*[dq for _ in range(ddq.shape[1])])
dxdt[nlp.states["qdot"].index, :] = ddq
dxdt[nlp.states["tau"].index, :] = horzcat(
*[dtau for _ in range(ddq.shape[1])])
return dxdt
def skew_mat(xyz):
'''
return the skew symmetrix matrix of a 3-vector
'''
x = xyz[0]
y = xyz[1]
z = xyz[2]
return C.vertcat([C.horzcat([ 0, -z, y]),
C.horzcat([ z, 0, -x]),
C.horzcat([-y, x, 0])])
def skew_mat(xyz):
'''
return the skew symmetrix matrix of a 3-vector
'''
x = xyz[0]
y = xyz[1]
z = xyz[2]
return C.vertcat(
[C.horzcat([0, -z, y]),
C.horzcat([z, 0, -x]),
C.horzcat([-y, x, 0])])
def Model_2 (param,x,*args):
# This model is used when the independent term is calculated
p = [] ; var = []
for i in range(param.rows()):
p = vertcat(p, param[i])
#for i in range(len(x)):
rows = args[0] # This argument corresponds to the amount of input data
var = horzcat(var,x)
var = horzcat(var, MX.ones(rows, 1))
return mtimes(var, p)
def minimize_markers_displacement(penalty, ocp, nlp, t, x, u, p, coordinates_system_idx=-1):
"""
Adds the objective that the specific markers displacement (difference between the position of the
markers at each neighbour frame)should be minimized.
:coordinates_system_idx: Index of the segment in which to project to displacement
"""
nq = nlp.mapping["q"].reduce.len
nb_rts = nlp.model.nbSegment()
markers_idx = PenaltyFunctionAbstract._check_and_fill_index(
penalty.index, nlp.model.nbMarkers(), "markers_idx"
)
PenaltyFunctionAbstract._add_to_casadi_func(nlp, "biorbd_markers", nlp.model.markers, nlp.q)
if coordinates_system_idx >= 0:
if coordinates_system_idx >= nb_rts:
raise RuntimeError(
f"coordinates_system_idx ({coordinates_system_idx}) cannot be higher than {nb_rts - 1}"
)
PenaltyFunctionAbstract._add_to_casadi_func(
nlp, f"globalJCS_{coordinates_system_idx}", nlp.model.globalJCS, nlp.q, coordinates_system_idx
)
for i in range(len(x) - 1):
q_0 = nlp.mapping["q"].expand.map(x[i][:nq])
q_1 = nlp.mapping["q"].expand.map(x[i + 1][:nq])
if coordinates_system_idx < 0:
jcs_0_T = nlp.CX.eye(4)
jcs_1_T = nlp.CX.eye(4)
elif coordinates_system_idx < nb_rts:
jcs_0 = nlp.casadi_func[f"globalJCS_{coordinates_system_idx}"](q_0)
jcs_0_T = vertcat(horzcat(jcs_0[:3, :3], -jcs_0[:3, :3] @ jcs_0[:3, 3]), horzcat(0, 0, 0, 1))
jcs_1 = nlp.casadi_func[f"globalJCS_{coordinates_system_idx}"](q_1)
jcs_1_T = vertcat(horzcat(jcs_1[:3, :3], -jcs_1[:3, :3] @ jcs_1[:3, 3]), horzcat(0, 0, 0, 1))
else:
raise RuntimeError(
f"Wrong choice of coordinates_system_idx. (Negative values refer to global coordinates system, "
f"positive values must be between 0 and {nb_rts})"
)
val = jcs_1_T @ vertcat(
nlp.casadi_func["biorbd_markers"](q_1)[:, markers_idx], nlp.CX.ones(1, markers_idx.shape[0])
) - jcs_0_T @ vertcat(
nlp.casadi_func["biorbd_markers"](q_0)[:, markers_idx], nlp.CX.ones(1, markers_idx.shape[0])
)
penalty.type.get_type().add_to_penalty(ocp, nlp, val[:3, :], penalty)
def _check_for_lineq(self):
g = []
for con in self.global_constraints:
lb, ub = con[1], con[2]
g = vertcat(g, con[0] - lb)
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k, _ in enumerate(lb):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = self.distr_problem.father.get_variables(child,
name,
spline=False,
symbolic=True,
substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = self.distr_problem.father.get_variables(
child,
name,
spline=False,
symbolic=True,
substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_global.values():
return False, None, None
par = struct_symMX(self.par_global_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.name() for s in symvar(b)]
dep_A = [s.name() for s in symvar(b)]
for name, sym in self.par_global.items():
if sym.name() in dep_b:
b = substitute(b, sym, par[name])
if sym.name() in dep_A:
A = substitute(A, sym, par[name])
A = Function('A', [par], [A]).expand()
b = Function('b', [par], [b]).expand()
return True, A, b
def fwd(x_all, u_all, k_all, K_all, alpha, F, dt, state, control):
n_sim = len(u_all[:])
xk = x_all[0]
x_new = [xk]
u_new = []
for k in range(n_sim):
uk = u_all[k] + alpha * k_all[k] + ca.mul(K_all[k], xk - x_all[k])
u_new.append(uk)
[xk_next] = F([xk, uk, dt])
x_new.append(xk_next)
xk = xk_next
x_new = state.repeated(ca.horzcat(x_new))
u_new = control.repeated(ca.horzcat(u_new))
return [x_new, u_new]
def solve(theta_value):
global x0
solution = solver(lbx=lbX, ubx=ubX, lbg=lbg, ubg=ubg, p=theta_value, x0=x0)
if solver.stats()["return_status"] != "Solve_Succeeded":
raise Exception("Solve failed with status {}".format(
solver.stats()["return_status"]))
x0 = solution["x"]
Q_res = ca.reshape(x0[:Q.size1() * Q.size2()], Q.size1(), Q.size2())
H_res = ca.reshape(x0[Q.size1() * Q.size2():], H.size1(), H.size2())
d = {}
d["Q_0"] = np.array(Q_left).flatten()
for i in range(n_level_nodes):
d[f"Q_{i + 1}"] = np.array(ca.horzcat(Q0[i], Q_res[i, :])).flatten()
d[f"H_{i + 1}"] = np.array(ca.horzcat(H0[i], H_res[i, :])).flatten()
results[theta_value] = d
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 get_x_and_u_at_idx(_penalty, _idx):
if _penalty.transition:
ocp = self.ocp
_x = vertcat(ocp.nlp[_penalty.phase_pre_idx].X[-1],
ocp.nlp[_penalty.phase_post_idx].X[0][:, 0])
_u = vertcat(ocp.nlp[_penalty.phase_pre_idx].U[-1],
ocp.nlp[_penalty.phase_post_idx].U[0])
elif _penalty.multinode_constraint:
ocp = self.ocp
_x = vertcat(
ocp.nlp[_penalty.phase_first_idx].X[_penalty.node_idx[0]],
ocp.nlp[_penalty.phase_second_idx].X[_penalty.node_idx[1]]
[:, 0],
)
# Make an exception to the fact that U is not available for the last node
mod_u0 = 1 if _penalty.first_node == Node.END else 0
mod_u1 = 1 if _penalty.second_node == Node.END else 0
_u = vertcat(
ocp.nlp[_penalty.phase_first_idx].U[_penalty.node_idx[0] -
mod_u0],
ocp.nlp[_penalty.phase_second_idx].U[_penalty.node_idx[1] -
mod_u1],
)
elif _penalty.integrate:
_x = nlp.X[_idx]
_u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else []
else:
_x = nlp.X[_idx][:, 0]
_u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else []
if _penalty.derivative or _penalty.explicit_derivative:
_x = horzcat(_x, nlp.X[_idx + 1][:, 0])
_u = horzcat(
_u, nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else [])
if _penalty.integration_rule == IntegralApproximation.TRAPEZOIDAL:
_x = horzcat(_x, nlp.X[_idx + 1][:, 0])
if nlp.control_type == ControlType.LINEAR_CONTINUOUS:
_u = horzcat(
_u,
nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else [])
if _penalty.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL:
if nlp.control_type == ControlType.LINEAR_CONTINUOUS:
_u = horzcat(
_u,
nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else [])
return _x, _u
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 getDcm(ocp,k):
m11 = ocp.lookup('e11',timestep=k)
m12 = ocp.lookup('e12',timestep=k)
m13 = ocp.lookup('e13',timestep=k)
m21 = ocp.lookup('e21',timestep=k)
m22 = ocp.lookup('e22',timestep=k)
m23 = ocp.lookup('e23',timestep=k)
m31 = ocp.lookup('e31',timestep=k)
m32 = ocp.lookup('e32',timestep=k)
m33 = ocp.lookup('e33',timestep=k)
return C.vertcat([C.horzcat([m11,m12,m13]),
C.horzcat([m21,m22,m23]),
C.horzcat([m31,m32,m33])])
def add_variables(self, stage, opti):
self.add_time_variables(stage, opti)
# We are creating variables in a special order such that the resulting constraint Jacobian
# is block-sparse
x = opti.variable(stage.nx)
self.X.append(x)
self.add_variables_V(stage, opti)
for k in range(self.N):
self.U.append(opti.variable(stage.nu))
xr = []
zr = []
Xc = []
Zc = []
for i in range(self.M):
xc = opti.variable(stage.nx, self.degree)
xr.append(xc)
zc = opti.variable(stage.nz, self.degree)
zr.append(zc)
x0 = x if i == 0 else opti.variable(stage.nx)
Xc.append(horzcat(x0, xc))
Zc.append(zc)
self.xr.append(xr)
self.zr.append(zr)
self.Xc.append(Xc)
self.Zc.append(Zc)
x = opti.variable(stage.nx)
self.X.append(x)
self.add_variables_V_control(stage, opti, k)
def __init__(self, ny, order):
self.order = order
self.ny = ny
self.y = cas.ssym("y", ny, self.order + 1)
self.x = dict()
self._nx = 0
self._dy = cas.horzcat([self.y[:, 1:], cas.SXMatrix.zeros(ny, 1)])
def create_system_set(A, B, v, Hv, hv, full=True):
nx = A.shape[0]
delta_bounds = {}
delta = []
AB = horzcat(A, B)
if full:
AB_delta = SX(np.zeros(AB.shape))
for row in range(AB.shape[0]):
for col in range(AB.shape[1]):
if AB[row, col].is_constant():
AB_delta[row, col] = AB[row, col]
else:
delta_str = 'd_' + str(row) + str(col)
# print("Solving for: " + delta_str)
delta.append(SX.sym(delta_str))
AB_delta[row, col] = delta[-1]
delta_bounds[delta_str] = delta_range(AB[row, col], v, Hv, hv)
eval_func = Function("eval_func", delta, [AB_delta])
else:
for i in range(v.shape[0]):
delta_str = 'd_' + str(i)
delta_bounds[delta_str] = delta_range(v[i], v, Hv, hv)
delta.append(v[i])
eval_func = Function("eval_func", delta, [AB])
A_set = []
B_set = []
for product in itertools.product(*delta_bounds.values()): # creating maximum difference
AB_extreme = np.asarray(eval_func(*product))
A_set.append(AB_extreme[:, :nx])
B_set.append(AB_extreme[:, nx:])
return np.stack(A_set), np.stack(B_set)
def set_augmented_discrete_system(self):
"""
Pendulum dynamics with integral action.
:param x: state
:type x: casadi.DM
:param u: control input
:type u: casadi.DM
"""
# Grab equilibrium dynamics
Ad_eq = self.Ad(self.x_eq, self.u_eq, self.w)
Bd_eq = self.Bd(self.x_eq, self.u_eq, self.w)
Bw_eq = self.Bw(self.x_eq, self.u_eq, self.w)
# Instantiate augmented system
self.Ad_i = ca.DM.zeros(5,5)
self.Bd_i = ca.DM.zeros(5,1)
self.Bw_i = ca.DM.zeros(5,1)
self.Cd_i = ca.DM.zeros(1,5)
self.R_i = ca.DM.zeros(5,1)
# Populate matrices
self.Ad_i = ca.diagcat(Ad_eq, ca.DM(1))
self.Ad_i[4,0:4] = -self.dt * self.Cd_eq
self.Bd_i = ca.vertcat(Bd_eq, ca.DM(0))
self.Bw_i = ca.vertcat(Bw_eq, ca.DM(0))
self.Cd_i = ca.horzcat(self.Cd_eq, ca.DM(1))
self.R_i[4,0] = self.dt
def simulate_observed_trajectory(model, x_all):
z_all = []
for xk in x_all[:]:
[zk] = model.hn([xk])
z_all.append(zk)
z_all = model.z.repeated(ca.horzcat(z_all))
return z_all
def torque_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact: bool) -> MX:
"""
Forward dynamics driven by joint torques, optional external forces can be declared.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
with_contact: bool
If the dynamic with contact should be used
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
tau = DynamicsFunctions.get(nlp.controls["tau"], controls)
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact)
dq = horzcat(*[dq for _ in range(ddq.shape[1])])
return vertcat(dq, ddq)
def shift_movement(T, t0, x0, u, f):
f_value = f(x0, u[:, 0])
st = x0 + T * f_value
t = t0 + T
u_end = ca.horzcat(u[:, 1:], u[:, -1])
return t, st, u_end.T
def getDcm(ocp,k,prefix='e'):
m11 = ocp.lookup(prefix+'11',timestep=k)
m12 = ocp.lookup(prefix+'12',timestep=k)
m13 = ocp.lookup(prefix+'13',timestep=k)
m21 = ocp.lookup(prefix+'21',timestep=k)
m22 = ocp.lookup(prefix+'22',timestep=k)
m23 = ocp.lookup(prefix+'23',timestep=k)
m31 = ocp.lookup(prefix+'31',timestep=k)
m32 = ocp.lookup(prefix+'32',timestep=k)
m33 = ocp.lookup(prefix+'33',timestep=k)
return C.vertcat([C.horzcat([m11,m12,m13]),
C.horzcat([m21,m22,m23]),
C.horzcat([m31,m32,m33])])
def get_forward_model_casadi(self, linearize_mu=False):
""" Return a symbolic casadi function representing predictive mean/variance
"""
return lambda x_new, u_new: self.predict_casadi_symbolic(
horzcat(x_new, u_new), linearize_mu)
def contact_forces(nlp: NonLinearProgram, q, qdot, tau):
"""
Easy accessor for the contact forces in contact dynamics
Parameters
----------
nlp: NonLinearProgram
The phase of the program
q: Union[MX, SX]
The value of q from "get"
qdot: Union[MX, SX]
The value of qdot from "get"
tau: Union[MX, SX]
The value of tau from "get"
Returns
-------
The contact forces
"""
cs = nlp.model.getConstraints()
if nlp.external_forces:
all_cs = MX()
for i, f_ext in enumerate(nlp.external_forces):
nlp.model.ForwardDynamicsConstraintsDirect(
q, qdot, tau, cs, f_ext).to_mx()
all_cs = horzcat(all_cs, cs.getForce().to_mx())
return all_cs
else:
nlp.model.ForwardDynamicsConstraintsDirect(q, qdot, tau,
cs).to_mx()
return cs.getForce().to_mx()
def roll(a, shift, axis: int = None):
"""
Roll array elements along a given axis.
Elements that roll beyond the last position are re-introduced at the first.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.roll.html
Parameters
----------
a : array_like
Input array.
shift : int
The number of places by which elements are shifted.
Returns
-------
res : ndarray
Output array, with the same shape as a.
"""
if not is_casadi_type(a):
return _onp.roll(a, shift, axis=axis)
else: # TODO add some checking to make sure shift < len(a), or shift is modulo'd down by len(a).
# assert shift < a.shape[axis]
if 1 in a.shape and axis == 0:
return _cas.vertcat(a[-shift, :], a[:-shift, :])
elif axis == 0:
return _cas.vertcat(a.T[:, -shift], a.T[:, :-shift]).T
elif axis == 1:
return _cas.horzcat(a[:, -shift], a[:, :-shift])
elif axis is None:
return roll(a, shift=shift, axis=0)
else:
raise Exception("CasADi types can only be up to 2D, so `axis` must be None, 0, or 1.")
def torque_driven(
states: MX.sym,
controls: MX.sym,
parameters: MX.sym,
nlp,
with_contact: bool,
implicit_dynamics: bool,
fatigue: FatigueList,
) -> MX:
"""
Forward dynamics driven by joint torques, optional external forces can be declared.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
with_contact: bool
If the dynamic with contact should be used
implicit_dynamics: bool
If the implicit dynamic should be used
fatigue : FatigueList
A list of fatigue elements
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
if implicit_dynamics:
dxdt = MX(nlp.states.shape, 1)
dxdt[nlp.states["q"].index, :] = dq
dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(
nlp.controls["qddot"], controls)
else:
tau = DynamicsFunctions.__get_fatigable_tau(
nlp, states, controls, fatigue)
ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau,
with_contact)
dxdt = MX(nlp.states.shape, ddq.shape[1])
dxdt[nlp.states["q"].index, :] = horzcat(
*[dq for _ in range(ddq.shape[1])])
dxdt[nlp.states["qdot"].index, :] = ddq
if fatigue is not None and "tau" in fatigue:
dxdt = fatigue["tau"].dynamics(dxdt, nlp, states, controls)
return dxdt
def __init__(self, ny, order):
self.order = order
self.ny = ny
self.y = cas.ssym("y", ny, self.order + 1)
self.x = dict()
self._nx = 0
self._dy = cas.horzcat([self.y[:, 1:], cas.SXMatrix.zeros(ny, 1)])
def skew_symmetric(v):
"""
Computes the skew-symmetric matrix of a 3D vector (PAMPC version)
:param v: 3D numpy vector or CasADi MX
:return: the corresponding skew-symmetric matrix of v with the same data type as v
"""
if isinstance(v, np.ndarray):
return np.array([[0, -v[0], -v[1], -v[2]], [v[0], 0, v[2], -v[1]],
[v[1], -v[2], 0, v[0]], [v[2], v[1], -v[0], 0]])
return cs.vertcat(cs.horzcat(0, -v[0], -v[1], -v[2]),
cs.horzcat(v[0], 0, v[2], -v[1]),
cs.horzcat(v[1], -v[2], 0, v[0]),
cs.horzcat(v[2], v[1], -v[0], 0))
def expandInput(self, u):
'''Return input at collocation points given the input u on collocation intervals.
'''
n = self.numIntervals
assert u.shape[1] == n
return cs.horzcat(*[cs.repmat(u[:, k], 1, len(self._collocationGroups[k]) - 1) for k in range(n)])
def compute_penalty_values(t, x, u, p, penalty, dt):
if len(x.shape) < 2:
x = x.reshape((-1, 1))
if isinstance(dt, Function):
# The division is to account for the steps in the integration. The else is for Mayer term
dt = dt(p)
dt = dt / (x.shape[1] - 1) if x.shape[1] > 1 else dt
if not isinstance(penalty.dt, (float, int)):
if dt.shape[0] > 1:
dt = dt[penalty.phase]
_target = (penalty.target[..., penalty.node_idx.index(t)]
if penalty.target is not None and isinstance(t, int)
else [])
out = []
if penalty.transition:
out.append(
penalty.weighted_function_non_threaded(
x.reshape((-1, 1)), u.reshape((-1, 1)), p,
penalty.weight, _target, dt))
elif penalty.derivative or penalty.explicit_derivative:
out.append(
penalty.weighted_function_non_threaded(
x[:, [0, -1]], u, p, penalty.weight, _target, dt))
else:
out.append(
penalty.weighted_function_non_threaded(
x, u, p, penalty.weight, _target, dt))
return sum1(horzcat(*out))
def inner_phase_continuity(ocp):
"""
Add continuity constraints between each nodes of a phase.
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
"""
# Dynamics must be sound within phases
for i, nlp in enumerate(ocp.nlp):
penalty = Constraint([])
penalty.name = f"CONTINUITY {i}"
penalty.list_index = -1
ConstraintFunction.clear_penalty(ocp, None, penalty)
# Loop over shooting nodes or use parallelization
if ocp.n_threads > 1:
end_nodes = nlp.par_dynamics(horzcat(*nlp.X[:-1]),
horzcat(*nlp.U),
nlp.parameters.cx)[0]
val = horzcat(*nlp.X[1:]) - end_nodes
ConstraintFunction.add_to_penalty(
ocp, None, val.reshape((nlp.states.shape * nlp.ns, 1)),
penalty)
else:
for k in range(nlp.ns):
# Create an evaluation node
if (isinstance(nlp.ode_solver, OdeSolver.RK4)
or isinstance(nlp.ode_solver, OdeSolver.RK8)
or isinstance(nlp.ode_solver, OdeSolver.IRK)):
if nlp.control_type == ControlType.CONSTANT:
u = nlp.U[k]
elif nlp.control_type == ControlType.LINEAR_CONTINUOUS:
u = horzcat(nlp.U[k], nlp.U[k + 1])
else:
raise NotImplementedError(
f"Dynamics with {nlp.control_type} is not implemented yet"
)
end_node = nlp.dynamics[k](
x0=nlp.X[k], p=u, params=nlp.parameters.cx)["xf"]
else:
end_node = nlp.dynamics[k](x0=nlp.X[k],
p=nlp.U[k])["xf"]
# Save continuity constraints
val = end_node - nlp.X[k + 1]
ConstraintFunction.add_to_penalty(ocp, None, val, penalty)
def muscles_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact: bool) -> MX:
"""
Forward dynamics driven by muscle.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
with_contact: bool
If the dynamic with contact should be used
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
residual_tau = DynamicsFunctions.get(nlp.controls["tau"], controls) if "tau" in nlp.controls else None
mus_act_nlp, mus_act = (nlp.states, states) if "muscles" in nlp.states else (nlp.controls, controls)
mus_activations = DynamicsFunctions.get(mus_act_nlp["muscles"], mus_act)
muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations)
tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact)
dq = horzcat(*[dq for _ in range(ddq.shape[1])])
dxdt = vertcat(dq, ddq)
has_excitation = True if "muscles" in nlp.states else False
if has_excitation:
mus_excitations = DynamicsFunctions.get(nlp.controls["muscles"], controls)
dmus = DynamicsFunctions.compute_muscle_dot(nlp, mus_excitations)
dmus = horzcat(*[dmus for _ in range(ddq.shape[1])])
dxdt = vertcat(dxdt, dmus)
return dxdt
def simulate_eb_trajectory(model, u_all):
ebk = model.eb0
eb_all = [ebk]
for uk in u_all[:]:
[ebk_next] = model.EBF([ebk, uk])
eb_all.append(ebk_next)
ebk = ebk_next
eb_all = model.eb.repeated(ca.horzcat(eb_all))
return eb_all
def simulate_trajectory(model, u_all):
xk = model.x0.cat
x_all = [xk]
for uk in u_all[:]:
[xk_next] = model.Fn([xk, uk])
x_all.append(xk_next)
xk = xk_next
x_all = model.x.repeated(ca.horzcat(x_all))
return x_all
def filter_observed_trajectory(model, z_all, u_all):
n = len(u_all[:])
bk = model.b0
b_all = [bk]
for k in range(n):
[bk_next] = model.EKF([bk, u_all[k], z_all[k+1]])
b_all.append(bk_next)
bk = bk_next
b_all = model.b.repeated(ca.horzcat(b_all))
return b_all
def _check_for_lineq(self):
g = []
for con in self.constraints:
lb, ub = con[1], con[2]
g = vertcat([g, con[0] - lb])
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k in range(len(lb)):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_i.values():
return False, None, None
par = struct_symMX(self.par_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.getName() for s in symvar(b)]
dep_A = [s.getName() for s in symvar(b)]
for name, sym in self.par_i.items():
if sym.getName() in dep_b:
b = substitute(b, sym, par[name])
if sym.getName() in dep_A:
A = substitute(A, sym, par[name])
A = MXFunction('A', [par], [A]).expand()
b = MXFunction('b', [par], [b]).expand()
return True, A, b
def _check_for_lineq(self):
g = []
for con in self.global_constraints:
lb, ub = con[1], con[2]
g = vertcat(g, con[0] - lb)
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k, _ in enumerate(lb):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_global.values():
return False, None, None
par = struct_symMX(self.par_global_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.name() for s in symvar(b)]
dep_A = [s.name() for s in symvar(b)]
for name, sym in self.par_global.items():
if sym.name() in dep_b:
b = substitute(b, sym, par[name])
if sym.name() in dep_A:
A = substitute(A, sym, par[name])
A = Function('A', [par], [A]).expand()
b = Function('b', [par], [b]).expand()
return True, A, b
def set_path(self, paths):
"""The path is defined as the convex combination of the paths in paths.
Args:
paths (list of lists of SXMatrix): The path is taken as the
convex combination of the paths in paths.
Example:
The path is defined as the convex combination of
(s, 0.5*s) and (2, 2*s):
>>> P.set_path([(P.s[0], 0.5 * P.s[0]), [P.s[0], 2 * P.s[0]]])
"""
l = len(paths)
self.h = cas.ssym("h", l, self.sys.order + 1)
self.path[:, 0] = np.sum(cas.SXMatrix(paths) * cas.horzcat([self.h[:, 0]] * len(paths[0])), axis=0)
dot_s = cas.vertcat([self.s[1:], 0])
dot_h = cas.horzcat([self.h[:, 1:], cas.SXMatrix.zeros(l, 1)])
for i in range(1, self.sys.order + 1): # Chainrule
self.path[:, i] = (cas.mul(cas.jacobian(self.path[:, i - 1], self.s), dot_s) +
sum([cas.mul(cas.jacobian(self.path[:, i - 1], self.h[j, :]), dot_h[j, :].trans()) for j in range(l)]) * self.s[1])
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]),
C.horzcat([ 0, conf['j2'], 0]),
C.horzcat([conf['j31'], 0, conf['j3']])])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
C.horzcat([mm10,mm11,mm12]),
C.horzcat([mm20,mm21,mm22])])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def invariantErrs():
dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
, C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
, C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
] ).trans()
err = C.mul(dcm.trans(), dcm)
dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('c'),dae.output('cdot'),dcmErr]
)
f.setOption('name','invariant errors')
f.init()
return f
def _make_constraints(self):
"""
Parse the constraints and put them in the correct format
"""
N = self.options['N']
con = self._ode(self.prob['vars'])
lb = np.alen(con) * [0]
ub = np.alen(con) * [0]
S = self.prob['s']
b = self.prob['vars']
path, bs = self._make_path()[0:2]
sbs = cas.vertcat([self.s[0], bs])
Sb = cas.horzcat([S, b]).T
for f in self.constraints:
F = cas.substitute(f[0], self.sys.y, path)
if f[3] is None:
F = cas.vertcat([cas.substitute(F, sbs, Sb[:, j])
for j in xrange(N + 1)])
Flb = f[1](S) if hasattr(f[1], '__call__') else [f[1]] * len(S)
Fub = f[2](S) if hasattr(f[2], '__call__') else [f[2]] * len(S)
con.append(F)
lb.extend(Flb)
ub.extend(Fub)
else:
F = cas.vertcat([cas.substitute(F, sbs, Sb[:, j])
for j in f[3]])
con.append(F)
lb.extend([f[1]])
ub.extend([f[2]])
if self.options['bc']:
con.append(b[0, 0])
lb.append(0)
ub.append(1e-50)
con.append(b[-1, 0])
lb.append(0)
ub.append(1e-50)
self.prob['con'] = [con, lb, ub]
def run_simulation(self, \
x0 = None, tsim = None, usim = None, psim = None, method = "rk"):
r'''
:param x0: initial value for the states
:math:`x_0 \in \mathbb{R}^{n_x}`
:type x0: list, numpy,ndarray, casadi.DMatrix
:param tsim: optional, switching time points for the controls
:math:`t_{sim} \in \mathbb{R}^{L}` to be used for the
simulation
:type tsim: list, numpy,ndarray, casadi.DMatrix
:param usim: optional, control values
:math:`u_{sim} \in \mathbb{R}^{n_u \times L}`
to be used for the simulation
:type usim: list, numpy,ndarray, casadi.DMatrix
:param psim: optional, parameter set
:math:`p_{sim} \in \mathbb{R}^{n_p}`
to be used for the simulation
:type psim: list, numpy,ndarray, casadi.DMatrix
:param method: optional, CasADi integrator to be used for the
simulation
:type method: str
This function performs a simulation of the system for a given
parameter set :math:`p_{sim}`, starting from a user-provided initial
value for the states :math:`x_0`. If the argument ``psim`` is not
specified, the estimated parameter set :math:`\hat{p}` is used.
For this, a parameter
estimation using :func:`run_parameter_estimation()` has to be
done beforehand, of course.
By default, the switching time points for
the controls :math:`t_u` and the corresponding controls
:math:`u_N` will be used for simulation. If desired, other time points
:math:`t_{sim}` and corresponding controls :math:`u_{sim}`
can be passed to the function.
For the moment, the function can only be used for systems of type
:class:`pecas.systems.ExplODE`.
'''
intro.pecas_intro()
print('\n' + 27 * '-' + \
' PECas system simulation ' + 26 * '-')
print('\nPerforming system simulation, this might take some time ...')
if not type(self.pesetup.system) is systems.ExplODE:
raise NotImplementedError("Until now, this function can only " + \
"be used for systems of type ExplODE.")
if x0 == None:
raise ValueError("You have to provide an initial value x0 " + \
"to run the simulation.")
x0 = np.squeeze(np.asarray(x0))
if np.atleast_1d(x0).shape[0] != self.pesetup.nx:
raise ValueError("Wrong dimension for initial value x0.")
if tsim == None:
tsim = self.pesetup.tu
if usim == None:
usim = self.pesetup.uN
if psim == None:
try:
psim = self.phat
except AttributeError:
errmsg = '''
You have to either perform a parameter estimation beforehand to obtain a
parameter set that can be used for simulation, or you have to provide a
parameter set in the argument psim.
'''
raise AttributeError(errmsg)
else:
if not np.atleast_1d(np.squeeze(psim)).shape[0] == self.pesetup.np:
raise ValueError("Wrong dimension for parameter set psim.")
fp = ca.MXFunction("fp", \
[self.pesetup.system.t, self.pesetup.system.u, \
self.pesetup.system.x, self.pesetup.system.eps_e, \
self.pesetup.system.eps_u, self.pesetup.system.p], \
[self.pesetup.system.f])
fpeval = fp([\
self.pesetup.system.t, self.pesetup.system.u, \
self.pesetup.system.x, np.zeros(self.pesetup.neps_e), \
np.zeros(self.pesetup.neps_u), psim])[0]
fsim = ca.MXFunction("fsim", \
ca.daeIn(t = self.pesetup.system.t, \
x = self.pesetup.system.x, \
p = self.pesetup.system.u), \
ca.daeOut(ode = fpeval))
Xsim = []
Xsim.append(x0)
u0 = ca.DMatrix()
for k,e in enumerate(tsim[:-1]):
try:
integrator = ca.Integrator("integrator", method, \
fsim, {"t0": e, "tf": tsim[k+1]})
except RuntimeError as err:
errmsg = '''
It seems like you want to use an integration method that is not currently
supported by CasADi. Please refer to the CasADi documentation for a list
of supported integrators, or use the default RK4-method by not setting the
method-argument of the function.
'''
raise RuntimeError(errmsg)
if not self.pesetup.nu == 0:
u0 = usim[:,k]
Xk_end = itemgetter('xf')(integrator({'x0':x0,'p':u0}))
Xsim.append(Xk_end)
x0 = Xk_end
self.Xsim = ca.horzcat(Xsim)
print( \
'''System simulation finished.''')
def register_three_points_casadi(a1,a2,a3, b1,b2,b3): assert a1.shape==(3,1) assert a2.shape==(3,1) assert a3.shape==(3,1) assert b1.shape==(3,1) assert b2.shape==(3,1) assert b3.shape==(3,1) A = casadi.horzcat((a1,a2,a3)) # place a1,a2,a3 into columns of A B = casadi.horzcat((b1,b2,b3)) # place b1,b2,b3 into columns of B temp1a = normalize_casadi(a2 - a1) temp2a = normalize_casadi(a3 - a1) temp1b = normalize_casadi(b2 - b1) temp2b = normalize_casadi(b3 - b1) na = normalize_casadi(cross_casadi(temp1a, temp2a)) nb = normalize_casadi(cross_casadi(temp1b, temp2b)) # [temp1a na temp3a] is a rotation matrix. temp3a = cross_casadi(temp1a, na) temp3b = cross_casadi(temp1b, nb) # Find the rotation that aligns the normals with the z-axis. # Be careful to preserve handedness. Ra = casadi.horzcat((temp3a, temp1a, na)).T Rb = casadi.horzcat((temp3b, temp1b, nb)).T Atilde = dot(Ra,A) # 3x3 Btilde = dot(Rb,B) # Subtract off the means of the points. mua = Atilde.mean(axis=1) # means of the rows mub = Btilde.mean(axis=1) # means of the rows AHat = Atilde - mua.reshape((3,1)) # make mua a column, broadcast for subtraction BHat = Btilde - mub.reshape((3,1)) # make mub a column, broadcast for subtraction # Compute the x-y plane rotation that aligns the points in least squared sense. aRow1 = AHat[0,:] aRow2 = AHat[1,:] bRow1 = BHat[0,:] bRow2 = BHat[1,:] g = (aRow1*bRow1 + aRow2*bRow2).sum() h = (aRow1*bRow2 - aRow2*bRow1).sum() temp = numpy.sqrt(g*g + h*h) g = g/temp # now, g = cos(theta) where theta is the optimal rotation amount. h = h/temp # similarly, h = sin(theta) # Go ahead and format as a full 3D rotation matrix. #Rxy = [g -h 0; h g 0; 0 0 1]; Rxy = casadi.SXMatrix(3,3) Rxy[0,0] = g Rxy[0,1] = -h Rxy[1,0] = h Rxy[1,1] = g Rxy[2,2] = 1 # Combine the transformations to the the global R, t we're looking for. #R = Rb.T * Rxy * Ra mul = casadi.mul R = mul(mul(Rb.T, Rxy), Ra) #t = -Rb.T * Rxy * mua + Rb.T * mub t = mul(-Rb.T, mul(Rxy, mua)) + mul(Rb.T, mub) return R,t
def horzcat(inputlist):
return ca.horzcat(inputlist)
def getFourierDcm(vars):
return C.vertcat([C.horzcat([vars['e11'], vars['e12'], vars['e13']]),
C.horzcat([vars['e21'], vars['e22'], vars['e23']]),
C.horzcat([vars['e31'], vars['e32'], vars['e33']])])
def crosswindModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "r"
, "dr"
, 'ddr'
, "aileron"
, "elevator"
, "rudder"
, "flaps"
] )
dae.addU( [ "daileron"
, "delevator"
, "drudder"
, "dflaps"
, 'dddr'
] )
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# get mass matrix, rhs
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in \
['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z',
'e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33',
'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z',
'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag'
])
dae.addU( [ 'daileron'
, 'delevator'
, 'drudder'
, 'dflaps'
, 'dprop_drag'
] )
dae.addP( ['r','w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] )
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul( dae['R_n2b'], v_bw_n )
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2, t3) = \
aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
# if we are running a homotopy,
# add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1*gamma_homotopy + dae.addZ('f1_homotopy')*(1 - gamma_homotopy)
f2 = f2*gamma_homotopy + dae.addZ('f2_homotopy')*(1 - gamma_homotopy)
f3 = f3*gamma_homotopy + dae.addZ('f3_homotopy')*(1 - gamma_homotopy)
t1 = t1*gamma_homotopy + dae.addZ('t1_homotopy')*(1 - gamma_homotopy)
t2 = t2*gamma_homotopy + dae.addZ('t2_homotopy')*(1 - gamma_homotopy)
t3 = t3*gamma_homotopy + dae.addZ('t3_homotopy')*(1 - gamma_homotopy)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual( [ode, alg] )
return dae
def carouselModel(conf,nSteps=None,extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
# dae.addZ( [ "dddelta"
# , "ddx"
# , "ddy"
# , "ddz"
# , "dw1"
# , "dw2"
# , "dw3"
# , "nu"
# ] )
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w1"
, "w2"
, "w3"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
] )
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta'],
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta']),
dae.ddt('sin_delta') - dae['cos_delta']*dae['ddelta']])
else:
raise ValueErorr('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "motor_torque"
, 'ddr'
] )
# add wind parameter if wind shear is in configuration
if 'z0' in conf and 'zt_roughness' in conf:
dae.addP( ['w0'] )
dae['RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron(deg)'] = dae['aileron']*180/C.pi
dae['elevator(deg)'] = dae['elevator']*180/C.pi
dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
dae['motor power'] = dae['motor_torque']*dae['ddelta']
dae['tether tension'] = dae['r']*dae['nu']
dae['winch power'] = -dae['tether tension']*dae['dr']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# line angle
dae['cos(line angle)'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
if nSteps is not None:
dae.addP('endTime')
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def orthonormalizeDcm(m):
## OGRE (www.ogre3d.org) is made available under the MIT License.
##
## Copyright (c) 2000-2009 Torus Knot Software Ltd
##
## Permission is hereby granted, free of charge, to any person obtaining a copy
## of this software and associated documentation files (the "Software"), to deal
## in the Software without restriction, including without limitation the rights
## to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
## copies of the Software, and to permit persons to whom the Software is
## furnished to do so, subject to the following conditions:
##
## The above copyright notice and this permission notice shall be included in
## all copies or substantial portions of the Software.
##
## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
## AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
## OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
## THE SOFTWARE.
# Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
# M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
#
# q0 = m0/|m0|
# q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
# q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
#
# where |V| indicates length of vector V and A*B indicates dot
# product of vectors A and B.
m00 = m[0,0]
m01 = m[0,1]
m02 = m[0,2]
m10 = m[1,0]
m11 = m[1,1]
m12 = m[1,2]
m20 = m[2,0]
m21 = m[2,1]
m22 = m[2,2]
# compute q0
fInvLength = 1.0/C.sqrt(m00*m00 + m10*m10 + m20*m20)
m00 *= fInvLength
m10 *= fInvLength
m20 *= fInvLength
# compute q1
fDot0 = m00*m01 + m10*m11 + m20*m21
m01 -= fDot0*m00
m11 -= fDot0*m10
m21 -= fDot0*m20
fInvLength = 1.0/C.sqrt(m01*m01 + m11*m11 + m21*m21)
m01 *= fInvLength
m11 *= fInvLength
m21 *= fInvLength
# compute q2
fDot1 = m01*m02 + m11*m12 + m21*m22
fDot0 = m00*m02 + m10*m12 + m20*m22
m02 -= fDot0*m00 + fDot1*m01
m12 -= fDot0*m10 + fDot1*m11
m22 -= fDot0*m20 + fDot1*m21
fInvLength = 1.0/C.sqrt(m02*m02 + m12*m12 + m22*m22)
m02 *= fInvLength
m12 *= fInvLength
m22 *= fInvLength
return C.vertcat([C.horzcat([m00,m01,m02]),
C.horzcat([m10,m11,m12]),
C.horzcat([m20,m21,m22])])
def __init__(self, system = None, \
tu = None, uN = None, \
ty = None, yN = None,
pinit = None, \
xinit = None, \
scheme = "radau", \
order = 3):
self.tstart_setup = time.time()
SetupsBaseClass.__init__(self)
if not type(system) is systems.ExplODE:
raise TypeError("Setup-method " + self.__class__.__name__ + \
" not allowed for system of type " + str(type(system)) + ".")
self.system = system
# Dimensions
self.nx = system.x.shape[0]
self.nu = system.u.shape[0]
self.np = system.p.shape[0]
self.neps_e = system.eps_e.shape[0]
self.neps_u = system.eps_u.shape[0]
self.nphi = system.phi.shape[0]
if np.atleast_2d(tu).shape[0] == 1:
self.tu = np.asarray(tu)
elif np.atleast_2d(tu).shape[1] == 1:
self.tu = np.squeeze(np.atleast_2d(tu).T)
else:
raise ValueError("Invalid dimension for argument tu.")
if ty == None:
self.ty = self.tu
elif np.atleast_2d(ty).shape[0] == 1:
self.ty = np.asarray(ty)
elif np.atleast_2d(ty).shape[1] == 1:
self.ty = np.squeeze(np.atleast_2d(ty).T)
else:
raise ValueError("Invalid dimension for argument ty.")
self.nsteps = self.tu.shape[0] - 1
self.scheme = scheme
self.order = order
self.tauroot = ca.collocationPoints(order, scheme)
# Degree of interpolating polynomial
self.ntauroot = len(self.tauroot) - 1
# Define the optimization variables
self.P = ca.MX.sym("P", self.np)
self.X = ca.MX.sym("X", (self.nx * (self.ntauroot+1)), self.nsteps)
self.XF = ca.MX.sym("XF", self.nx)
self.V = ca.MX.sym("V", self.nphi, self.nsteps+1)
if self.neps_e != 0:
self.EPS_E = ca.MX.sym("EPS_E", \
(self.neps_e * self.ntauroot), self.nsteps)
else:
self.EPS_E = ca.DMatrix(0, self.nsteps)
if self.neps_u != 0:
self.EPS_U = ca.MX.sym("EPS_U", \
(self.neps_u * self.ntauroot), self.nsteps)
else:
self.EPS_U = ca.DMatrix(0, self.nsteps)
# Define bounds and initial values
self.check_and_set_initials( \
uN = uN, \
pinit = pinit, \
xinit = xinit)
# Set tp the collocation coefficients
# Coefficients of the collocation equation
self.C = np.zeros((self.ntauroot + 1, self.ntauroot + 1))
# Coefficients of the continuity equation
self.D = np.zeros(self.ntauroot + 1)
# Dimensionless time inside one control interval
tau = ca.SX.sym("tau")
# Construct the matrix T that contains all collocation time points
self.T = np.zeros((self.nsteps, self.ntauroot + 1))
for k in range(self.nsteps):
for j in range(self.ntauroot + 1):
self.T[k,j] = self.tu[k] + \
(self.tu[k+1] - self.tu[k]) * self.tauroot[j]
self.T = self.T.T
# For all collocation points
self.lfcns = []
for j in range(self.ntauroot + 1):
# Construct Lagrange polynomials to get the polynomial basis
# at the collocation point
L = 1
for r in range(self.ntauroot + 1):
if r != j:
L *= (tau - self.tauroot[r]) / \
(self.tauroot[j] - self.tauroot[r])
lfcn = ca.SXFunction("lfcn", [tau],[L])
# Evaluate the polynomial at the final time to get the
# coefficients of the continuity equation
[self.D[j]] = lfcn([1])
# Evaluate the time derivative of the polynomial at all
# collocation points to get the coefficients of the
# collocation equation
tfcn = lfcn.tangent()
for r in range(self.ntauroot + 1):
self.C[j,r] = tfcn([self.tauroot[r]])[0]
self.lfcns.append(lfcn)
# Initialize phiN
self.phiN = []
# Initialize measurement function
phifcn = ca.MXFunction("phifcn", \
[system.t, system.u, system.x, system.eps_u, system.p], \
[system.phi])
# Initialzie setup of g
self.g = []
# Initialize ODE right-hand-side
ffcn = ca.MXFunction("ffcn", \
[system.t, system.u, system.x, system.eps_e, system.eps_u, \
system.p], [system.f])
# Collect information for measurement function
# Structs to hold variables for later mapped evaluation
Tphi = []
Uphi = []
Xphi = []
EPS_Uphi = []
for k in range(self.nsteps):
hk = self.tu[k + 1] - self.tu[k]
t_meas = self.ty[np.where(np.logical_and( \
self.ty >= self.tu[k], self.ty < self.tu[k + 1]))]
for t_meas_j in t_meas:
Uphi.append(self.uN[:, k])
EPS_Uphi.append(self.EPS_U[:self.neps_u, k])
if t_meas_j == self.tu[k]:
Tphi.append(self.tu[k])
Xphi.append(self.X[:self.nx, k])
else:
tau = (t_meas_j - self.tu[k]) / hk
x_temp = 0
for r in range(self.ntauroot + 1):
x_temp += self.lfcns[r]([tau])[0] * \
self.X[r*self.nx : (r+1) * self.nx, k]
Tphi.append(t_meas_j)
Xphi.append(x_temp)
if self.tu[-1] in self.ty:
Tphi.append(self.tu[-1])
Uphi.append(self.uN[:,-1])
Xphi.append(self.XF)
EPS_Uphi.append(self.EPS_U[:self.neps_u,-1])
# Mapped calculation of the collocation equations
# Collocation nodes
hc = ca.MX.sym("hc", 1)
tc = ca.MX.sym("tc", self.ntauroot)
xc = ca.MX.sym("xc", self.nx * (self.ntauroot+1))
eps_ec = ca.MX.sym("eps_ec", self.neps_e * self.ntauroot)
eps_uc = ca.MX.sym("eps_uc", self.neps_u * self.ntauroot)
coleqn = ca.vertcat([ \
hc * ffcn([tc[j-1], \
system.u, \
xc[j*self.nx : (j+1)*self.nx], \
eps_ec[(j-1)*self.neps_e : j*self.neps_e], \
eps_uc[(j-1)*self.neps_u : j*self.neps_u], \
system.p])[0] - \
sum([self.C[r,j] * xc[r*self.nx : (r+1)*self.nx] \
for r in range(self.ntauroot + 1)]) \
for j in range(1, self.ntauroot + 1)])
coleqnfcn = ca.MXFunction("coleqnfcn", \
[hc, tc, system.u, xc, eps_ec, eps_uc, system.p], [coleqn])
coleqnfcn = coleqnfcn.expand()
[gcol] = coleqnfcn.map([ \
np.atleast_2d((self.tu[1:] - self.tu[:-1])), self.T[1:,:], \
self.uN, self.X, self.EPS_E, self.EPS_U, self.P])
# Continuity nodes
xnext = ca.MX.sym("xnext", self.nx)
conteqn = xnext - sum([self.D[r] * xc[r*self.nx : (r+1)*self.nx] \
for r in range(self.ntauroot + 1)])
conteqnfcn = ca.MXFunction("conteqnfcn", [xnext, xc], [conteqn])
conteqnfcn = conteqnfcn.expand()
[gcont] = conteqnfcn.map([ \
ca.horzcat([self.X[:self.nx, 1:], self.XF]), self.X])
# Stack equality constraints together
self.g = ca.veccat([gcol, gcont])
# Evaluation of the measurement function
[self.phiN] = phifcn.map( \
[ca.horzcat(k) for k in Tphi, Uphi, Xphi, EPS_Uphi] + \
[self.P])
# self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN])
self.tend_setup = time.time()
self.duration_setup = self.tend_setup - self.tstart_setup
print('Initialization of ExplODE system sucessful.')
def _simulate_with_casadi_with_inputs(self, initcon, tsim, varying_inputs,
integrator, integrator_options):
xalltemp = [self._templatemap[i] for i in self._diffvars]
xall = casadi.vertcat(*xalltemp)
time = casadi.SX.sym('time')
odealltemp = [time * convert_pyomo2casadi(self._rhsdict[i])
for i in self._derivlist]
odeall = casadi.vertcat(*odealltemp)
# Time-varying inputs
ptemp = [self._templatemap[i]
for i in self._siminputvars.values()]
pall = casadi.vertcat(time, *ptemp)
dae = {'x': xall, 'p': pall, 'ode': odeall}
if len(self._algvars) != 0:
zalltemp = [self._templatemap[i] for i in self._simalgvars]
zall = casadi.vertcat(*zalltemp)
# Need to do anything special with time scaling??
algalltemp = [convert_pyomo2casadi(i) for i in self._alglist]
algall = casadi.vertcat(*algalltemp)
dae['z'] = zall
dae['alg'] = algall
integrator_options['tf'] = 1.0
F = casadi.integrator('F', integrator, dae, integrator_options)
N = len(tsim)
# This approach removes the time scaling from tsim so must
# create an array with the time step between consecutive
# time points
tsimtemp = np.hstack([0, tsim[1:] - tsim[0:-1]])
tsimtemp.shape = (1, len(tsimtemp))
palltemp = [casadi.DM(tsimtemp)]
# Need a similar np array for each time-varying input
for p in self._siminputvars.keys():
profile = varying_inputs[p]
tswitch = list(profile.keys())
tswitch.sort()
tidx = [tsim.searchsorted(i) for i in tswitch] + \
[len(tsim) - 1]
ptemp = [profile[0]] + \
[casadi.repmat(profile[tswitch[i]], 1,
tidx[i + 1] - tidx[i])
for i in range(len(tswitch))]
temp = casadi.horzcat(*ptemp)
palltemp.append(temp)
I = F.mapaccum('simulator', N)
sol = I(x0=initcon, p=casadi.vertcat(*palltemp))
profile = sol['xf'].full().T
if len(self._algvars) != 0:
algprofile = sol['zf'].full().T
profile = np.concatenate((profile, algprofile), axis=1)
return [tsim, profile]
def get_fourier_dcm(fourier_traj):
return C.vertcat([C.horzcat([fourier_traj['e11'], fourier_traj['e12'], fourier_traj['e13']]),
C.horzcat([fourier_traj['e21'], fourier_traj['e22'], fourier_traj['e23']]),
C.horzcat([fourier_traj['e31'], fourier_traj['e32'], fourier_traj['e33']])])
dae['cD_tether'] = 0.25*dae['r']*0.001/sref
dae['L_over_D'] = cL/cD
cD = dae['cD'] + dae['cD_tether']
dae['L_over_D_with_tether'] = cL/cD
######## moment coefficients #######
# offset
dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0])
# with roll rates
# non-dimensionalized angular velocity
w_bn_b_hat = C.veccat([0.5*conf['bref']/dae['airspeed']*w_bn_b[0],
0.5*conf['cref']/dae['airspeed']*w_bn_b[1],
0.5*conf['bref']/dae['airspeed']*w_bn_b[2]])
momentCoeffs_pqr = C.mul(C.vertcat([C.horzcat([conf['cl_p'], conf['cl_q'], conf['cl_r']]),
C.horzcat([conf['cm_p'], conf['cm_q'], conf['cm_r']]),
C.horzcat([conf['cn_p'], conf['cn_q'], conf['cn_r']])]),
w_bn_b_hat)
dae['momentCoeffs_pqr'] = momentCoeffs_pqr
# with alpha beta
momentCoeffs_AB = C.mul(C.vertcat([C.horzcat([ 0, conf['cl_B'], conf['cl_AB']]),
C.horzcat([conf['cm_A'], 0, 0]),
C.horzcat([ 0, conf['cn_B'], conf['cn_AB']])]),
C.vertcat([alpha, beta, alpha*beta]))
dae['momentCoeffs_AB'] = momentCoeffs_AB
# with control surfaces
momentCoeffs_surf = C.SXMatrix(3, 1, 0)
momentCoeffs_surf[0] += conf['cl_ail']*dae['aileron']
# ----------------------------------------------------------------------------
# iLQR
# ----------------------------------------------------------------------------
# Play nominal policy
u_all = control.repeated(ca.DMatrix.zeros(nu,n_sim))
u_all[:,'v'] = 5
u_all[:,'w'] = 1
xk = x0
x_all = [xk]
for k in range(n_sim):
[xk_next] = F([ xk, u_all[k], dt ])
x_all.append(xk_next)
xk = xk_next
x_all = state.repeated(ca.horzcat(x_all))
J0 = policy_cost(x_all,u_all,dt,l,lf)
# Plot policy
fig, ax = plt.subplots(1, 2, figsize=(12, 6))
plot_policy(ax, x_all[:,'x'], x_all[:,'y'], x_all[:,'phi'],
u_all[:,'v'], u_all[:,'w'], t)
# Iterations
mu = 0; mu_min = 1e-6; d_mu0 = 2; d_mu = d_mu0;
mu_flag = False
while True:
# Backward pass with regularization
