def set_path(self, path, r2r=False): """Define an analytic expression of the geometric path. Note: The path must be defined as a function of self.s[0]. Args: path (list of SXMatrix): An expression of the geometric path as a function of self.s[0]. Its dimension must equal self.sys.ny r2r (boolean): Reparameterize path such that a rest to rest transition is performed Example: >>> S = FlatSystem(2, 4) >>> P = PathFollowing(S) >>> P.set_path([P.s[0], P.s[0]]) """ if isinstance(path, list): path = cas.vertcat(path) if r2r: path = cas.substitute(path, self.s[0], self._r2r()) self.path[:, 0] = path dot_s = cas.vertcat([self.s[1:], 0]) for i in range(1, self.sys.order + 1): self.path[:, i] = cas.mul( cas.jacobian(self.path[:, i - 1], self.s), dot_s)
def _simulate_with_casadi_no_inputs(self, initcon, tsim, integrator, integrator_options): # Old way (10 times faster, but can't incorporate time # varying parameters/controls) xalltemp = [self._templatemap[i] for i in self._diffvars] xall = casadi.vertcat(*xalltemp) odealltemp = [convert_pyomo2casadi(self._rhsdict[i]) for i in self._derivlist] odeall = casadi.vertcat(*odealltemp) dae = {'x': xall, 'ode': odeall} if len(self._algvars) != 0: zalltemp = [self._templatemap[i] for i in self._simalgvars] zall = casadi.vertcat(*zalltemp) algalltemp = [convert_pyomo2casadi(i) for i in self._alglist] algall = casadi.vertcat(*algalltemp) dae['z'] = zall dae['alg'] = algall integrator_options['grid'] = tsim integrator_options['output_t0'] = True F = casadi.integrator('F', integrator, dae, integrator_options) sol = F(x0=initcon) 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 pendulum_model(): """ Nonlinear inverse pendulum model. """ M = 1 # mass of the cart [kg] m = 0.1 # mass of the ball [kg] g = 9.81 # gravity constant [m/s^2] l = 0.8 # length of the rod [m] p = SX.sym('p') # horizontal displacement [m] theta = SX.sym('theta') # angle with the vertical [rad] v = SX.sym('v') # horizontal velocity [m/s] omega = SX.sym('omega') # angular velocity [rad/s] F = SX.sym('F') # horizontal force [N] ode_rhs = vertcat(v, omega, (- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2), (- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2))) nx = 4 # for IRK xdot = SX.sym('xdot', nx, 1) z = SX.sym('z',0,1) return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']), nx, # number of states 1, # number of controls Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot], ['x', 'u','xdot','z'], ['rhs']))
def exitEquation(self, tree): logger.debug('exitEquation') if isinstance(tree.left, list): src_left = ca.vertcat(*[self.get_mx(c) for c in tree.left]) else: src_left = self.get_mx(tree.left) if isinstance(tree.right, list): src_right = ca.vertcat(*[self.get_mx(c) for c in tree.right]) else: src_right = self.get_mx(tree.right) src_left = ca.MX(src_left) src_right = ca.MX(src_right) # According to the Modelica spec, # "It is possible to omit left hand side component references and/or truncate the left hand side list in order to discard outputs from a function call." if isinstance(tree.right, ast.Expression) and tree.right.operator in self.root.classes: if src_left.size1() < src_right.size1(): src_right = src_right[0:src_left.size1()] if isinstance(tree.left, ast.Expression) and tree.left.operator in self.root.classes: if src_left.size1() > src_right.size1(): src_left = src_left[0:src_right.size1()] # If dimensions between the lhs and rhs do not match, but the dimensions of lhs # and transposed rhs do match, transpose the rhs. if src_left.shape != src_right.shape and src_left.shape == src_right.shape[::-1]: src_right = ca.transpose(src_right) self.src[tree] = src_left - src_right
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 get_derivative(self, s): # Case 1: s is a constant, e.g. MX(5) if ca.MX(s).is_constant(): return 0 # Case 2: s is a symbol, e.g. MX(x) elif s.is_symbolic(): if s.name() not in self.derivative: if len(self.for_loops) > 0 and s in self.for_loops[-1].indexed_symbols: # Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol. for_loop_symbol = self.for_loops[-1].indexed_symbols[s] s_without_index = self.get_mx(ast.ComponentRef(name=for_loop_symbol.tree.name)) der_s_without_index = self.get_derivative(s_without_index) if ca.MX(der_s_without_index).is_symbolic(): return self.get_indexed_symbol(ast.ComponentRef(name=der_s_without_index.name(), indices=for_loop_symbol.tree.indices), der_s_without_index) else: return 0 else: der_s = _new_mx("der({})".format(s.name()), s.size()) # If the derivative contains an expression (e.g. der(x + y)) this method is # called with MX variables that are the result of a ca.symvar call. This # ca.symvar call strips the _modelica_shape field from the MX variable, # therefore we need to find the original MX to get the modelica shape. der_s._modelica_shape = \ self.nodes[self.current_class][s.name()]._modelica_shape self.derivative[s.name()] = der_s self.nodes[self.current_class][der_s.name()] = der_s return der_s else: return self.derivative[s.name()] # Case 3: s is an already indexed symbol, e.g. MX(x[1]) elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic(): slice_info = s.info()['slice'] dep = s.dep() if dep.name() not in self.derivative: der_dep = _new_mx("der({})".format(dep.name()), dep.size()) der_dep._modelica_shape = \ self.nodes[self.current_class][dep.name()]._modelica_shape self.derivative[dep.name()] = der_dep self.nodes[self.current_class][der_dep.name()] = der_dep return der_dep[slice_info['start']:slice_info['stop']:slice_info['step']] else: return self.derivative[dep.name()][slice_info['start']:slice_info['stop']:slice_info['step']] # Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2) # Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2) else: # Differentiate expression using CasADi orig_deps = ca.symvar(s) deps = ca.vertcat(*orig_deps) J = ca.Function('J', [deps], [ca.jacobian(s, deps)]) J_sparsity = J.sparsity_out(0) der_deps = [self.get_derivative(dep) if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size()) for j, dep in enumerate(orig_deps)] return ca.mtimes(J(deps), ca.vertcat(*der_deps))
def _stateDictToVec(self, stateDict): stateList = [stateDict[s] for s in self.states] # concatanate and return appropriate type if isinstance(stateList[0], C.MX): # MX return C.vertcat(stateList) elif isinstance(stateList[0], C.SX): # SX return C.vertcat(stateList) else: # numpy array return np.concatenate(stateList, axis=0)
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t/T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho*x) G = -(1/rho)*mtimes(A, A.T) h = b + (1/rho)*mtimes(A, f) mu = solve(G, h) z = -(1/rho)*(mtimes(A.T, mu)+f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def split_dae_alg(eqs: SYM, dx: SYM) -> Dict[str, SYM]: """Split equations into differential algebraic and algebraic only""" dae = [] alg = [] for eq in ca.vertsplit(eqs): if ca.depends_on(eq, dx): dae.append(eq) else: alg.append(eq) return { 'dae': ca.vertcat(*dae), 'alg': ca.vertcat(*alg) }
def solve(self): """ define extra variables homotopy parameters >= 0!! """ if not self.prob['s']: self.set_grid() N = self.options['N'] Nc = self.options['Nc'] self.prob['vars'] = [cas.ssym("b", N + 1, self.sys.order), cas.ssym("h", Nc, self.h.size1())] # Vectorize variables V = cas.vertcat([ cas.vec(self.prob['vars'][0]), cas.vec(self.prob['vars'][1]) ]) self._make_constraints() self._make_objective() self._h_init() con = cas.SXFunction([V], [self.prob['con'][0]]) obj = cas.SXFunction([V], [self.prob['obj']]) if self.options.get('solver') == 'Ipopt': solver = cas.IpoptSolver(obj, con) else: print """Other solver than Ipopt are currently not supported, switching to Ipopt""" solver = cas.IpoptSolver(obj, con) for option, value in self.options.iteritems(): if solver.hasOption(option): solver.setOption(option, value) solver.init() # Setting constraints solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg") solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg") solver.setInput( cas.vertcat([ [np.inf] * self.sys.order * (N + 1), [1] * self.h.size1() * Nc ]), "ubx" ) solver.setInput( cas.vertcat([ [0] * (N + 1), [-np.inf] * (self.sys.order - 1) * (N + 1), [0] * self.h.size1() * Nc ]), "lbx" ) solver.solve() self.prob['solver'] = solver self._get_solution()
def makeModel(conf, propertiesDir = '../properties'): print "\n#### File '%s' is old. It does not use the NED convention you should use carouselModel in offline_mhe_stuff instead.\n" \ % inspect.getfile(inspect.currentframe()) #input("Press Enter if you wish to continue...") dae = rawe.models.carousel(conf) (xDotSol, zSol) = dae.solveForXDotAndZ() ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']]) ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']]) x = dae['x'] y = dae['y'] dx = dae['dx'] dy = dae['dy'] ddelta = dae['ddelta'] dddelta = xDotSol['ddelta'] R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13', 'e21', 'e22', 'e23', 'e31', 'e32', 'e33']] ).reshape((3,3)) rA = conf['rArm'] g = conf['g'] pIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat'))) RIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat'))) ddpIMU = C.mul(R.T,ddp) - ddelta**2*C.mul(R.T,C.vertcat([x+rA,y,0])) + 2*ddelta*C.mul(R.T,C.vertcat([-dy,dx,0])) + dddelta*C.mul(R.T,C.vertcat([-y,x+rA,0])) + C.mul(R.T,C.vertcat([0,0,g])) aBridle = C.cross(ddt_w_bn_b,pIMU) dae['IMU_acceleration'] = C.mul(RIMU,ddpIMU+aBridle) dae['IMU_angular_velocity'] = C.mul(RIMU,dae['w_bn_b']) camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC1.dat'))), 'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC2.dat'))), 'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC1.dat'))), 'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC2.dat'))), 'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body1.dat'))), 'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body2.dat'))), 'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body3.dat')))} dae['marker_positions'] = camModel.fullCamModel(dae,camConf) dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1 dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23'] dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33'] dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1 dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33'] dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1 #dae['ConstDelta'] = dae['cos_delta']*dae['cos_delta'] + dae['sin_delta']*dae['sin_delta'] - 1 dae['ConstDelta'] = ( dae['cos_delta']**2 + dae['sin_delta']**2 - 1 ) return dae
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 set_data(self, data, weights=None): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.boundary_species, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.boundary_species] # Rename columns with state indicies df.columns = np.arange(self.nx) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] if weights is None: weights = self.data.max() obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]] obj_resid = cs.sum_square(cs.vertcat(obj_list)) self.objective_sx += obj_resid
def create_plan_fc(b0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=belief), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Objective function m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']] m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']] dm_bc = m_bN - m_cN J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0 # Regularize controls J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs g = [] for k in range(N_sim): # Multiple shooting [x_next] = BF([V['X',k], V['U',k]]) g.append(x_next - V['X',k+1]) # Penalize uncertainty J += 1e-1 * ca.trace(V['X',k,'S']) * dt g = ca.vertcat(g) # log-probability, doesn't work with full collocation #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']] #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \ # ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v <= v_max lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # m(t=0) = m0 lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m'] # S(t=0) = S0 lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S'] # Solve the NLP sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) return V(sol['x'])
def solve(self): """Solve the optimal control problem solve() first check for steady state feasibility and defines the optimal control problem. After solving, the instance variable sol can be used to examine the solution. TODO: Add support for other solvers TODO: version bump """ if len(self.prob['s']) == 0: self.set_grid() # Check feasibility # self.check_ss_feasibility() # Construct optimization problem self.prob['solver'] = None N = self.options['N'] self.prob['vars'] = cas.ssym("b", N + 1, self.sys.order) V = cas.vec(self.prob['vars']) self._make_objective() self._make_constraints() con = cas.SXFunction([V], [self.prob['con'][0]]) obj = cas.SXFunction([V], [self.prob['obj']]) if self.options.get('solver') == 'Ipopt': solver = cas.IpoptSolver(obj, con) else: print """Other solver than Ipopt are currently not supported, switching to Ipopt""" solver = cas.IpoptSolver(obj, con) for option, value in self.options.iteritems(): if solver.hasOption(option): solver.setOption(option, value) solver.init() # Setting constraints solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg") solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg") solver.setInput([np.inf] * self.sys.order * (N + 1), "ubx") solver.setInput( cas.vertcat(( [0] * (N + 1), (self.sys.order - 1) * (N + 1) * [-np.inf])), "lbx") solver.solve() self.prob['solver'] = solver self._get_solution()
def setUp(self): # System self.x = ca.MX.sym("x", 2) self.p = ca.MX.sym("p", 2) self.u = ca.MX.sym("u", 0) # self.v = ca.MX.sym("v", 2) self.eps_e = ca.MX.sym("eps_e", 2) self.f = ca.vertcat( \ [-1.0 * self.x[0] + self.p[0] * self.x[0] * self.x[1], 1.0 * self.x[1] - self.p[1] * self.x[0] * self.x[1]]) + \ self.eps_e self.phi = self.x self.odesys = pecas.systems.ExplODE(x = self.x, u = self.u, \ p = self.p, eps_e = self.eps_e, f = self.f, phi = self.phi) # Inputs data = np.array(np.loadtxt("test/data_lotka_volterra.txt")) self.tu = data[:, 0] self.invalidpargs = [[0, 1, 2], [[2, 3], [2, 3]], \ np.asarray([1, 2, 3]), np.asarray([[2, 3], [2, 3]])] self.validpargs = [None, [2, 3], np.asarray([3, 4]), \ np.asarray([1, 2]).T, np.asarray([[2], [3]])] self.invalidxargs = [np.ones((self.x.size() - 1, self.tu.size)), \ np.ones((self.tu.size - 1, self.x.size()))] self.validxargs = [None, np.ones((self.x.size(), self.tu.size)), \ np.ones((self.tu.size, self.x.size()))] # Since the supported values are never used, there is no case of # invalid u-arguments in this testcase self.invaliduargs = [] self.validuargs = [None, [], [1, 2, 3]] # -- TODO! -- # None of the checks will detect an invalidxpvbarg of # [[2, 1], [3]], since shape and size both fit. # --> How to check for this? self.yN = data[:, 1::2] self.wv = 1.0 / data[:, 2::2]**2 self.uN = None self.weps_e = [1.0 / 1e-4, 1.0 / 1e-4] self.weps_u = [None] self.pinit = [0.5, 1.0] self.xinit = self.yN self.phat = np.atleast_2d([0.69348187, 0.34116928]).T
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 _set_objective_from_data(self, data, weights): obj_list = [] for ((ti, state), xi) in data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]] obj_resid = cs.sum_square(cs.vertcat(obj_list)) self.objective_sx += obj_resid
def chen_model(): """ The following ODE model comes from Chen1998. """ nx, nu = (2, 1) x = SX.sym('x', nx) u = SX.sym('u', nu) mu = 0.5 rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1])) return Function('chen', [x, u], [rhs]), nx, nu
def casadi_struct2vec(s): flat = [] if isinstance(s,OrderedDict): for f in s.keys(): flat.append(casadi_struct2vec(s[f])) return C.vertcat(flat) else: return C.vec(s)
def setUp(self): self.x = ca.MX.sym("x", 4) self.p = ca.MX.sym("p", 6) self.u = ca.MX.sym("u", 2) self.f = ca.vertcat( \ [self.x[3] * np.cos(self.x[2] + self.p[0] * self.u[0]), self.x[3] * np.sin(self.x[2] + self.p[0] * self.u[0]), self.x[3] * self.u[0] * self.p[1], self.p[2] * self.u[1] \ - self.p[3] * self.u[1] * self.x[3] \ - self.p[4] * self.x[3]**2 \ - self.p[5] \ - (self.x[3] * self.u[0])**2 * self.p[1] * self.p[0]]) self.phi = self.x data = np.array(np.loadtxt("test/data_2d_vehicle_pe.dat", \ delimiter = ", ", skiprows = 1)) self.time_points = data[100:250, 1] self.ydata = data[100:250, [2, 4, 6, 8]] self.udata = data[100:249, [9, 10]] self.pinit =[0.5, 17.06, 12.0, 2.17, 0.1, 0.6] self.xinit = self.ydata self.phat = np.atleast_2d( \ [0.200652, 11.6528, -26.2501, -74.1967, 16.8705, -1.80125]).T self.covariance_matrix = np.loadtxt( \ "test/covariance_matrix_2d_vehicle_pe.txt", delimiter=",") self.time_points_doe = data[200:205, 1] self.ydata_doe = data[200:205, [2, 4, 6, 8]] self.uinit_doe = data[200:205, [9, 10]][:-1, :] self.pdata_doe = [0.273408, 11.5602, 2.45652, 7.90959, -0.44353, -0.249098] self.umin_doe = [-0.436332, -0.3216] self.umax_doe = [0.436332, 1.0] self.xmin_doe = [-0.787, -1.531, -12.614, 0.0] self.xmax_doe = [1.2390, 0.014, 0.013, 0.7102] self.design_results = np.atleast_2d( \ np.loadtxt("test/optimized_controls_2d_vehicle_doe.txt")).T
def exitIfEquation(self, tree): logger.debug('exitIfEquation') # Check if every equation block contains the same number of equations if len(set((len(x) for x in tree.blocks))) != 1: raise Exception("Every branch in an if-equation needs the same number of equations.") # NOTE: We currently assume that we always have an else-clause. This # is not strictly necessary, see the Modelica Spec on if equations. assert tree.conditions[-1] == True src = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-1]]) for cond_index in range(1, len(tree.conditions)): cond = self.get_mx(tree.conditions[-(cond_index + 1)]) expr1 = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-(cond_index + 1)]]) src = ca.if_else(cond, expr1, src, True) self.src[tree] = src
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 _get_solution(self): """Get the solution from the solver output Fills the dictionary self.sol with the information: * 's': The optimal s as a function of time * 't': The time vector * 'states': Numerical values of the states defined in self.sys TODO: perform accurate integration to determine time TODO: Do exact interpolation """ solver = self.prob['solver'] N = self.options['N'] x_opt = np.array(solver.getOutput("x")).ravel() b_opt = np.reshape(x_opt, (N + 1, -1), order='F') self.sol['b'] = b_opt # Determine time on a sufficiently fine spatial grid s0 = np.linspace(0, 1, 1001) delta = s0[1] - s0[0] pieces = [lambda s, b=b_opt, ss=ss, j=j: sum([bb * (s - ss) ** i / fact[i] for i, bb in enumerate(b[j])]) for j, ss in enumerate(self.prob['s'][:-1])] conds = lambda s0: [np.logical_and(self.prob['s'][i] <= s0, s0 <= self.prob['s'][i+1]) for i in range(N)] b0_opt = np.piecewise(s0, conds(s0), pieces) b0_opt[b0_opt < 0] = 0 time = np.cumsum(np.hstack([0, 2 * delta / (np.sqrt(b0_opt[:-1]) + np.sqrt(b0_opt[1:]))])) # Resample to constant time-grid t = np.arange(time[0], time[-1], self.options['Ts']) st = np.interp(t, time, s0) # Evaluate solution on equidistant time grid b_opt = np.c_[[np.piecewise(st, conds(st), pieces, b=b_opt[:, i:]) for i in range(self.sys.order)]].T st = np.matrix(st) # Determine s and derivatives from b_opt b, Ds = self._make_path()[1:] Ds_f = cas.SXFunction([b], [Ds]) # derivatives of s wrt b Ds_f.init() s_opt = np.hstack((st.T, np.array([evalf(Ds_f, bb).toArray().ravel() for bb in b_opt]))) self.sol['s'] = np.asarray(s_opt) self.sol['t'] = t # Evaluate the states f = cas.SXFunction([self.s], [cas.substitute(cas.vertcat(self.sys.x.values()), self.sys.y, self.path)]) f_val = np.array([evalf(f, s.T).toArray().ravel() for s in s_opt]) self.sol['states'] = dict([(k, f_val[:, i]) for i, k in enumerate(self.sys.x.keys())])
def init(self, horizon_times=None): if self.options['spline_traj'] == False: # pos, vel, acc x = self.define_parameter('x', self.n_dim) v = self.define_parameter('v', self.n_dim) a = self.define_parameter('a', self.n_dim) # pos, vel, acc at time zero of time horizon self.t = self.define_symbol('t') # motion time can be passed from environment if horizon_times is None: self.T = self.define_symbol('T') elif not isinstance(horizon_times, list): horizon_times = [horizon_times] v0 = v - self.t*a x0 = x - self.t*v0 - 0.5*(self.t**2)*a a0 = a if horizon_times: # not None # build up pos_spline gradually, e.g. the pos_spline for second segment starts at # end position for first segment (pos0(1)) pos0 = x0 self.pos_spline = [0]*self.n_dim for horizon_time in horizon_times: for k in range(self.n_dim): self.pos_spline[k] = BSpline(self.basis, vertcat(pos0[k], 0.5*v0[k]*horizon_time + pos0[k], pos0[k] + v0[k]*horizon_time + 0.5*a0[k]*(horizon_time**2))) # update start position for next segment pos0 = [self.pos_spline[k](1) for k in range(self.n_dim)] else: # horizon_times was None # pos spline over time horizon self.pos_spline = [BSpline(self.basis, vertcat(x0[k], 0.5*v0[k]*self.T + x0[k], x0[k] + v0[k]*self.T + 0.5*a0[k]*(self.T**2))) for k in range(self.n_dim)] else: # using a spline to define obstacle trajectory self.basis = BSplineBasis(self.options['spline_params']['knots'], self.options['spline_params']['degree']) traj_coeffs = self.define_parameter('traj_coeffs', len(self.basis), self.n_dim) # pos spline over time horizon self.pos_spline = [BSpline(self.basis, traj_coeffs[:, k]) for k in range(self.n_dim)] # checkpoints + radii checkpoints, _ = self.shape.get_checkpoints() self.checkpoints = self.define_parameter('checkpoints', len(checkpoints)*self.n_dim) self.rad = self.define_parameter('rad', len(checkpoints))
def _initialize_polynomial_coefs(self): """ Setup radau polynomials and initialize the weight factor matricies """ self.col_vars['tau_root'] = cs.collocationPoints(self.d, "radau") # Dimensionless time inside one control interval tau = cs.SX.sym("tau") # For all collocation points L = [[]]*(self.d+1) for j in range(self.d+1): # Construct Lagrange polynomials to get the polynomial basis at the # collocation point L[j] = 1 for r in range(self.d+1): if r != j: L[j] *= ( (tau - self.col_vars['tau_root'][r]) / (self.col_vars['tau_root'][j] - self.col_vars['tau_root'][r])) self.col_vars['lfcn'] = lfcn = cs.SXFunction( 'lfcn', [tau], [cs.vertcat(L)]) # Evaluate the polynomial at the final time to get the coefficients of # the continuity equation # Coefficients of the continuity equation self.col_vars['D'] = lfcn([1.0])[0].toArray().squeeze() # Evaluate the time derivative of the polynomial at all collocation # points to get the coefficients of the continuity equation tfcn = lfcn.tangent() # Coefficients of the collocation equation self.col_vars['C'] = np.zeros((self.d+1, self.d+1)) for r in range(self.d+1): self.col_vars['C'][:,r] = tfcn([self.col_vars['tau_root'][r]] )[0].toArray().squeeze() # Find weights for gaussian quadrature: approximate int_0^1 f(x) by # Sum( xtau = cs.SX.sym("xtau") Phi = [[]] * (self.d+1) for j in range(self.d+1): tau_f_integrator = cs.SXFunction('ode', cs.daeIn(t=tau, x=xtau), cs.daeOut(ode=L[j])) tau_integrator = cs.Integrator( "integrator", "cvodes", tau_f_integrator, {'t0':0., 'tf':1}) Phi[j] = np.asarray(tau_integrator({'x0' : 0})['xf'])[0][0] self.col_vars['Phi'] = np.array(Phi) self.col_vars['alpha'] = cs.SX.sym('alpha')
def fullCamModel(dae,conf): PdatC1 = conf['PdatC1'] PdatC2 = conf['PdatC2'] RPC1 = conf['RPC1'] RPC2 = conf['RPC2'] pos_marker_body1 = conf['pos_marker_body1'] pos_marker_body2 = conf['pos_marker_body2'] pos_marker_body3 = conf['pos_marker_body3'] RpC1 = C.DMatrix.eye(4) RpC1[0:3,0:3] = RPC1[0:3,0:3].T RpC1[0:3,3] = C.mul(-RPC1[0:3,0:3].T,RPC1[0:3,3]) RpC2 = C.DMatrix.eye(4); RpC2[0:3,0:3] = RPC2[0:3,0:3].T RpC2[0:3,3] = C.mul(-RPC2[0:3,0:3].T,RPC2[0:3,3]) PC1 = C.SXMatrix(3,3) PC1[0,0] = PdatC1[0] PC1[1,1] = PdatC1[1] PC1[0,2] = PdatC1[2] PC1[1,2] = PdatC1[3] PC1[2,2] = 1.0 PC2 = C.SXMatrix(3,3) PC2[0,0] = PdatC2[0] PC2[1,1] = PdatC2[1] PC2[0,2] = PdatC2[2] PC2[1,2] = PdatC2[3] PC2[2,2] = 1.0 p = C.vertcat([dae['x'],dae['y'],dae['z']]) R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13', 'e21', 'e22', 'e23', 'e31', 'e32', 'e33']] ).reshape((3,3)) uv_all = C.vertcat([C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body1)) ,\ C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body2)) ,\ C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body3)) ,\ C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body1)) ,\ C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body2)) ,\ C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body3))]) return uv_all
def _initialize_tgrid(self): # Choose collocation points tau_root = cs.collocationPoints(self.opts["degree"], self.opts["polynomial"]) # Degree of interpolating polynomial d = self.opts["degree"] # Size of the finite elements self.opts["h"] = self.opts["tf"] / self.opts["nk"] # Coefficients of the collocation equation self.opts["C"] = np.zeros((d + 1, d + 1)) # Dimensionless time inside one control interval tau = cs.SX.sym("tau") T = np.zeros((self.opts["nk"], d + 1)) for k in range(self.opts["nk"]): for j in range(d + 1): T[k, j] = self.opts["h"] * (k + tau_root[j]) self.opts["T"] = T # Construct Lagrange polynomials to get the polynomial basis at the # collocation point L = [[]] * (d + 1) # For all collocation points for j in range(d + 1): L[j] = 1 for r in range(d + 1): if r != j: L[j] *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r]) self._lfcn = cs.SXFunction("lfcn", [tau], [cs.vertcat(L)]) # Evaluate the polynomial at the final time to get the coefficients of # the continuity equation self.opts["D"] = np.asarray(self._lfcn([1.0])[0]).squeeze() # Evaluate the time derivative of the polynomial at all collocation # points to get the coefficients of the continuity equation tfcn = self._lfcn.tangent() for r in range(d + 1): self.opts["C"][:, r] = tfcn([tau_root[r]])[0].toArray().squeeze() self._tgrid = np.array( [ point + self.opts["h"] * np.array(tau_root) for point in np.linspace(0, self.opts["tf"], self.opts["nk"], endpoint=False) ] ).flatten()
def _make_objective(self): """Construct objective function from the problem definition Make time optimal objective function and add a regularization to ensure a unique solution. When additional objective terms are defined these are added to the objective as well. TODO: Improve accuracy of integration """ order = self.sys.order N = self.options['N'] b = self.prob['vars'] # ds = 1.0/(N+1) obj = 2 * sum(np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0]))) # reg = sum(cas.sqrt((b[1:, order - 1] - b[:N, order - 1]) ** 2)) reg = sum((b[2:, order - 1] - 2 * b[1:N, order - 1] + b[:(N - 1), order - 1]) ** 2) for f in self.objective['Lagrange']: path, bs = self._make_path()[0:2] # S = np.arange(0, 1, 1.0/(N+1)) S = self.prob['s'] b = self.prob['vars'] L = cas.substitute(f, self.sys.y, path) L = 2 * sum(cas.vertcat([ cas.substitute( L, cas.vertcat([self.s[0], bs]), cas.vertcat([S[j], b[j, :].T]) ) for j in range(1, N + 1) ]) * np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0])) ) # L = sum(cas.vertcat([cas.substitute(L, cas.vertcat([self.s[0], # [bs[i] for i in range(0, bs.numel())]]), # cas.vertcat([S[j], [b[j, i] for i in range(0, self.sys.order)]])) # for j in range(0, N + 1)])) obj = obj + L self.prob['obj'] = obj + self.options['reg'] * reg
def T2WJ(T,p): """ w_101 = T2WJ(T_10,p).diff(p,t) """ R = T2R(T) RT = R.T temp = [] for i,k in [(2,1),(0,2),(1,0)]: #temp.append(c.mul(c.jacobian(R[:,k],p).T,R[:,i]).T) temp.append(c.mul(RT[i,:],c.jacobian(R[:,k],p))) return c.vertcat(temp)
-5.53636820e-01, 1.86726808e-01, -1.32319806e-01, -2.06761360e+00, 3.12421835e-02, 8.89043596e-01, -7.03329152e-01 ] q0_2 = [ 0.36148756, 0.19562711, 0.34339407, -2.06759027, -0.08427634, 0.89133467, 0.75131025 ] #Implementing with my L1 norm method hqp = hqp() #decision variables for robot 1 q1, q_dot1 = hqp.create_variable(7, 1e-6) J1 = jac_fun_rob(q1) J1 = cs.vertcat(J1[0], J1[1]) #progress variable for robot 1 s_1, s_dot1 = hqp.create_variable(1, 1e-6) fk_vals1 = robot.fk(q1)[6] #forward kinematics first robot #decision variables for robot 2 q2, q_dot2 = hqp.create_variable(7, 1e-6) J2 = jac_fun_rob(q2) J2 = cs.vertcat(J2[0], J2[1]) #progress variables for robot 2 s_2, s_dot2 = hqp.create_variable(1, 1e-6) fk_vals2 = robot.fk(q2)[6] #forward kinematics second robot fk_vals2[1, 3] += 0.3 #accounting for the base offset in the y-direction
def exitExpression(self, tree): if isinstance(tree.operator, ast.ComponentRef): op = tree.operator.name else: op = tree.operator if op == '*': op = 'mtimes' # .* differs from * if op.startswith('.'): op = op[1:] logger.debug('exitExpression') n_operands = len(tree.operands) if op == 'der': v = self.get_mx(tree.operands[0]) src = self.get_derivative(v) elif op == '-' and n_operands == 1: src = -self.get_mx(tree.operands[0]) elif op == 'not' and n_operands == 1: src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True) elif op == 'mtimes': assert n_operands >= 2 src = self.get_mx(tree.operands[0]) for i in tree.operands[1:]: src = ca.mtimes(src, self.get_mx(i)) elif op == 'transpose' and n_operands == 1: src = self.get_mx(tree.operands[0]).T elif op == 'sum' and n_operands == 1: v = self.get_mx(tree.operands[0]) src = ca.sum1(v) elif op == 'linspace' and n_operands == 3: a = self.get_mx(tree.operands[0]) b = self.get_mx(tree.operands[1]) n_steps = self.get_integer(tree.operands[2]) src = ca.linspace(a, b, n_steps) elif op == 'fill' and n_operands == 2: val = self.get_mx(tree.operands[0]) n_row = self.get_integer(tree.operands[1]) src = val * ca.DM.ones(n_row) elif op == 'fill' and n_operands == 3: val = self.get_mx(tree.operands[0]) n_row = self.get_integer(tree.operands[1]) n_col = self.get_integer(tree.operands[2]) src = val * ca.DM.ones(n_row, n_col) elif op == 'zeros' and n_operands == 1: n_row = self.get_integer(tree.operands[0]) src = ca.DM.zeros(n_row) elif op == 'zeros' and n_operands == 2: n_row = self.get_integer(tree.operands[0]) n_col = self.get_integer(tree.operands[1]) src = ca.DM.zeros(n_row, n_col) elif op == 'ones' and n_operands == 1: n_row = self.get_integer(tree.operands[0]) src = ca.DM.ones(n_row) elif op == 'ones' and n_operands == 2: n_row = self.get_integer(tree.operands[0]) n_col = self.get_integer(tree.operands[1]) src = ca.DM.ones(n_row, n_col) elif op == 'identity' and n_operands == 1: n = self.get_integer(tree.operands[0]) src = ca.DM.eye(n) elif op == 'diagonal' and n_operands == 1: diag = self.get_mx(tree.operands[0]) n = len(diag) indices = list(range(n)) src = ca.DM.triplet(indices, indices, diag, n, n) elif op == 'cat': axis = self.get_integer(tree.operands[0]) assert axis == 1, "Currently only concatenation on first axis is supported" entries = [] for sym in [self.get_mx(op) for op in tree.operands[1:]]: if isinstance(sym, list): for e in sym: entries.append(e) else: entries.append(sym) src = ca.vertcat(*entries) elif op == 'delay' and n_operands == 2: expr = self.get_mx(tree.operands[0]) duration = self.get_mx(tree.operands[1]) src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size()) self.delay_counter += 1 for f in self.for_loops: syms = set(ca.symvar(expr)) if syms.intersection(f.indexed_symbols): f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable) self.model.delay_states.append(src.name()) self.model.inputs.append(Variable(src)) delay_argument = DelayArgument(expr, duration) self.model.delay_arguments.append(delay_argument) elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4: entered_class = self.entered_classes[-1] if isinstance(tree.operands[0], ast.ComponentRef): xp = self.get_mx( entered_class.symbols[tree.operands[0].name].value) else: xp = self.get_mx(tree.operands[0]) if isinstance(tree.operands[1], ast.ComponentRef): yp = self.get_mx( entered_class.symbols[tree.operands[1].name].value) else: yp = self.get_mx(tree.operands[1]) arg = self.get_mx(tree.operands[2]) if n_operands == 4: assert isinstance(tree.operands[3], ast.Primary) mode = tree.operands[3].value else: mode = 'linear' func = ca.interpolant('interpolant', mode, [xp], yp) src = func(arg) elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6: entered_class = self.entered_classes[-1] if isinstance(tree.operands[0], ast.ComponentRef): xp = self.get_mx( entered_class.symbols[tree.operands[0].name].value) else: xp = self.get_mx(tree.operands[0]) if isinstance(tree.operands[1], ast.ComponentRef): yp = self.get_mx( entered_class.symbols[tree.operands[1].name].value) else: yp = self.get_mx(tree.operands[1]) if isinstance(tree.operands[2], ast.ComponentRef): zp = self.get_mx( entered_class.symbols[tree.operands[2].name].value) else: zp = self.get_mx(tree.operands[2]) arg_1 = self.get_mx(tree.operands[3]) arg_2 = self.get_mx(tree.operands[4]) if n_operands == 6: assert isinstance(tree.operands[5], ast.Primary) mode = tree.operands[5].value else: mode = 'linear' func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F')) src = func(ca.vertcat(arg_1, arg_2)) elif op in OP_MAP and n_operands == 2: lhs = ca.MX(self.get_mx(tree.operands[0])) rhs = ca.MX(self.get_mx(tree.operands[1])) lhs_op = getattr(lhs, OP_MAP[op]) src = lhs_op(rhs) elif op in OP_MAP and n_operands == 1: lhs = ca.MX(self.get_mx(tree.operands[0])) lhs_op = getattr(lhs, OP_MAP[op]) src = lhs_op() else: src = ca.MX(self.get_mx(tree.operands[0])) # Check for built-in operations, such as the # elementary functions, first. if hasattr(src, op) and n_operands <= 2: if n_operands == 1: src = ca.MX(self.get_mx(tree.operands[0])) src = getattr(src, op)() else: lhs = ca.MX(self.get_mx(tree.operands[0])) rhs = ca.MX(self.get_mx(tree.operands[1])) lhs_op = getattr(lhs, op) src = lhs_op(rhs) else: func = self.get_function(op) src = ca.vertcat(*func.call( [self.get_mx(operand) for operand in tree.operands], *self.function_mode)) self.src[tree] = src
def ODEmodel(): #================================================================== #State variable definitions #================================================================== M = cs.ssym("M") Pc = cs.ssym("Pc") Pn = cs.ssym("Pn") #for Casadi y = cs.vertcat([M, Pc, Pn]) # Time Variable t = cs.ssym("t") #=================================================================== #Parameter definitions #=================================================================== vs0 = cs.ssym('vs0') light = cs.ssym('light') alocal = cs.ssym('alocal') couplingStrength = cs.ssym('couplingStrength') n = cs.ssym('n') vm = cs.ssym('vm') k1 = cs.ssym('k1') km = cs.ssym('km') ks = cs.ssym('ks') vd = cs.ssym('vd') kd = cs.ssym('kd') k1_ = cs.ssym('k1_') k2_ = cs.ssym('k2_') paramset = cs.vertcat([vs0, light, alocal, couplingStrength, n, vm, k1, km, ks, vd, kd, k1_, k2_]) #=================================================================== # Model Equations #=================================================================== ode = [[]]*EqCount def pm_prod(Pn, K, v0, n, light, a, M, Mi): vs = v0+light+a*(M-Mi) return (vs*K**n)/(K**n + Pn**n) def pm_deg(M, Km, vm): return vm*M/(Km+M) def Pc_prod(ks, M): return ks*M def Pc_deg(Pc, K, v): return v*Pc/(K+Pc) def Pc_comp(k1,k2,Pc,Pn): return -k1*Pc + k2*Pn def Pn_comp(k1,k2,Pc,Pn): return k1*Pc - k1*Pn #Rxns ode[0] = (pm_prod(Pn, k1, vs0, n, light, alocal, M, M) - pm_deg(M,km,vm)) ode[1] = Pc_prod(ks,M) - Pc_deg(Pc,kd,vd) + Pc_comp(k1_,k2_,Pc,Pn) ode[2] = Pn_comp(k1_,k2_,Pc,Pn) ode = cs.vertcat(ode) fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=paramset), cs.daeOut(ode=ode)) fn.setOption("name","arya") return fn
theta = ocp.state() # Defince controls delta = ocp.control() V = ocp.control(order=0) # Specify ODE ocp.set_der(x, V*cos(theta)) ocp.set_der(y, V*sin(theta)) ocp.set_der(theta, V/L*tan(delta)) # Define parameter X_0 = ocp.parameter(nx) # Initial constraints X = vertcat(x, y, theta) ocp.subject_to(ocp.at_t0(X) == X_0) # Initial guess ocp.set_initial(x, 0) ocp.set_initial(y, 0) ocp.set_initial(theta, 0) ocp.set_initial(V, 0.5) # Path constraints ocp.subject_to( 0 <= (V <= 1) ) #ocp.subject_to( -0.3 <= (ocp.der(V) <= 0.3) ) ocp.subject_to( -pi/6 <= (delta <= pi/6) ) # Minimal time
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case): """ QP solver for path-following algorithm inputs: prob - problem description obj - problem equations p_init - initial parameter x_init - initial primal variable y_init - initial dual variable lam_opt - Lagrange multipliers of equality and active constraints mu_opt - Lagrange multipliers of inequality constraints outputs: y - solution primal variable qp_val - objective function value qp_exit - return status of QP solver deriv - derivatives of the problem k_zero_tilde - active set index k_plus_tilde - inactive set index grad - gradient of objective function """ print 'Current point x:', x_init #Importing problem to be solved nx, np, neq, niq, name = prob() x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj( x_init, y_init, p_init, neq, niq, nx, np) #Deteriming constraint types eq_con_ind = array([]) #indices of equality constraints iq_con_ind = array([]) #indices of inequality constraints eq_con = array([]) #equality constraints iq_con = array([]) #inequality constraints for i in range(0, len(lbg[0])): if lbg[0, i] == 0: eq_con = vertcat(eq_con, con[i]) eq_con_ind = append(eq_con_ind, i) elif lbg[0, i] < 0: iq_con = vertcat(iq_con, con[i]) iq_con_ind = append(iq_con_ind, i) # print 'Equality Constraint:', eq_con # print 'Inequality Constraint:', iq_con # if case == 'pure-predictor': # return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt if case == 'predictor-corrector': #Evaluating constraints at current iteration point con_vals = conf(x_init, p_init) #Determining which inequality constraints are active k_plus_tilde = array([]) #active constraint k_zero_tilde = array([]) #inactive constraint tol = 10e-5 #tolerance for i in range(0, len(iq_con_ind)): if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0, i] + tol: k_plus_tilde = append(k_plus_tilde, i) else: k_zero_tilde = append(k_zero_tilde, i) # print 'Active constraints:', k_plus_tilde # print 'Inactive constraints:', k_zero_tilde # print 'Constraint values:', con_vals nk_pt = len(k_plus_tilde) #number of active constraints nk_zt = len(k_zero_tilde) #number of inactive constraints #Calculating Lagrangian lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints lag_f = f + mtimes(lam.T, eq_con) + mtimes( mu.T, iq_con) #Lagrangian equation #Calculating derivatives g = gradient(f, x) #Derivative of objective function g_fun = Function('g_fun', [x, p], [gradient(f, x)]) H = 2 * jacobian(gradient(lag_f, x), x) #Second derivative of the Lagrangian H_fun = Function('H_fun', [x, p, lam, mu], [jacobian(jacobian(lag_f, x), x)]) if len(eq_con_ind) > 0: deq = jacobian(eq_con, x) #Derivative of equality constraints else: deq = array([]) if len(iq_con_ind) > 0: diq = jacobian(iq_con, x) #Derivative of inequality constraints else: diq = array([]) #Creating constraint matrices nc = niq + neq #Total number of constraints if (niq > 0) and (neq > 0): #Equality and inequality constraints #this part needs to be tested if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) print deq A[0, :] = deq #A matrix lba = -1e16 * SX.zeros((nc, 1)) lba[0, :] = -eq_con #lower bound of A uba = 1e16 * SX.zeros((nc, 1)) uba[0, :] = -eq_con #upper bound of A for j in range(0, nk_pt): #adding active constraints A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :] lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])] uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])] for i in range(0, nk_zt): #adding inactive constraints A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :] uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: #Active constraints only A = vertcat(deq, diq) lba = vertcat(-eq_con, -iq_con) uba = vertcat(-eq_con, -iq_con) elif (niq > 0) and (neq == 0): #Inquality constraints if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) lba = -1e16 * SX.ones((nc, 1)) uba = 1e16 * SX.ones((nc, 1)) for j in range(0, nk_pt): #adding active constraints A[j, :] = diq[int(k_plus_tilde[j]), :] lba[j] = -iq_con[int(k_plus_tilde[j])] uba[j] = -iq_con[int(k_plus_tilde[j])] for i in range(0, nk_zt): #adding inactive constraints A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :] uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: raw_input() A = vertcat(deq, diq) lba = -iq_con uba = -iq_con elif (niq == 0) and (neq > 0): #Equality constriants A = deq lba = -eq_con uba = -eq_con A_fun = Function('A_fun', [x, p], [A]) lba_fun = Function('lba_fun', [x, p], [lba]) uba_fun = Function('uba_fun', [x, p], [uba]) #Checking that matrices are correct sizes and types if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'): #H matrix should be a sparse (nxn) and symmetrical print( 'WARNING: H matrix is not the correct dimensions or matrix type' ) if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True': #g matrix should be a dense (nx1) print( 'WARNING: g matrix is not the correct dimensions or matrix type' ) if (A.size1() != (neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'): #A should be a sparse (nc x n) print( 'WARNING: A matrix is not the correct dimensions or matrix type' ) if lba.size1() != (neq + niq) or (lba.size2() != 1) or lba.is_dense() == 'False': print( 'WARNING: lba matrix is not the correct dimensions or matrix type' ) if uba.size1() != (neq + niq) or (uba.size2() != 1) or uba.is_dense() == 'False': print( 'WARNING: uba matrix is not the correct dimensions or matrix type' ) #Evaluating QP matrices at optimal points H_opt = H_fun(x_init, p_init, lam_opt, mu_opt) g_opt = g_fun(x_init, p_init) A_opt = A_fun(x_init, p_init) lba_opt = lba_fun(x_init, p_init) uba_opt = uba_fun(x_init, p_init) # print 'Lower bounds', lba_opt # print 'Upper bounds', uba_opt # print 'Bound matrix', A_opt #Defining QP structure qp = {} qp['h'] = H_opt.sparsity() qp['a'] = A_opt.sparsity() optimize = conic('optimize', 'qpoases', qp) optimal = optimize(h=H_opt, g=g_opt, a=A_opt, lba=lba_opt, uba=uba_opt, x0=x_init) x_qpopt = optimal['x'] if x_qpopt.shape == x_init.shape: qp_exit = 'optimal' else: qp_exit = '' lag_qpopt = optimal['lam_a'] #Determing Lagrangian multipliers (lambda and mu) lam_qpopt = zeros( (nk_pt, 1)) #Lagrange multiplier of active constraints mu_qpopt = zeros( (nk_zt, 1)) #Lagrange multiplier of inactive constraints if nk_pt > 0: for j in range(0, len(k_plus_tilde)): lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])] if nk_zt > 0: for k in range(0, len(k_zero_tilde)): print lag_qpopt[int(k_zero_tilde[k])] return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
def create_integrator(self, model, inputs, t_eval=None, use_event_switch=False): """ Method to create a casadi integrator object. If t_eval is provided, the integrator uses t_eval to make the grid. Otherwise, the integrator has grid [0,1]. """ pybamm.logger.debug("Creating CasADi integrator") # Use grid if t_eval is given use_grid = not (t_eval is None) if use_grid is True: t_eval_shifted = t_eval - t_eval[0] t_eval_shifted_rounded = np.round(t_eval_shifted, decimals=12).tobytes() # Only set up problem once if model in self.integrators: # If we're not using the grid, we don't need to change the integrator if use_grid is False: return self.integrators[model]["no grid"] # Otherwise, create new integrator with an updated grid # We don't need to update the grid if reusing the same t_eval # (up to a shift by a constant) else: if t_eval_shifted_rounded in self.integrators[model]: return self.integrators[model][t_eval_shifted_rounded] else: method, problem, options = self.integrator_specs[model] options["grid"] = t_eval_shifted integrator = casadi.integrator("F", method, problem, options) self.integrators[model][ t_eval_shifted_rounded] = integrator return integrator else: y0 = model.y0 rhs = model.casadi_rhs algebraic = model.casadi_algebraic # When not in DEBUG mode (level=10), suppress warnings from CasADi if (pybamm.logger.getEffectiveLevel() == 10 or pybamm.settings.debug_mode is True): show_eval_warnings = True else: show_eval_warnings = False options = { **self.extra_options_setup, "reltol": self.rtol, "abstol": self.atol, "show_eval_warnings": show_eval_warnings, } # set up and solve t = casadi.MX.sym("t") p = casadi.MX.sym("p", inputs.shape[0]) y_diff = casadi.MX.sym("y_diff", rhs(0, y0, p).shape[0]) y_alg = casadi.MX.sym("y_alg", algebraic(0, y0, p).shape[0]) y_full = casadi.vertcat(y_diff, y_alg) if use_grid is False: # rescale time t_min = casadi.MX.sym("t_min") t_max = casadi.MX.sym("t_max") t_max_minus_t_min = t_max - t_min t_scaled = t_min + (t_max - t_min) * t # add time limits as inputs p_with_tlims = casadi.vertcat(p, t_min, t_max) else: options.update({"grid": t_eval_shifted, "output_t0": True}) # rescale time t_min = casadi.MX.sym("t_min") # Set dummy parameters for consistency with rescaled time t_max_minus_t_min = 1 t_scaled = t_min + t p_with_tlims = casadi.vertcat(p, t_min) # define the event switch as the point when an event is crossed # we don't do this for ODE models # see #1082 event_switch = 1 if use_event_switch is True and not algebraic(0, y0, p).is_empty(): for event in model.casadi_terminate_events: event_switch *= event(t_scaled, y_full, p) problem = { "t": t, "x": y_diff, # rescale rhs by (t_max - t_min) "ode": (t_max_minus_t_min) * rhs(t_scaled, y_full, p) * event_switch, "p": p_with_tlims, } if algebraic(0, y0, p).is_empty(): method = "cvodes" else: method = "idas" problem.update({ "z": y_alg, "alg": algebraic(t_scaled, y_full, p), }) integrator = casadi.integrator("F", method, problem, options) self.integrator_specs[model] = method, problem, options if use_grid is False: self.integrators[model] = {"no grid": integrator} else: self.integrators[model] = {t_eval_shifted_rounded: integrator} return integrator
def _integrate(self, model, t_eval, inputs_dict=None): """ Solve a DAE model defined by residuals with initial conditions y0. Parameters ---------- model : :class:`pybamm.BaseModel` The model whose solution to calculate. t_eval : numeric type The times at which to compute the solution inputs_dict : dict, optional Any external variables or input parameters to pass to the model when solving """ # Record whether there are any symbolic inputs inputs_dict = inputs_dict or {} has_symbolic_inputs = any( isinstance(v, casadi.MX) for v in inputs_dict.values()) # convert inputs to casadi format inputs = casadi.vertcat(*[x for x in inputs_dict.values()]) # Calculate initial event signs needed for some of the modes if (has_symbolic_inputs is False and self.mode != "fast" and model.terminate_events_eval): init_event_signs = np.sign( np.concatenate([ event(t_eval[0], model.y0, inputs) for event in model.terminate_events_eval ])) else: init_event_signs = np.sign([]) if has_symbolic_inputs: # Create integrator without grid to avoid having to create several times self.create_integrator(model, inputs) solution = self._run_integrator(model, model.y0, inputs_dict, inputs, t_eval, use_grid=False) solution.termination = "final time" return solution elif self.mode in ["fast", "fast with events"] or not model.events: if not model.events: pybamm.logger.info("No events found, running fast mode") if self.mode == "fast with events": # Create the integrator with an event switch that will set the rhs to # zero when voltage limits are crossed use_event_switch = True else: use_event_switch = False # Create an integrator with the grid (we just need to do this once) self.create_integrator(model, inputs, t_eval, use_event_switch=use_event_switch) solution = self._run_integrator(model, model.y0, inputs_dict, inputs, t_eval) # Check if the sign of an event changes, if so find an accurate # termination point and exit solution = self._solve_for_event(solution, init_event_signs) return solution elif self.mode in ["safe", "safe without grid"]: y0 = model.y0 # Step-and-check t = t_eval[0] t_f = t_eval[-1] pybamm.logger.debug("Start solving {} with {}".format( model.name, self.name)) if self.mode == "safe without grid": # in "safe without grid" mode, # create integrator once, without grid, # to avoid having to create several times self.create_integrator(model, inputs) # Initialize solution solution = pybamm.Solution(np.array([t]), y0, model, inputs_dict) solution.solve_time = 0 solution.integration_time = 0 use_grid = False else: solution = None use_grid = True # Try to integrate in global steps of size dt_max. Note: dt_max must # be at least as big as the the biggest step in t_eval (multiplied # by some tolerance, here 1.01) to avoid an empty integration window below if self.dt_max: # Non-dimensionalise provided dt_max dt_max = self.dt_max / model.timescale_eval else: dt_max = 0.01 dt_eval_max = np.max(np.diff(t_eval)) * 1.01 dt_max = np.max([dt_max, dt_eval_max]) while t < t_f: # Step solved = False count = 0 dt = dt_max while not solved: # Get window of time to integrate over (so that we return # all the points in t_eval, not just t and t+dt) t_window = np.concatenate( ([t], t_eval[(t_eval > t) & (t_eval < t + dt)])) # Sometimes near events the solver fails between two time # points in t_eval (i.e. no points t < t_i < t+dt for t_i # in t_eval), so we simply integrate from t to t+dt if len(t_window) == 1: t_window = np.array([t, t + dt]) if self.mode == "safe": # update integrator with the grid self.create_integrator(model, inputs, t_window) # Try to solve with the current global step, if it fails then # halve the step size and try again. try: current_step_sol = self._run_integrator( model, y0, inputs_dict, inputs, t_window, use_grid=use_grid) solved = True except pybamm.SolverError: dt /= 2 # also reduce maximum step size for future global steps dt_max = dt count += 1 if count >= self.max_step_decrease_count: raise pybamm.SolverError( "Maximum number of decreased steps occurred at t={}. Try " "solving the model up to this time only or reducing dt_max " "(currently, dt_max={})." "".format(t * model.timescale_eval, dt_max * model.timescale_eval)) # Check if the sign of an event changes, if so find an accurate # termination point and exit current_step_sol = self._solve_for_event( current_step_sol, init_event_signs) # assign temporary solve time current_step_sol.solve_time = np.nan # append solution from the current step to solution solution = solution + current_step_sol if current_step_sol.termination == "event": break else: # update time t = t_window[-1] # update y0 y0 = solution.all_ys[-1][:, -1] return solution
def setup_initial_problem_solver(self): """Sets up the initial problem solver, for finding slack and virtual variables before the solver should run.""" # Test if we don't need to do anything shortcut = self.skill_spec._has_virtual is None shortcut = shortcut and self.skill_spec.slack_var is None if shortcut: # If no slack, and no virtual, nothing to initializes self._has_initial = False return None # Prepare variables time_var = self.skill_spec.time_var robot_var = self.skill_spec.robot_var robot_vel_var = self.skill_spec.robot_vel_var virtual_var = self.skill_spec.virtual_var virtual_vel_var = self.skill_spec.virtual_vel_var input_var = self.skill_spec.input_var slack_var = self.skill_spec.slack_var nvirt = self.skill_spec.n_virtual_var nslack = self.skill_spec.n_slack_var mu = self.weight_shifter # Prepare cost expression opt_var = [] opt_weights = [] if nvirt > 0: opt_var += [virtual_vel_var] opt_weights += [mu * self.virtual_var_weights] if nslack > 0: opt_var += [slack_var] opt_weights += [(1 + mu) * self.slack_var_weights] H_expr = cs.diag(cs.vertcat(*opt_weights)) # Prepare constraints expressions cnstr_expr_list = [] lb_cnstr_expr_list = [] ub_cnstr_expr_list = [] slack_ind = 0 virt_ind = 0 for cnstr in self.skill_spec.constraints: found_virt = False found_slack = False expr_size = cnstr.expression.size() # Look for virtual variables if nvirt > 0: J_virt = cs.jacobian(cnstr.expression, virtual_var) if J_virt.nnz() > 0: # if it has non-zero elements cnstr_expr = J_virt found_virt = True virt_ind += 1 else: cnstr_expr = cs.DM.zeros((expr_size[0], nvirt)) # Setup bounds/functions for numerics rob_der = cnstr.jtimes(robot_var, robot_vel_var) lb_cnstr_expr = -cnstr.jacobian(time_var) - rob_der ub_cnstr_expr = -cnstr.jacobian(time_var) - rob_der 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 # Look for slack variables if nslack > 0: slack_mat = cs.DM.zeros((expr_size[0], nslack)) 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] found_slack = True if nvirt > 0: cnstr_expr = cs.horzcat(cnstr_expr, slack_mat) else: cnstr_expr = slack_mat # Only care about this expression if it's actually relevant if (found_virt or found_slack): cnstr_expr_list += [cnstr_expr] lb_cnstr_expr_list += [lb_cnstr_expr] ub_cnstr_expr_list += [ub_cnstr_expr] if slack_ind == 0 and virt_ind == 0: # Didn't find any of them.. return self._has_initial = False return None A_expr = cs.vertcat(*cnstr_expr_list) Blb_expr = cs.vertcat(*lb_cnstr_expr_list) Bub_expr = cs.vertcat(*ub_cnstr_expr_list) currval_vars = [time_var, robot_var, robot_vel_var] currval_names = ["time_var", "robot_var", "robot_vel_var"] if self.skill_spec._has_virtual: currval_vars += [virtual_var] currval_names += ["virtual_var"] if self.skill_spec._has_input: currval_vars += [input_var] currval_names += ["input_var"] func_opts = self.options["function_opts"] self._initial_problem = { "H": cs.Function("H_initial", currval_vars, [H_expr], currval_names, ["H"], func_opts), "A": cs.Function("A_initial", currval_vars, [A_expr], currval_names, ["A"], func_opts), "Blb": cs.Function("Blb_initial", currval_vars, [Blb_expr], currval_names, ["Blb"], func_opts), "Bub": cs.Function("Bub_initial", currval_vars, [Bub_expr], currval_names, ["Bub"], func_opts) } self.initial_solver = cs.conic("solver", self.options["solver_name"], { "h": H_expr.sparsity(), "a": A_expr.sparsity() }, self.options["initial_solver_opts"]) self._has_initial = True
def solve(self, time_var, robot_var, virtual_var=None, input_var=None, warmstart_robot_vel_var=None, warmstart_virtual_vel_var=None, warmstart_slack_var=None): """Solve the skill specification. """ # Useful sizes nrob = self.skill_spec.n_robot_var nvirt = self.skill_spec.n_virtual_var nslack = self.skill_spec.n_slack_var has_virtual = self.skill_spec._has_virtual has_input = self.skill_spec._has_input # Pack current values currvals = [time_var, robot_var] if virtual_var is not None and has_virtual: currvals += [virtual_var] if input_var is not None and has_input: currvals += [input_var] # Get numerics H = self.H_func(*currvals) A = self.A_func(*currvals) Blb = self.Blb_func(*currvals) Bub = self.Bub_func(*currvals) # Do we have warmstart? ws_rob = warmstart_robot_vel_var is not None ws_virt = warmstart_virtual_vel_var is not None and has_virtual ws_slack = warmstart_slack_var is not None and nslack > 0 if not (ws_rob or ws_virt or ws_slack): # If no warmstart, then just calculate results self.res = self.solver(h=H, a=A, lba=Blb, uba=Bub) else: # Pack warmstart vector warmstart = [] if ws_rob: warmstart += [warmstart_robot_vel_var] else: warmstart += [cs.DM.zeros(nrob)] if ws_virt: warmstart += [warmstart_virtual_vel_var] else: if nvirt > 0: warmstart += [cs.DM.zeros(nvirt)] if ws_slack: warmstart += [warmstart_slack_var] else: if nslack > 0: warmstart += [cs.DM.zeros(nslack)] # Calculate results self.res = self.solver(x0=cs.vertcat(*warmstart), h=H, a=A, lba=Blb, uba=Bub) res_robot_vel = self.res["x"][:nrob] if nvirt > 0 and has_virtual: res_virtual_vel = self.res["x"][nrob:nrob + nvirt] else: res_virtual_vel = None if nslack > 0: if not has_virtual: # handles user error when user adds virtual_var # but it's not actually in the expressions nvirt = 0 res_slack = self.res["x"][nrob + nvirt:nrob + nvirt + nslack] else: res_slack = None return res_robot_vel, res_virtual_vel, res_slack
def generate_c_code_external_cost(model, stage_type): casadi_version = CasadiMeta.version() casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") if casadi_version not in (ALLOWED_CASADI_VERSIONS): casadi_version_warning(casadi_version) x = model.x p = model.p if isinstance(x, MX): symbol = MX.sym else: symbol = SX.sym if stage_type == 'terminal': suffix_name = "_cost_ext_cost_e_fun" suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_e_fun_jac" u = symbol("u", 0, 0) ext_cost = model.cost_expr_ext_cost_e elif stage_type == 'path': suffix_name = "_cost_ext_cost_fun" suffix_name_hess = "_cost_ext_cost_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_fun_jac" u = model.u ext_cost = model.cost_expr_ext_cost elif stage_type == 'initial': suffix_name = "_cost_ext_cost_0_fun" suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_0_fun_jac" u = model.u ext_cost = model.cost_expr_ext_cost_0 # set up functions to be exported fun_name = model.name + suffix_name fun_name_hess = model.name + suffix_name_hess fun_name_jac = model.name + suffix_name_jac # generate expression for full gradient and Hessian full_hess, grad = hessian(ext_cost, vertcat(u, x)) ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) ext_cost_fun_jac_hess = Function(fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]) ext_cost_fun_jac = Function(fun_name_jac, [x, u, p], [ext_cost, grad]) # generate C code if not os.path.exists("c_generated_code"): os.mkdir("c_generated_code") os.chdir("c_generated_code") gen_dir = model.name + '_cost' if not os.path.exists(gen_dir): os.mkdir(gen_dir) gen_dir_location = "./" + gen_dir os.chdir(gen_dir_location) ext_cost_fun.generate(fun_name, casadi_opts) ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) os.chdir("../..") return
# Adjust the relevant constraints. for t in range(Nt): varlb["u",t,:] = ulb varub["u",t,:] = uub # Now build up constraints and objective. obj = casadi.SX(0) con = [] for t in range(Nt): con.append(ode_rk4_casadi(var["x",t], var["u",t]) - var["x",t+1]) obj += l(var["x",t], var["u",t]) obj += Pf(var["x",Nt]) # Build solver object. con = casadi.vertcat(*con) conlb = np.zeros((Nx*Nt,)) conub = np.zeros((Nx*Nt,)) nlp = dict(x=var, f=obj, g=con) nlpoptions = { "ipopt" : { "print_level" : 0, "max_cpu_time" : 60, "linear_solver" : "ma27", # Comment this line if you don't have MA27 "max_iter" : 100, }, "print_time" : False, } solver = casadi.nlpsol("solver",
import casadi import numpy as np import matplotlib.pyplot as plt CONSTANT_ACCELERATION = 0.1 TF = 10*np.pi t = casadi.SX.sym('t') q = casadi.SX.sym('q') qd = casadi.SX.sym('qd') state = casadi.vertcat(q, qd) rhs = casadi.vertcat(qd, casadi.sin(t)) dae = {'x': state, 't': t, 'ode': rhs} ts = np.linspace(0, TF, 10000) integrator = casadi.integrator('integrator', 'cvodes', dae, {'grid':ts, 'output_t0':True}) sol = integrator(x0=[0, -1.0]) import pdb # pdb.set_trace() sol_q = np.array(sol['xf'][0, :]).flatten() sol_qd = np.array(sol['xf'][1, :]).flatten() plt.plot(ts, sol_q, 'r-', label='Position')
[2.0, 3.5, 0.0, 0.5, 0.3], [3.5, 1.5, ca.pi, 0.7, 0.2], [2.0, 2.0, -ca.pi, 0.6, 0.3]]) n_MO = len(MO_init[:, 0]) SO_init = np.array([[1.0, 3.0, 0.3], [9.0, 1.5, 0.1], [2.0, 2.0, 0.3], [6.0, 2.5, 0.2]]) n_SO = len(SO_init[:, 0]) # System Model x = ca.SX.sym('x') y = ca.SX.sym('y') theta = ca.SX.sym('theta') states = ca.vertcat(x, y, theta) n_states = 3 # len([states]) # Control system v = ca.SX.sym('v') omega = ca.SX.sym('omega') controls = ca.vertcat(v, omega) n_controls = 2 # len([controls]) rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega) # Obstacle states in each predictions MOx = ca.SX.sym('MOx') MOy = ca.SX.sym('MOy') MOth = ca.SX.sym('MOth') MOv = ca.SX.sym('MOv')
def gen_long_mpc_solver(): ocp = AcadosOcp() ocp.model = gen_long_model() Tf = T_IDXS[-1] # set dimensions ocp.dims.N = N # set cost module ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' QR = np.zeros((COST_DIM, COST_DIM)) Q = np.zeros((COST_E_DIM, COST_E_DIM)) ocp.cost.W = QR ocp.cost.W_e = Q x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2] j_ego = ocp.model.u[0] a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] prev_a = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) desired_dist_comfort = get_safe_obstacle_distance(v_ego) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car # or other object. In e2e mode we can use x_position targets as a cost # instead. costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), x_ego, v_ego, a_ego, 20 * (a_ego - prev_a), j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) # Constraints on speed, acceleration and desired distance to # the obstacle, which is treated as a slack constraint so it # behaves like an assymetrical cost. constraints = vertcat((v_ego), (a_ego - a_min), (a_max - a_ego), ((x_obstacle - x_ego) - (3 / 4) * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) ocp.cost.zl = cost_weights ocp.cost.Zl = cost_weights ocp.cost.Zu = cost_weights ocp.cost.zu = cost_weights ocp.constraints.lh = np.zeros(CONSTR_DIM) ocp.constraints.lh_e = np.zeros(CONSTR_DIM) ocp.constraints.uh = 1e4 * np.ones(CONSTR_DIM) ocp.constraints.uh_e = 1e4 * np.ones(CONSTR_DIM) ocp.constraints.idxsh = np.arange(CONSTR_DIM) # The HPIPM solver can give decent solutions even when it is stopped early # Which is critical for our purpose where the compute time is strictly bounded # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This # does not cause issues since the problem is well bounded. ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' ocp.solver_options.qp_solver_cond_N = N // 4 # More iterations take too much time and less lead to inaccurate convergence in # some situations. Ideally we would run just 1 iteration to ensure fixed runtime. ocp.solver_options.qp_solver_iter_max = 10 # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.shooting_nodes = T_IDXS ocp.code_export_directory = EXPORT_DIR return ocp
def system_dynamics(x, u): return casadi.vertcat(x[2], x[3], u[0], u[1])
def steer(self, from_node, to_node): if self.mode == 'mpc': # steer tree to desired node (using 1. MPC 2. LMPC 3. Feedback policy) horizon = 5 temp_node = Node(self.node_arr[from_node].state) dist = self.get_dist(temp_node, to_node) n = self.env.observation_space.shape[0] d = self.env.action_space.shape[0] # while(dist >= 0.1): # define variables x = ca.SX.sym('x', (horizon + 1) * n) u = ca.SX.sym('u', horizon * d) # define cost cost = 0 for i in range(horizon): cost += ca.norm_2(x[(i + 1) * n:(i + 2) * n] - to_node.state) # define constraints current_state = temp_node.state constraints = [] for j in range(horizon): next_state = (self.env.A @ (current_state)) + ( self.env.B @ (u[j * d:(j + 1) * d])) constraints = ca.vertcat(constraints, x[(j + 1) * n + 0] == next_state[0]) constraints = ca.vertcat(constraints, x[(j + 1) * n + 1] == next_state[1]) constraints = ca.vertcat(constraints, x[(j + 1) * n + 2] == next_state[2]) constraints = ca.vertcat(constraints, x[(j + 1) * n + 3] == next_state[3]) lbg = [0] * (horizon) * n ubg = [0] * (horizon) * n # solve opts = {'verbose': False, 'ipopt.print_level': 0, 'print_time': 0} nlp = {'x': ca.vertcat(x, u), 'f': cost, 'g': constraints} solver = ca.nlpsol('solver', 'ipopt', nlp, opts) lbx = current_state.tolist( ) + [-100] * n * horizon + [-1] * d * horizon ubx = current_state.tolist() + [100 ] * n * horizon + [1] * d * horizon sol = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) sol_val = np.array(sol['x']) u_star = sol_val[(horizon + 1) * n:(horizon + 1) * n + d] new_node = Node() new_node.state = self.env._next_state(current_state, u_star.reshape(-1)) new_node.parent = self.node_arr[from_node] if self.mode == 'lqr': n = self.env.observation_space.shape[0] d = self.env.action_space.shape[0] # get lqr gains Q = np.eye(n) R = 50 * np.eye(d) A = self.env.A B = self.env.B P = la.solve_discrete_are(A, B, Q, R) Ks = -np.linalg.inv(R + B.T.dot(P).dot(B)).dot(B.T).dot(P).dot(A) # obtain optimal control action start_state = self.node_arr[from_node].state goal_state = to_node.state u_star = np.dot(Ks, (start_state - goal_state)) u_star = np.clip(u_star, -1, 1) # obtain new node new_node = Node() if len(self.node_arr[from_node].state_arr) == 0: self.node_arr[from_node].state_arr.append(start_state) # for j in range(len(self.node_arr[from_node].state_arr)): for i in range(self.n_states): new_node.state_arr.append( self.env._next_state(start_state, u_star)) new_node.state = sum(new_node.state_arr) / len(new_node.state_arr) new_node.parent = self.node_arr[from_node] new_node.cost = self.node_arr[from_node].cost + 1 return new_node
def _run_integrator(self, model, y0, inputs_dict, inputs, t_eval, use_grid=True): pybamm.logger.debug("Running CasADi integrator") if use_grid is True: t_eval_shifted = t_eval - t_eval[0] t_eval_shifted_rounded = np.round(t_eval_shifted, decimals=12).tobytes() integrator = self.integrators[model][t_eval_shifted_rounded] else: integrator = self.integrators[model]["no grid"] len_rhs = model.concatenated_rhs.size y0_diff = y0[:len_rhs] y0_alg = y0[len_rhs:] try: # Try solving if use_grid is True: t_min = t_eval[0] inputs_with_tmin = casadi.vertcat(inputs, t_min) # Call the integrator once, with the grid timer = pybamm.Timer() casadi_sol = integrator(x0=y0_diff, z0=y0_alg, p=inputs_with_tmin, **self.extra_options_call) integration_time = timer.time() y_sol = casadi.vertcat(casadi_sol["xf"], casadi_sol["zf"]) sol = pybamm.Solution(t_eval, y_sol, model, inputs_dict) sol.integration_time = integration_time return sol else: # Repeated calls to the integrator x = y0_diff z = y0_alg y_diff = x y_alg = z for i in range(len(t_eval) - 1): t_min = t_eval[i] t_max = t_eval[i + 1] inputs_with_tlims = casadi.vertcat(inputs, t_min, t_max) timer = pybamm.Timer() casadi_sol = integrator(x0=x, z0=z, p=inputs_with_tlims, **self.extra_options_call) integration_time = timer.time() x = casadi_sol["xf"] z = casadi_sol["zf"] y_diff = casadi.horzcat(y_diff, x) if not z.is_empty(): y_alg = casadi.horzcat(y_alg, z) if z.is_empty(): sol = pybamm.Solution(t_eval, y_diff, model, inputs_dict) else: y_sol = casadi.vertcat(y_diff, y_alg) sol = pybamm.Solution(t_eval, y_sol, model, inputs_dict) sol.integration_time = integration_time return sol except RuntimeError as e: # If it doesn't work raise error raise pybamm.SolverError(e.args[0])
def model(): #=============================================================================== # Variable Assignments #=============================================================================== MP = cs.ssym("MP") MC = cs.ssym("MC") MB = cs.ssym("MB") PC = cs.ssym("PC") CC = cs.ssym("CC") PCP = cs.ssym("PCP") CCP = cs.ssym("CCP") PCC = cs.ssym("PCC") PCN = cs.ssym("PCN") PCCP = cs.ssym("PCCP") PCNP = cs.ssym("PCNP") BC = cs.ssym("BC") BCP = cs.ssym("BCP") BN = cs.ssym("BN") BNP = cs.ssym("BNP") IN = cs.ssym("IN") y = [MP,MC,MB,PC,CC,PCP,CCP,PCC,PCN,PCCP,PCNP,BC,BCP,BN,BNP,IN] #======================================================================= # Parameter Assignments #======================================================================= k1 = cs.ssym("k1") k2 = cs.ssym("k2") k3 = cs.ssym("k3") k4 = cs.ssym("k4") k5 = cs.ssym("k5") k6 = cs.ssym("k6") k7 = cs.ssym("k7") k8 = cs.ssym("k8") KAP = cs.ssym("KAP") KAC = cs.ssym("KAC") KIB = cs.ssym("KIB") kdmb = cs.ssym("kdmb") kdmc = cs.ssym("kdmc") kdmp = cs.ssym("kdmp") kdn = cs.ssym("kdn") kdnc = cs.ssym("kdnc") Kd = cs.ssym("Kd") Kdp = cs.ssym("Kdp") Kp = cs.ssym("Kp") KmB = cs.ssym("KmB") KmC = cs.ssym("KmC") KmP = cs.ssym("KmP") ksB = cs.ssym("ksB") ksC = cs.ssym("ksC") ksP = cs.ssym("ksP") m = cs.ssym("m") n = cs.ssym("n") V1B = cs.ssym("V1B") V1C = cs.ssym("V1C") V1P = cs.ssym("V1P") V1PC = cs.ssym("V1PC") V2B = cs.ssym("V2B") V2C = cs.ssym("V2C") V2P = cs.ssym("V2P") V2PC = cs.ssym("V2PC") V3B = cs.ssym("V3B") V3PC = cs.ssym("V3PC") V4B = cs.ssym("V4B") V4PC = cs.ssym("V4PC") vdBC = cs.ssym("vdBC") vdBN = cs.ssym("vdBN") vdCC = cs.ssym("vdCC") vdIN = cs.ssym("vdIN") vdPC = cs.ssym("vdPC") vdPCC = cs.ssym("vdPCC") vdPCN = cs.ssym("vdPCN") vmB = cs.ssym("vmB") vmC = cs.ssym("vmC") vmP = cs.ssym("vmP") vsB = cs.ssym("vsB") vsC = cs.ssym("vsC") vsP = cs.ssym("vsP") p = [k1, k2, k3, k4, k5, k6, k7, k8, KAP, KAC, KIB, kdmb, kdmc, kdmp, kdn, kdnc, Kd, Kdp, Kp, KmB, KmC, KmP, ksB, ksC, ksP, m, n, V1B, V1C, V1P, V1PC, V2B, V2C, V2P, V2PC, V3B, V3PC, V4B, V4PC, vdBC, vdBN, vdCC, vdIN, vdPC, vdPCC, vdPCN, vmB, vmC, vmP, vsB, vsC, vsP] # Time Variable t = cs.ssym("t") ode = [[]]*NEQ # /* mRNA of per */ ode[0] = ((1 + 0.5*cs.sin(2*np.pi*t/24.))*vsP*pow(BN,n)/(pow(KAP,n) + pow(BN,n)) - vmP*MP/(KmP + MP) - kdmp*MP) # /* mRNA of cry */ ode[1] = (vsC*pow(BN,n)/(pow(KAC,n) + pow(BN,n)) - vmC*MC/(KmC + MC) - kdmc*MC) # /* mRNA of BMAL1 */ ode[2] = (vsB*pow(KIB,m)/(pow(KIB,m) + pow(BN,m)) - vmB*MB/(KmB + MB) - kdmb *MB) # /* protein PER cytosol */ ode[3] = (ksP*MP - V1P*PC/(Kp + PC) + V2P*PCP/(Kdp + PCP) + k4*PCC - k3*PC*CC - kdn*PC) # /* protein CRY cytosol */ ode[4] = (ksC*MC - V1C*CC/(Kp + CC) + V2C*CCP/(Kdp + CCP) + k4*PCC - k3*PC*CC - kdnc*CC) # /* phosphorylated PER cytosol */ ode[5] = (V1P*PC/(Kp + PC) - V2P*PCP/(Kdp + PCP) - vdPC*PCP/(Kdp + PCP) - kdn*PCP) # /* phosphorylated CRY cytosol */ ode[6] = (V1C*CC/(Kp + CC) - V2C*CCP/(Kdp + CCP) - vdCC*CCP/(Kd + CCP) - kdn*CCP) # /* PER:CRY complex cytosol */ ode[7] = ( - V1PC*PCC/(Kp + PCC) + V2PC*PCCP/(Kdp + PCCP) - k4*PCC + k3*PC*CC + k2*PCN - k1*PCC - kdn*PCC) # /* PER:CRY complex nucleus */ ode[8] = ( - V3PC*PCN/(Kp + PCN) + V4PC*PCNP/(Kdp + PCNP) - k2*PCN + k1*PCC - k7*BN*PCN + k8*IN - kdn*PCN) # /* phopshorylated [PER:CRY)c cytosol */ ode[9] = (V1PC*PCC/(Kp + PCC) - V2PC*PCCP/(Kdp + PCCP) - vdPCC*PCCP/(Kd + PCCP) - kdn*PCCP) # /* phosphorylated [PER:CRY)n */ ode[10] = (V3PC*PCN/(Kp + PCN) - V4PC*PCNP/(Kdp + PCNP) - vdPCN*PCNP/(Kd + PCNP) - kdn*PCNP) # /* protein BMAL1 cytosol */ ode[11] = (ksB*MB - V1B*BC/(Kp + BC) + V2B*BCP/(Kdp + BCP) - k5*BC + k6*BN - kdn*BC) # /* phosphorylated BMAL1 cytosol */ ode[12] = (V1B*BC/(Kp + BC) - V2B*BCP/(Kdp + BCP) - vdBC*BCP/(Kd + BCP) - kdn*BCP) # /* protein BMAL1 nucleus */ ode[13] = ( - V3B*BN/(Kp + BN) + V4B*BNP/(Kdp + BNP) + k5*BC - k6*BN - k7*BN*PCN + k8*IN - kdn*BN) # /* phosphorylatd BMAL1 nucleus */ ode[14] = (V3B*BN/(Kp + BN) - V4B*BNP/(Kdp + BNP) - vdBN*BNP/(Kd + BNP) - kdn*BNP) # /* inactive complex between [PER:CRY)n abd [CLOCK:BMAL1)n */ ode[15] = ( - k8*IN + k7*BN*PCN - vdIN*IN/(Kd + IN) - kdn*IN) fn = cs.SXFunction(cs.daeIn(t=t, x=cs.vertcat(y), p=cs.vertcat(p)), cs.daeOut(ode=cs.vertcat(ode))) fn.setOption("name","Leloup 16") return fn
def _solve_for_event(self, coarse_solution, init_event_signs): """ Check if the sign of an event changes, if so find an accurate termination point and exit Locate the event time using a root finding algorithm and event state using interpolation. The solution is then truncated so that only the times up to the event are returned """ pybamm.logger.debug("Solving for events") model = coarse_solution.all_models[-1] inputs_dict = coarse_solution.all_inputs[-1] inputs = casadi.vertcat(*[x for x in inputs_dict.values()]) def find_t_event(sol, typ): # Check most recent y to see if any events have been crossed if model.terminate_events_eval: y_last = sol.all_ys[-1][:, -1] crossed_events = np.sign(init_event_signs * np.concatenate([ event(sol.t[-1], y_last, inputs) for event in model.terminate_events_eval ]) - 1e-5) else: crossed_events = np.sign([]) # Return None if no events have been triggered if (crossed_events == 1).all(): return None, None # get the index of the events that have been crossed event_ind = np.where(crossed_events != 1)[0] active_events = [model.terminate_events_eval[i] for i in event_ind] # loop over events to compute the time at which they were triggered t_events = [None] * len(active_events) event_idcs_lower = [None] * len(active_events) for i, event in enumerate(active_events): # Implement our own bisection algorithm for speed # This is used to find the time range in which the event is triggered # Evaluations of the "event" function are (relatively) expensive init_event_sign = init_event_signs[event_ind[i]][0] f_eval = {} def f(idx): try: return f_eval[idx] except KeyError: # We take away 1e-5 to deal with the case where the event sits # exactly on zero, as can happen when the event switch is used # (fast with events mode) f_eval[idx] = ( init_event_sign * event(sol.t[idx], sol.y[:, idx], inputs) - 1e-5) return f_eval[idx] def integer_bisect(): a_n = 0 b_n = len(sol.t) - 1 for _ in range(len(sol.t)): if a_n + 1 == b_n: return a_n m_n = (a_n + b_n) // 2 f_m_n = f(m_n) if np.isnan(f_m_n): a_n = a_n b_n = m_n elif f_m_n < 0: a_n = a_n b_n = m_n elif f_m_n > 0: a_n = m_n b_n = b_n event_idx_lower = integer_bisect() if typ == "window": event_idcs_lower[i] = event_idx_lower elif typ == "exact": # Linear interpolation between the two indices to find the root time # We could do cubic interpolation here instead but it would be # slower t_lower = sol.t[event_idx_lower] t_upper = sol.t[event_idx_lower + 1] event_lower = abs(f(event_idx_lower)) event_upper = abs(f(event_idx_lower + 1)) t_events[i] = (event_lower * t_upper + event_upper * t_lower) / (event_lower + event_upper) if typ == "window": event_idx_lower = np.nanmin(event_idcs_lower) return event_idx_lower, None elif typ == "exact": # t_event is the earliest event triggered t_event = np.nanmin(t_events) # create interpolant to evaluate y in the current integration # window y_sol = interp1d(sol.t, sol.y, kind="linear") y_event = y_sol(t_event) return t_event, y_event # Find the interval in which the event was triggered event_idx_lower, _ = find_t_event(coarse_solution, "window") # Return the existing solution if no events have been triggered if event_idx_lower is None: # Flag "final time" for termination self.check_interpolant_extrapolation(model, coarse_solution) coarse_solution.termination = "final time" return coarse_solution # If events have been triggered, we solve for a dense window in the interval # where the event was triggered, then find the precise location of the event # Solve again with a more dense idx_window, starting from the start of the # window where the event was triggered t_window_event_dense = np.linspace( coarse_solution.t[event_idx_lower], coarse_solution.t[event_idx_lower + 1], 100, ) if self.mode == "safe without grid": use_grid = False else: self.create_integrator(model, inputs, t_window_event_dense) use_grid = True y0 = coarse_solution.y[:, event_idx_lower] dense_step_sol = self._run_integrator(model, y0, inputs_dict, inputs, t_window_event_dense, use_grid=use_grid) # Find the exact time at which the event was triggered t_event, y_event = find_t_event(dense_step_sol, "exact") # If this returns None, no event was crossed in dense_step_sol. This can happen # if the event crossing was right at the end of the interval in the coarse # solution. In this case, return the t and y from the end of the interval # (i.e. next point in the coarse solution) if y_event is None: # pragma: no cover # This is extremely rare, it's difficult to find a test that triggers this # hence no coverage check t_event = coarse_solution.t[event_idx_lower + 1] y_event = coarse_solution.y[:, event_idx_lower + 1].full().flatten() # Return solution truncated at the first coarse event time # Also assign t_event t_sol = coarse_solution.t[:event_idx_lower + 1] y_sol = coarse_solution.y[:, :event_idx_lower + 1] solution = pybamm.Solution( t_sol, y_sol, model, inputs_dict, np.array([t_event]), y_event[:, np.newaxis], "event", ) solution.integration_time = (coarse_solution.integration_time + dense_step_sol.integration_time) self.check_interpolant_extrapolation(model, solution) return solution
def setup(self, bending_BC_type="cantilevered"): """ Sets up the problem. Run this last. :return: None (in-place) """ ### Discretize and assign loads # Discretize point_load_locations = [load["location"] for load in self.point_loads] point_load_locations.insert(0, 0) point_load_locations.append(self.length) self.x = cas.vertcat(*[ cas.linspace(point_load_locations[i], point_load_locations[i + 1], self.points_per_point_load) for i in range(len(point_load_locations) - 1) ]) # Post-process the discretization self.n = self.x.shape[0] dx = cas.diff(self.x) # Add point forces self.point_forces = cas.GenMX_zeros(self.n - 1) for i in range(len(self.point_loads)): load = self.point_loads[i] self.point_forces[self.points_per_point_load * (i + 1) - 1] = load["force"] # Add distributed loads self.force_per_unit_length = cas.GenMX_zeros(self.n) self.moment_per_unit_length = cas.GenMX_zeros(self.n) for load in self.distributed_loads: if load["type"] == "uniform": self.force_per_unit_length += load["force"] / self.length elif load["type"] == "point": pass else: raise ValueError( "Bad value of \"type\" for a load within beam.loads!") # Initialize optimization variables log_nominal_diameter = self.opti.variable(self.n) self.opti.set_initial(log_nominal_diameter, cas.log(self.diameter_guess)) self.nominal_diameter = cas.exp(log_nominal_diameter) self.opti.subject_to([log_nominal_diameter > cas.log(self.thickness)]) def trapz(x): out = (x[:-1] + x[1:]) / 2 # out[0] += x[0] / 2 # out[-1] += x[-1] / 2 return out # Mass self.volume = cas.sum1( cas.pi / 4 * trapz((self.nominal_diameter + self.thickness)**2 - (self.nominal_diameter - self.thickness)**2) * dx) self.mass = self.volume * self.density # Mass proxy self.volume_proxy = cas.sum1(cas.pi * trapz(self.nominal_diameter) * dx * self.thickness) self.mass_proxy = self.volume_proxy * self.density # Find moments of inertia self.I = cas.pi / 64 * ( # bending (self.nominal_diameter + self.thickness)**4 - (self.nominal_diameter - self.thickness)**4) self.J = cas.pi / 32 * ( # torsion (self.nominal_diameter + self.thickness)**4 - (self.nominal_diameter - self.thickness)**4) if self.bending: # Set up derivatives self.u = 1 * self.opti.variable(self.n) self.du = 0.1 * self.opti.variable(self.n) self.ddu = 0.01 * self.opti.variable(self.n) self.dEIddu = 1 * self.opti.variable(self.n) self.opti.set_initial(self.u, 0) self.opti.set_initial(self.du, 0) self.opti.set_initial(self.ddu, 0) self.opti.set_initial(self.dEIddu, 0) # Define derivatives self.opti.subject_to([ cas.diff(self.u) == trapz(self.du) * dx, cas.diff(self.du) == trapz(self.ddu) * dx, cas.diff(self.E * self.I * self.ddu) == trapz(self.dEIddu) * dx, cas.diff( self.dEIddu) == trapz(self.force_per_unit_length) * dx + self.point_forces, ]) # Add BCs if bending_BC_type == "cantilevered": self.opti.subject_to([ self.u[0] == 0, self.du[0] == 0, self.ddu[-1] == 0, # No tip moment self.dEIddu[-1] == 0, # No tip higher order stuff ]) else: raise ValueError("Bad value of bending_BC_type!") # Stress self.stress_axial = (self.nominal_diameter + self.thickness) / 2 * self.E * self.ddu if self.torsion: # Set up derivatives phi = 0.1 * self.opti.variable(self.n) dphi = 0.01 * self.opti.variable(self.n) # Add forcing term ddphi = -self.moment_per_unit_length / (self.G * self.J) self.stress = self.stress_axial self.opti.subject_to([ self.stress / self.max_allowable_stress < 1, self.stress / self.max_allowable_stress > -1, ])
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 generate_safety_constraints(self, p_all, q_all, u_0, k_fb_ctrl, k_ff_all): """ Generate all safety constraints Parameters ---------- p_all: q_all: k_fb_0: k_fb_ctrl: k_ff: ctrl_bounds: Returns ------- g: list[casadi.SX] lbg: list[casadi.SX] ubg: list[casadi.SX] """ g = [] lbg = [] ubg = [] g_name = [] H = np.shape(p_all)[0] # control constraints if self.has_ctrl_bounds: g_u_0, lbg_u_0, ubg_u_0 = self._generate_control_constraint(u_0) g = vertcat(g, g_u_0) lbg += lbg_u_0 ubg += ubg_u_0 g_name += ["u_0_ctrl_constraint"] for i in range(H - 1): p_i = p_all[i, :].T q_i = q_all[i, :].reshape((self.n_s, self.n_s)) k_ff_i = k_ff_all[i, :].reshape((self.n_u, 1)) k_fb_i = k_fb_ctrl g_u_i, lbg_u_i, ubg_u_i = self._generate_control_constraint(k_ff_i, q_i, k_fb_i) g = vertcat(g, g_u_i) lbg += lbg_u_i ubg += ubg_u_i g_name += ["ellipsoid_ctrl_constraint_{}".format(i)] * len(lbg_u_i) # intermediate state constraints if not self.h_mat_obs is None: for i in range(H - 1): p_i = p_all[i, :].T q_i = q_all[i, :].reshape((self.n_s, self.n_s)) g_state = lin_ellipsoid_safety_distance(p_i, q_i, self.h_mat_obs, self.h_obs) g = vertcat(g, g_state) lbg += [-cas.inf] * self.m_obs ubg += [0] * self.m_obs g_name += ["obstacle_avoidance_constraint{}".format(i)] * self.m_obs # terminal state constraint p_T = p_all[-1, :].T q_T = q_all[-1, :].reshape((self.n_s, self.n_s)) g_terminal = lin_ellipsoid_safety_distance(p_T, q_T, self.h_mat_safe, self.h_safe) g = vertcat(g, g_terminal) g_name += ["terminal constraint"] * self.m_safe lbg += [-cas.inf] * self.m_safe ubg += [0] * self.m_safe return g, lbg, ubg, g_name
# In[103]: # Build the constraints g = equations lbg = [0.0] * len(g) ubg = lbg.copy() # Build the objective f = 0.0 for n in nodes.values(): f += n.objective**2 # In[104]: # Construct the qp, and solver qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)} solver = ca.qpsol('qp', 'cplex', qp, {}) # In[105]: results = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) # In[112]: total_shortage = 0 max_shortage = 0 i = 0 for n in nodes.values(): if n.q_control is not None:
def init_solver(self, cost_func=None): """ Generate the exloration NLP in casadi Parameters ---------- T: int, optional the safempc horizon """ u_0 = MX.sym("init_control", (self.n_u, 1)) k_ff_all = MX.sym("feed-forward control", (self.T - 1, self.n_u)) g = [] lbg = [] ubg = [] g_name = [] p_0 = MX.sym("initial state", (self.n_s, 1)) k_fb_safe_ctrl = MX.sym("Feedback term", (self.n_u, self.n_s)) p_all, q_all, gp_sigma_pred_safe_all = cas_multistep(p_0, u_0, k_fb_safe_ctrl, k_ff_all, self.gp.get_forward_model_casadi(True), self.l_mu, self.l_sigma, self.beta_safety, self.a, self.b, self.lin_trafo_gp_input) # generate open_loop trajectory function [vertcat(x_0,u_0)],[f_x]) self.f_multistep_eval = cas.Function("safe_multistep", [p_0, u_0, k_fb_safe_ctrl, k_ff_all], [p_all, q_all]) g_safe, lbg_safe, ubg_safe, g_names_safe = self.generate_safety_constraints( p_all, q_all, u_0, k_fb_safe_ctrl, k_ff_all) g = vertcat(g, g_safe) lbg += lbg_safe ubg += ubg_safe g_name += g_names_safe if cost_func is None: cost = -sum1(sum2(gp_sigma_pred_safe_all)) else: cost = cost_func(p_all, q_all, gp_sigma_pred_safe_all, u_0, k_ff_all) opt_vars = vertcat(p_0, u_0, k_ff_all.reshape((-1, 1))) opt_params = vertcat(k_fb_safe_ctrl.reshape((-1, 1))) prob = {'f': cost, 'x': opt_vars, 'p': opt_params, 'g': g} opt = {'error_on_fail': False, 'ipopt': {'hessian_approximation': 'exact', "max_iter": 120, "expect_infeasible_problem": "no", \ 'acceptable_tol': 1e-4, "acceptable_constr_viol_tol": 1e-5, "bound_frac": 0.5, "start_with_resto": "no", "required_infeasibility_reduction": 0.85, "acceptable_iter": 8}} # ipopt # opt = {'qpsol':'qpoases','max_iter':120,'hessian_approximation':'exact'}#,"c1":5e-4} #sqpmethod #,'hessian_approximation':'limited-memory' # opt = {'max_iter':120,'qpsol':'qpoases'} self.solver = cas.nlpsol('solver', 'ipopt', prob, opt) self.lbg = lbg self.ubg = ubg
def get_derivative(self, s): # Case 1: s is a constant, e.g. MX(5) if ca.MX(s).is_constant(): return 0 # Case 2: s is a symbol, e.g. MX(x) elif s.is_symbolic(): if s.name() not in self.derivative: if len(self.for_loops ) > 0 and s in self.for_loops[-1].indexed_symbols: # Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol. for_loop_symbol = self.for_loops[-1].indexed_symbols[s] s_without_index = self.get_mx( ast.ComponentRef(name=for_loop_symbol.tree.name)) der_s_without_index = self.get_derivative(s_without_index) if ca.MX(der_s_without_index).is_symbolic(): return self.get_indexed_symbol( ast.ComponentRef( name=der_s_without_index.name(), indices=for_loop_symbol.tree.indices), der_s_without_index) else: return 0 else: der_s = _new_mx("der({})".format(s.name()), s.size()) # If the derivative contains an expression (e.g. der(x + y)) this method is # called with MX variables that are the result of a ca.symvar call. This # ca.symvar call strips the _modelica_shape field from the MX variable, # therefore we need to find the original MX to get the modelica shape. der_s._modelica_shape = \ self.nodes[self.current_class][s.name()]._modelica_shape self.derivative[s.name()] = der_s self.nodes[self.current_class][der_s.name()] = der_s return der_s else: return self.derivative[s.name()] # Case 3: s is an already indexed symbol, e.g. MX(x[1]) elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic(): slice_info = s.info()['slice'] dep = s.dep() if dep.name() not in self.derivative: der_dep = _new_mx("der({})".format(dep.name()), dep.size()) der_dep._modelica_shape = \ self.nodes[self.current_class][dep.name()]._modelica_shape self.derivative[dep.name()] = der_dep self.nodes[self.current_class][der_dep.name()] = der_dep return der_dep[ slice_info['start']:slice_info['stop']:slice_info['step']] else: return self.derivative[dep.name( )][slice_info['start']:slice_info['stop']:slice_info['step']] # Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2) # Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2) else: # Differentiate expression using CasADi orig_deps = ca.symvar(s) deps = ca.vertcat(*orig_deps) J = ca.Function('J', [deps], [ca.jacobian(s, deps)]) J_sparsity = J.sparsity_out(0) der_deps = [ self.get_derivative(dep) if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size()) for j, dep in enumerate(orig_deps) ] return ca.mtimes(J(deps), ca.vertcat(*der_deps))
sint = ca.sin( x[1] ) cost = ca.cos( x[1] ) fric = mic * ca.sign( x[2] ) num = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l ) denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) ) ddtheta = num / denom ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp) x_err = x-x_target cost_mat = np.diag( [1,1,1,1,0] ) ode = ca.vertcat([ ddtheta, \ x[0], \ ddx, \ x[2], \ ca.mul( [ x_err.T, cost_mat, x_err ] ) ] ) dae = ca.SXFunction( 'dae', ca.daeIn( x=x, p=u, t=t ), ca.daeOut( ode=ode ) ) # Create an integrator opts = { 'tf': tf/nk } # final time if coll: opts[ 'number_of_finite_elements' ] = 5 opts['interpolation_order'] = 5 opts['collocation_scheme'] = 'legendre' opts['implicit_solver'] = 'kinsol' opts['implicit_solver_options'] = {'linear_solver' : 'csparse'} opts['expand_f'] = True integrator = ca.Integrator('integrator', 'oldcollocation', dae, opts)
def permute(x: SYM, perm: List[int]) -> SYM: """Perumute a vector""" x_s = [] for i in perm: x_s.append(x[i]) return ca.vertcat(*x_s)
def __set_costs(self, ocp): # set weight for states and controls (default: 1.00) # Q = 1.00 * np.eye(self.acados_ocp.dims.nx) # R = 1.00 * np.eye(self.acados_ocp.dims.nu) # self.acados_ocp.cost.W = linalg.block_diag(Q, R) # self.acados_ocp.cost.W_e = Q self.y_ref = [] self.y_ref_end = [] if self.acados_ocp.cost.cost_type == "LINEAR_LS": # set Lagrange terms self.acados_ocp.cost.Vx = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nx)) self.acados_ocp.cost.Vx[: self.acados_ocp.dims.nx, :] = np.eye(self.acados_ocp.dims.nx) Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu)) Vu[self.acados_ocp.dims.nx :, :] = np.eye(self.acados_ocp.dims.nu) self.acados_ocp.cost.Vu = Vu # set Mayer term self.acados_ocp.cost.Vx_e = np.zeros((self.acados_ocp.dims.nx, self.acados_ocp.dims.nx)) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": if ocp.nb_phases != 1: # TODO: Please confirm this raise NotImplementedError("ACADOS with more than one phase is not implemented yet") for i in range(ocp.nb_phases): # TODO: I think ocp.J is missing here (the parameters would be stored there) for j, J in enumerate(ocp.nlp[i]["J"]): if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J]) else: raise RuntimeError("Should we put y_ref = zeros?") elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction: raise RuntimeError("TODO: I may have broken this (is this the right J?)") mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]]) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) if J[0]["target"] is not None: self.y_ref_end.append([J[0]["target"]]) else: raise RuntimeError("TODO: Should we put y_ref_end = zeros?") else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") if self.lagrange_costs.numel(): self.acados_ocp.model.cost_y_expr = self.lagrange_costs else: self.acados_ocp.model.cost_y_expr = SX(1, 1) if self.mayer_costs.numel(): self.acados_ocp.model.cost_y_expr_e = self.mayer_costs else: self.acados_ocp.model.cost_y_expr_e = SX(1, 1) self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0] self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1),)) self.acados_ocp.cost.yref_e = np.zeros((max(self.acados_ocp.dims.ny_e, 1),)) # TODO changed hard coded values below (you can use J["weight"]) Q_ocp = np.zeros((15, 15)) np.fill_diagonal(Q_ocp, 1000) R_ocp = np.zeros((4, 4)) np.fill_diagonal(R_ocp, 1000) self.acados_ocp.cost.W = linalg.block_diag(Q_ocp, R_ocp) self.acados_ocp.cost.W_e = np.zeros((1, 1)) # TODO: Is the following useful? # if len(self.y_ref): # self.y_ref = np.vstack(self.y_ref) # else: # self.y_ref = [np.zeros((1, 1))] * self.ocp # if len(self.y_ref_end): # self.y_ref_end = np.vstack(self.y_ref_end) # else: # self.y_ref_end = np.zeros((1, 1)) elif self.acados_ocp.cost.cost_type == "EXTERNAL": for i in range(ocp.nb_phases): for j in range(len(ocp.nlp[i]["J"])): J = ocp.nlp[i]["J"][j][0] raise RuntimeError("TODO: The target is not right currently") if J["type"] == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J["val"][0] - J["target"][0]) elif J["type"] == ObjectiveFunction.MayerFunction: raise RuntimeError("TODO: I may have broken this (is this the right J?)") mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]]) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") self.acados_ocp.model.cost_expr_ext_cost = sum1(self.lagrange_costs) self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs) else: raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
import casadi as ca import matplotlib.pyplot as plt eqs = rocket_casadi.rocket_equations() x0, p0 = eqs['initialize'](75) dt = 0.1 m_dot = 0.1 t_vect = np.arange(0, 4, dt) n = len(t_vect) elv_vect = ca.SX.sym('elv', n) x = x0 for i in range(n): t = t_vect[i] elv = elv_vect[i] u = ca.vertcat(m_dot, 0, elv, 0) x = eqs['predict'](x, u, p0, t, dt) x_final = x dx_final = eqs['rhs'](x_final, u, p0) s0 = 0 * np.ones(n) x = ca.SX.sym('x') nlp = { 'x': elv_vect, 'f': ca.dot(elv_vect, elv_vect), 'g': ca.vertcat(dx_final[12]) } args = { 'print_time': 1, 'monitor': ['nlp_f', 'nlp_g'],
# constant for this problem m = 1.0 L = 3.0 g = 9.81 # psi = pl.pi / 2.0 psi = pl.pi / (180.0 * 2) # System x = ca.MX.sym("x", 2) p = ca.MX.sym("p", 1) u = ca.MX.sym("u", 1) # f = ca.vertcat([x[1], p[0]/(m*(L**2))*(u-x[0]) - g/L * pl.sin(x[0])]) f = ca.vertcat([x[1], p[0]/(m*(L**2))*(u-x[0]) - g/L * x[0]]) phi = x odesys = pecas.systems.ExplODE(x = x, u = u, p = p, f = f, phi = phi) odesys.show_system_information(showEquations = True) data = pl.loadtxt('data_pendulum.txt') tu = data[:500, 0] numeas = data[:500, 1] wmeas = data[:500, 2] yN = pl.array([numeas,wmeas]) N = tu.size uN = [psi] * (N-1) wv = pl.ones(yN.shape)