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 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