def run(self, x0): """ Run simulator with specified system dynamics and control function. """ print("Running simulation....") sim_loop_length = int( self.total_sim_time / self.dt) + 1 # account for 0th t = np.array([0]) y_vec = np.array([x0]).T u_vec = np.array([0, 0, 0, 0]).T # Start figure #fig = plt.figure() plt.rc('grid', linestyle="--", color='black') fig, (ax1, ax2, ax3, ax4) = plt.subplots(4) #fig, (ax3,ax4) = plt.subplots(2) if len(x0) > 12: fig, (ax5) = plt.subplots(1) for i in range(sim_loop_length): # Translate data to ca.DM x = ca.DM(np.size(y_vec, 0), 1).full() x = np.array([y_vec[:, -1]]).T print("Step " + str(i) + "/" + str(sim_loop_length)) try: # Get control input and obtain next state u = self.controller(x) #print("Control input is set as "+str(u)+" ") x_next = self.dynamics(x, u) except RuntimeError: print( "Uh oh, your simulator crashed due to unstable dynamics.\n \ Retry with new controller parameters.") exit() # Store data t = np.append(t, t[-1] + self.dt) y_vec = np.append(y_vec, np.array(x_next), axis=1) u_vec = np.append(u_vec, np.array(u)) # Get plot window values: if self.plt_window != float("inf"): l_wnd = 0 if int(i + 1 - self.plt_window / self.dt) < 1 else int( i + 1 - self.plt_window / self.dt) else: l_wnd = 0 ax1.clear() #ax1.set_title("Quadcopter - Ref: "+str(self.model.x_d)) ax1.set_title("Quadcopter") ax1.plot( t[l_wnd:-1], y_vec[0,l_wnd:-1], 'r-', \ t[l_wnd:-1], y_vec[1,l_wnd:-1], 'b--', \ t[l_wnd:-1], y_vec[2,l_wnd:-1], 'g-') ax1.legend(["X position", "Y position", "Z position"], loc=1) ax1.set_ylabel("meters") ax1.grid() ax2.clear() ax2.plot( t[l_wnd:-1], y_vec[3,l_wnd:-1], 'r-', \ t[l_wnd:-1], y_vec[4,l_wnd:-1], 'b--', \ t[l_wnd:-1], y_vec[5,l_wnd:-1], 'g-') ax2.legend(["X velocity", "Y velocity", "Z velocity"], loc=1) ax2.set_ylabel("m/s") ax2.grid() ax3.clear() ax3.plot( t[l_wnd:-1], ((y_vec[6,l_wnd:-1])*(180/np.pi)), 'r-', \ t[l_wnd:-1], ((y_vec[7,l_wnd:-1])*(180/np.pi)), 'b-', \ t[l_wnd:-1], ((y_vec[8,l_wnd:-1])*(180/np.pi)), 'g-') ax3.legend(["roll angle", "pitch angle", "yaw angle"], loc=1) ax3.set_ylabel("degree") ax3.grid() ax4.clear() ax4.plot( t[l_wnd:-1], (y_vec[9,l_wnd:-1]) , 'r-', \ t[l_wnd:-1], (y_vec[10,l_wnd:-1]), 'b--', \ t[l_wnd:-1], (y_vec[11,l_wnd:-1]), 'g-') ax4.legend(["Omega x", "Omega y", "Omega z"], loc=1) ax4.set_ylabel("rad/s") ax4.grid() if len(x0) > 12: ax5.clear() ax5.plot( t[l_wnd:-1], (y_vec[12,l_wnd:-1]) , 'r-', \ t[l_wnd:-1], (y_vec[13,l_wnd:-1]), 'b--', \ t[l_wnd:-1], (y_vec[14,l_wnd:-1]), 'g-') ax5.legend(["X error", "Y error", "Z error"], loc=1) ax5.set_ylabel("error m") ax5.grid() plt.show() #print(y_vec) return t, y_vec, u_vec
print("Angular velocity: " + str(target_angular_velocity) + " microrad") # Create system model in casadi x = cas.MX.sym('x', spacecraft.num_states, 1) u = cas.MX.sym('u', spacecraft.num_forces, 1) state = PolarOrbiterState(x) thrust = PolarOrbiterThrust(u) state_derivative = spacecraft.ode_scaled(state, thrust).vector ode = cas.Function('ode', [x, u], [spacecraft.ode_scaled(PolarOrbiterState(x), PolarOrbiterThrust(u)).vector], ['state', 'thrust'], ['state_derivative']) initial_state = PolarOrbiterState( cas.DM([ spacecraft.moon_radius, 0, 0, 0, spacecraft.full_mass ])).scale(spacecraft.scale) print(f"Initial state: {initial_state.vector}") # Create an integrator for the ode Xk = x for k in range(integrator_substeps): Xk = rk4step_ode(ode, Xk, u, integrator_stepsize) F = cas.Function('F', [x, u], [Xk], ['x', 'u'], ['xk']) # Create stage cost for the OCP state_cost = u[0] ** 2 + u[1] ** 2 state_cost = cas.Function('l', [x, u], [state_cost], ['x', 'u'], ['l'])
if 'car_w_trailers': X0 = np.array([0, 0, m.pi / 3, 0, 0, 0]) Xg = np.array([1, 4, m.pi / 2, 0, 0, 0]) if FILE_WRITE: filename = "/home/naveed/Dropbox/Research/Data/WAFR20/car_w_trailers/TPFC_1_wo_obs_wo_Txu.csv" file = open(filename, "a") file.write('epsilon' + ',' + 'Average Cost' + ',' + 'Cost variance' + ',' + 'Average Time' + '\n') np.random.seed(4) if Model == 'car_w_trailers': robot = car_w_trailers(dt=0.3) #open-loop optimisation gain R = c.DM([[20, 0], [0, 10]]) Q = 5 * c.DM.eye(robot.nx) Qf = 1000 * c.DM.eye(robot.nx) def SSP(Q, R, Qf): #State space planning control = algo.SSP(T, X0, Xg, R, Q, Qf, params.Xmin, params.Xmax) U_guess = np.zeros((robot.nu, T)) U_guess = np.reshape(U_guess, (robot.nu * T, 1)) print("Solving state space problem...") U_opti_ss = control.solve_SSP(robot, X0, np.zeros(robot.nu), U_guess)
if L1_regularization: hqp.create_constraint(q_dot1 - 0, 'equality', priority=3) hqp.opti.solver( "sqpmethod", { "expand": True, "qpsol": 'qpoases', 'print_iteration': True, 'print_header': True, 'print_status': True, "print_time": True, "record_time": True, 'max_iter': 1000 }) q_opt = cs.vertcat(cs.DM(q0_1), cs.DM.zeros(7, 1)) ee_vel_history = cs.vertcat(cs.DM.zeros(2, 1)) q_opt_history = q_opt q_dot_opt = cs.DM([0] * 7) q_dot_opt_history = q_dot_opt hqp.configure() hqp.time_taken = 0 tic = time.time() constraint_violations = cs.DM([0, 0, 0]) #Setup for visualization visualizationBullet = True counter = 1 if visualizationBullet:
def MPC_cost(v,K,S,S_i,x_i,goal,true_goal,signal_positive,signal_negative): #velocity, Horizon, Steering variables, Steering angle initial, start, goal, true_goal cost = 0 S = casadi.reshape(S,1,K) R = casadi.DM([[0.5,0],[0,0.5]]) # s, s_dot Q = casadi.DM([35]) # heading_error Qf = casadi.DM([100]) X = casadi.MX(3,K) #x,y,theta,heading_error S_vec = casadi.MX(2,1) # s and s_dot for i in range(K): if i==0: X[:,i] = dynamics_rear(x_i[:],S[i],casadi.DM([v])) #calculate new state S_vec[0] = S[i] S_vec[1] = S_i-S[i] else: X[:,i] = dynamics_rear(X[:,i-1],S[i],casadi.DM([v])) #calculate new state S_vec[0] = S[i] S_vec[1] = S[i]-S[i-1] """ Longitudinal control """ x = X[0,i] y = X[1,i] theta = X[2,i] """ heading error """ beta = casadi.atan2((goal[1]-y),(goal[0]-x)) heading_error = casadi.atan2(casadi.sin(beta - theta), casadi.cos(beta - theta)) e = heading_error #may be need MX here if signal_positive: e = (casadi.pi - e) if signal_negative: e = -(casadi.pi - casadi.fabs(e)) cost = cost + (casadi.mtimes(casadi.mtimes(S_vec.T,R),S_vec) + casadi.mtimes(casadi.mtimes(e.T,Q),e)) x = X[0,i] y = X[1,i] theta = X[2,i] beta = casadi.atan2((goal[1]-y),(goal[0]-x)) heading_error = casadi.atan2(casadi.sin(beta - theta), casadi.cos(beta - theta)) e = heading_error #may be need MX here if signal_positive: e = (casadi.pi - e) if signal_negative: e = -(casadi.pi - casadi.fabs(e)) cost = cost + casadi.mtimes(casadi.mtimes(e.T,Q),e) #terminal cost return cost
if 'car_w_trailers': X0 = np.array([0, 0, m.pi / 3, 0, 0, 0]) Xg = np.array([2, 2, 0, 0, 0, 0]) if FILE_WRITE: filename = "/home/naveed/Dropbox/Research/Data/WAFR20/car_w_trailers/TLQR_1_wo_obs_modified1.csv" file = open(filename, "a") file.write('epsilon' + ',' + 'Average Cost' + ',' + 'Cost variance' + ',' + 'Average Time' + '\n') #np.random.seed(4) if Model == 'car_w_trailers': robot = car_w_trailers(dt=0.1) #open-loop optimisation gain R = c.DM([[5, 0], [0, 5]]) Q = c.DM([[10, 0, 0, 0, 0, 0], [0, 10, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) Qf = 20 * c.DM([[50, 0, 0, 0, 0, 0], [0, 50, 0, 0, 0, 0], [0, 0, 50, 0, 0, 0], [0, 0, 0, 5, 0, 0], [0, 0, 0, 0, 5, 0], [0, 0, 0, 0, 0, 5]]) def SSP(Q, R, Qf): #State space planning control = algo.SSP(T, X0, Xg, R, Q, Qf, params.Xmin, params.Xmax) U_guess = np.zeros((robot.nu, T)) U_guess = np.reshape(U_guess, (robot.nu * T, 1))
s_dot2 = tc.create_expression('s_dot2', 'state', (1,1)) s_ddot2 = tc.create_expression('s_ddot2', 'control', (1,1)) tc.ocp.subject_to(0 <= (s_2 <= 1)) tc.set_dynamics(s_2, s_dot2) tc.set_dynamics(s_dot2, s_ddot2) 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 print(robot.fk(q0_1)[6]) print(robot2.fk(q0_2)[6]) #specifying the cartesian trajectory of the two robots as a function #of the progress variable rob1_start_pose = cs.DM([0.3, -0.24, 0.4]) rob1_end_pose = cs.DM([0.6, 0.2, 0.25]) traj1 = rob1_start_pose + cs.fmin(s_1, 1)*(rob1_end_pose - rob1_start_pose) rob2_start_pose = cs.DM([0.3, 0.54, 0.4]) rob2_end_pose = cs.DM([0.6, 0.1, 0.25]) traj2 = rob2_start_pose + cs.fmin(s_2, 1)*(rob2_end_pose - rob2_start_pose) #Highest priority: Hard constraints on joint velocity and joint position limits #Already added by default by TaSHo #2nd priority: Orientation constraints are fixed, And also the TODO: obstacle avoidance # slack_priority2 = opti.variable(6, 1) orientation_rob1 = {'equality':True, 'hard':False, 'expression':fk_vals1, 'reference':robot.fk(q0_1)[6], 'type':'Frame', 'rot_gain':50, 'trans_gain':0, 'norm':'L1'} orientation_rob2 = {'equality':True, 'hard':False, 'expression':fk_vals2, 'reference':robot.fk(q0_2)[6], 'type':'Frame', 'rot_gain':50, 'trans_gain':0, 'norm':'L1'}
def compute_freestream_direction_geometry_axes(self): # Computes the freestream direction (direction the wind is GOING TO) in the geometry axes vel_dir_wind = cas.DM([-1, 0, 0]) vel_dir_geometry = self.compute_rotation_matrix_wind_to_geometry( ) @ vel_dir_wind return vel_dir_geometry
def __init__( self, dynamics, obstacles=[], h=0.05, horizon=10, Q=None, P=None, R=None, S=None, Sl=None, ulb=None, uub=None, xlb=None, xub=None, terminal_constraint=None, solver_opts=None, x_d=[0] * 12, ): """ Initialize and build the MPC solver # Arguments: horizon: Prediction horizon in seconds model: System model # Optional Argumants: Q: State penalty matrix, default=diag(1,...,1) P: Termial penalty matrix, default=diag(1,...,1) R: Input penalty matrix, default=diag(1,...,1)*0.01 ulb: Lower boundry input uub: Upper boundry input xlb: Lower boundry state xub: Upper boundry state terminal_constraint: Terminal condition on the state * if None: No terminal constraint is used * if zero: Terminal state is equal to zero * if nonzero: Terminal state is bounded within +/- the constraint solver_opts: Additional options to pass to the NLP solver e.g.: solver_opts['print_time'] = False solver_opts['ipopt.tol'] = 1e-8 """ self.horizon = horizon build_solver_time = -time.time() self.dt = h self.Nx, self.Nu = 12, 4 # TODO Nopt = self.Nu + self.Nx self.Nt = int(horizon) self.dynamics = dynamics # Initialize variables self.set_cost_functions() # Cost function weights if P is None: P = np.eye(self.Nx) * 10 if Q is None: Q = np.eye(self.Nx) if R is None: R = np.eye(self.Nu) * 0.01 if S is None: S = np.eye(self.Nx) * 1000 if Sl is None: Sl = np.full((self.Nx, ), 1000) self.Q = ca.MX(Q) self.P = ca.MX(P) self.R = ca.MX(R) self.S = ca.MX(S) self.Sl = ca.MX(Sl) if xub is None: xub = np.full((self.Nx), np.inf) if xlb is None: xlb = np.full((self.Nx), -np.inf) if uub is None: uub = np.full((self.Nu), np.inf) if ulb is None: ulb = np.full((self.Nu), -np.inf) self.ulb = ulb self.uub = uub self.terminal_constraint = terminal_constraint # Starting state parameters - add slack here x0 = ca.MX.sym('x0', self.Nx) x_ref = ca.MX.sym('x_ref', self.Nx * (self.Nt + 1)) u0 = ca.MX.sym('u0', self.Nu) param_s = ca.vertcat(x0, x_ref, u0) # Create optimization variables opt_var = ctools.struct_symMX([(ctools.entry('u', shape=(self.Nu, ), repeat=self.Nt), ctools.entry('x', shape=(self.Nx, ), repeat=self.Nt + 1), ctools.entry('s', shape=(self.Nx, ), repeat=1))]) self.opt_var = opt_var self.num_var = opt_var.size # Decision variable boundries self.optvar_lb = opt_var(-np.inf) self.optvar_ub = opt_var(np.inf) """ Set initial values """ obj = ca.MX(0) con_eq = [] con_ineq = [] con_ineq_lb = [] con_ineq_ub = [] con_eq.append(opt_var['x', 0] - x0) # Generate MPC Problem for t in range(self.Nt): # Get variables x_t = opt_var['x', t] u_t = opt_var['u', t] # Dynamics constraint x_t_next = self.dynamics(x_t, u_t) con_eq.append(x_t_next - opt_var['x', t + 1]) # Input constraints if uub is not None: con_ineq.append(u_t) con_ineq_ub.append(uub) con_ineq_lb.append(np.full((self.Nu, ), -ca.inf)) if ulb is not None: con_ineq.append(u_t) con_ineq_ub.append(np.full((self.Nu, ), ca.inf)) con_ineq_lb.append(ulb) # State constraints if xub is not None: con_ineq.append(x_t) con_ineq_ub.append(xub) con_ineq_lb.append(np.full((self.Nx, ), -ca.inf)) if xlb is not None: con_ineq.append(x_t) con_ineq_ub.append(np.full((self.Nx, ), ca.inf)) con_ineq_lb.append(xlb) if obstacles is not None: for obs in obstacles: con_ineq.append(self.distance_obs(x_t, obs)) con_ineq_ub.append(ca.inf) con_ineq_lb.append(ca.DM((obs.eps + obs.radius)**2)) obj += self.running_cost((x_t - x_ref[t * 12:12 + t * 12]), self.Q, u_t, self.R) # Terminal Cost obj += self.terminal_cost( (opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 + (self.Nt) * 12]), self.P) # Terminal contraint s_t = opt_var['s', 0] con_ineq.append((opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 + (self.Nt) * 12])**2 - s_t) con_ineq_lb.append(np.full((self.Nx, ), 0.0)) con_ineq_ub.append(np.full((self.Nx, ), terminal_constraint**2)) obj += self.slack_cost(s_t, self.S, self.Sl) # Equality constraints bounds are 0 (they are equality constraints), # -> Refer to CasADi documentation num_eq_con = ca.vertcat(*con_eq).size1() num_ineq_con = ca.vertcat(*con_ineq).size1() con_eq_lb = np.zeros((num_eq_con, 1)) con_eq_ub = np.zeros((num_eq_con, 1)) # Set constraints con = ca.vertcat(*(con_eq + con_ineq)) self.con_lb = ca.vertcat(con_eq_lb, *con_ineq_lb) self.con_ub = ca.vertcat(con_eq_ub, *con_ineq_ub) # Build NLP Solver (can also solve QP) nlp = dict(x=opt_var, f=obj, g=con, p=param_s) options = { 'ipopt.print_level': 0, 'ipopt.mu_init': 0.01, 'ipopt.tol': 1e-4, 'ipopt.sb': 'yes', 'ipopt.warm_start_init_point': 'yes', 'ipopt.warm_start_bound_push': 1e-3, 'ipopt.warm_start_bound_frac': 1e-3, 'ipopt.warm_start_slack_bound_frac': 1e-3, 'ipopt.warm_start_slack_bound_push': 1e-3, 'ipopt.warm_start_mult_bound_push': 1e-3, 'ipopt.mu_strategy': 'adaptive', 'print_time': False, 'verbose': False, 'expand': True, 'ipopt.max_iter': 100 } if solver_opts is not None: options.update(solver_opts) self.solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options) # self.solver = ca.nlpsol('mpc_solver', 'sqpmethod', nlp, self.sol_options_sqp) build_solver_time += time.time() print('\n________________________________________') print('# Time to build mpc solver: %f sec' % build_solver_time) print('# Number of variables: %d' % self.num_var) print('# Number of equality constraints: %d' % num_eq_con) print('# Number of inequality constraints: %d' % num_ineq_con) print('----------------------------------------') pass
def testRotation(self): rpy = np.random.sample(3) R = euler2mat(rpy[2], rpy[1], rpy[0], axes='rzyx') R_cs = self.dynamics.euler2mat(cs.DM(rpy)).full() np_testing.assert_almost_equal(R, R_cs)
def testOmegaToRpydot(self): omega = np.array([2, 0, 0]) rpydot = self.dynamics.omegaToRpyDot(cs.DM(np.random.sample(3)), cs.DM(omega)).full().ravel() np_testing.assert_almost_equal(rpydot, omega)
# Create discretized system model with CasADi x = cas.MX.sym('x', spacecraft.num_states, 1) u = cas.MX.sym('u', spacecraft.num_forces, 1) state_derivative_fun = cas.Function( 'ode', [x, u], [spacecraft.ode_scaled(PolarOrbiterState(x), PolarOrbiterThrust(u)).vector], ['state', 'thrust'], ['state_derivative']) next_state = rk4step_ode(state_derivative_fun, x, u, timestep) next_state_fun = cas.Function('next_state', [x, u], [next_state], ['x', 'u'], ['xnext']) # Initial state initial_state = PolarOrbiterState( cas.DM([ spacecraft.moon_radius, 0, 0, 0, spacecraft.full_mass ]) ).scale(spacecraft.scale) print(f"Initial state: {initial_state.vector}") # Choose controls for simulation thrusts = np.zeros((spacecraft.num_forces, num_samples)) thrust_radial_stop = 20 thrust_angular_stop = 50 thrusts[0, 0:thrust_radial_stop] = 0.19 * np.ones(thrust_radial_stop) thrusts[1, 0:thrust_angular_stop] = 0.36 * np.ones(thrust_angular_stop) # Simulate the system xs = cas.DM.zeros((spacecraft.num_states, num_samples + 1))
def test_norm_vector(): a = np.array([1, 2, 3]) cas_a = cas.DM(a) assert np.linalg.norm(a) == np.linalg.norm(cas_a)
def _integrate(self, model, t_eval, inputs_dict=None): """ Calculate the solution of the algebraic equations through root-finding Parameters ---------- model : :class:`pybamm.BaseModel` The model whose solution to calculate. t_eval : :class:`numpy.array`, size (k,) The times at which to compute the solution inputs_dict : dict, optional Any input parameters to pass to the model when solving. If any input parameters that are present in the model are missing from "inputs", then the solution will consist of `ProcessedSymbolicVariable` objects, which must be provided with inputs to obtain their value. """ # 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()) symbolic_inputs = casadi.vertcat( *[v for v in inputs_dict.values() if isinstance(v, casadi.MX)]) # Create casadi objects for the root-finder inputs = casadi.vertcat(*[v for v in inputs_dict.values()]) y0 = model.y0 # If y0 already satisfies the tolerance for all t then keep it if has_symbolic_inputs is False and all( np.all( abs(model.casadi_algebraic(t, y0, inputs).full()) < self.tol) for t in t_eval): pybamm.logger.debug("Keeping same solution at all times") return pybamm.Solution(t_eval, y0, model, inputs_dict, termination="success") # The casadi algebraic solver can read rhs equations, but leaves them unchanged # i.e. the part of the solution vector that corresponds to the differential # equations will be equal to the initial condition provided. This allows this # solver to be used for initialising the DAE solvers if model.rhs == {}: len_rhs = 0 y0_diff = casadi.DM() y0_alg = y0 else: len_rhs = model.concatenated_rhs.size y0_diff = y0[:len_rhs] y0_alg = y0[len_rhs:] y_alg = None # Set up t_sym = casadi.MX.sym("t") y_alg_sym = casadi.MX.sym("y_alg", y0_alg.shape[0]) y_sym = casadi.vertcat(y0_diff, y_alg_sym) t_and_inputs_sym = casadi.vertcat(t_sym, symbolic_inputs) alg = model.casadi_algebraic(t_sym, y_sym, inputs) # Check interpolant extrapolation if model.interpolant_extrapolation_events_eval: extrap_event = [ event(0, y0, inputs) for event in model.interpolant_extrapolation_events_eval ] if extrap_event: if (np.concatenate(extrap_event) < self.extrap_tol).any(): extrap_event_names = [] for event in model.events: if (event.event_type == pybamm.EventType.INTERPOLANT_EXTRAPOLATION and (event.expression.evaluate( 0, y0.full(), inputs=inputs) < self.extrap_tol)): extrap_event_names.append(event.name[12:]) raise pybamm.SolverError( "CasADi solver failed because the following interpolation " "bounds were exceeded at the initial conditions: {}. " "You may need to provide additional interpolation points " "outside these bounds.".format(extrap_event_names)) # Set constraints vector in the casadi format # Constrain the unknowns. 0 (default): no constraint on ui, 1: ui >= 0.0, # -1: ui <= 0.0, 2: ui > 0.0, -2: ui < 0.0. constraints = np.zeros_like(model.bounds[0], dtype=int) # If the lower bound is positive then the variable must always be positive constraints[model.bounds[0] >= 0] = 1 # If the upper bound is negative then the variable must always be negative constraints[model.bounds[1] <= 0] = -1 # Set up rootfinder roots = casadi.rootfinder( "roots", "newton", dict(x=y_alg_sym, p=t_and_inputs_sym, g=alg), { **self.extra_options, "abstol": self.tol, "constraints": list(constraints[len_rhs:]), }, ) timer = pybamm.Timer() integration_time = 0 for idx, t in enumerate(t_eval): # Evaluate algebraic with new t and previous y0, if it's already close # enough then keep it # We can't do this if there are symbolic inputs if has_symbolic_inputs is False and np.all( abs(model.casadi_algebraic(t, y0, inputs).full()) < self.tol): pybamm.logger.debug("Keeping same solution at t={}".format( t * model.timescale_eval)) if y_alg is None: y_alg = y0_alg else: y_alg = casadi.horzcat(y_alg, y0_alg) # Otherwise calculate new y_sol else: t_eval_inputs_sym = casadi.vertcat(t, symbolic_inputs) # Solve try: timer.reset() y_alg_sol = roots(y0_alg, t_eval_inputs_sym) integration_time += timer.time() success = True message = None # Check final output y_sol = casadi.vertcat(y0_diff, y_alg_sol) fun = model.casadi_algebraic(t, y_sol, inputs) except RuntimeError as err: success = False message = err.args[0] fun = None # If there are no symbolic inputs, check the function is below the tol # Skip this check if there are symbolic inputs if success and (has_symbolic_inputs is True or (not any(np.isnan(fun)) and np.all(casadi.fabs(fun) < self.tol))): # update initial guess for the next iteration y0_alg = y_alg_sol y0 = casadi.vertcat(y0_diff, y0_alg) # update solution array if y_alg is None: y_alg = y_alg_sol else: y_alg = casadi.horzcat(y_alg, y_alg_sol) elif not success: raise pybamm.SolverError( "Could not find acceptable solution: {}".format( message)) elif any(np.isnan(fun)): raise pybamm.SolverError( "Could not find acceptable solution: solver returned NaNs" ) else: raise pybamm.SolverError(""" Could not find acceptable solution: solver terminated successfully, but maximum solution error ({}) above tolerance ({}) """.format(casadi.mmax(casadi.fabs(fun)), self.tol)) # Concatenate differential part y_diff = casadi.horzcat(*[y0_diff] * len(t_eval)) y_sol = casadi.vertcat(y_diff, y_alg) # Return solution object (no events, so pass None to t_event, y_event) sol = pybamm.Solution([t_eval], y_sol, model, inputs_dict, termination="success") sol.integration_time = integration_time return sol
# You should have received a copy of the GNU Lesser General Public License # along with casiopeia. If not, see <http://www.gnu.org/licenses/>. # This example is an adapted version of the system identification example # included in CasADi, for the original file see: # https://github.com/casadi/casadi/blob/master/docs/examples/python/sysid.py import pylab as pl import casadi as ca import casiopeia as cp N = 10000 fs = 610.1 p_true = ca.DM([5.625e-6, 2.3e-4, 1, 4.69]) p_guess = ca.DM([5, 3, 1, 5]) scale = ca.vertcat(1e-6, 1e-4, 1, 1) x = ca.MX.sym("x", 2) u = ca.MX.sym("u", 1) p = ca.MX.sym("p", 4) f = ca.vertcat( x[1], \ (u - scale[3] * p[3] * x[0]**3 - scale[2] * p[2] * x[0] - \ scale[1] * p[1] * x[1]) / (scale[0] * p[0]), \ ) phi = x
def make_preismann_model(simulation_type, pred_horizon): """ Construct the Preismann model based state space model, using the prediction horizon and the disturbance magnitude :param simulation_type: This is to denote in the state_space_mode|l which type of simulation it is. :param pred_horizon: Length of the prediction horizon :return: Entire state space model of the Preismann model with both the initial conditions and with the weight matrices. """ Ap = ca.DM([[1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [ 0., -0.57690, 0.32141, 0.11616, -0.00806, -1.3343, 0.01643, 1.5745, -0.02942, -1.9671, 0.04957, -1.0914, 0.90771 ], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [ 0., 1.3295, -0.25344, 0.63522, 0.04337, 1.3427, -0.01652, -1.5843, 0.02961, 1.9795, -0.04988, 1.0983, -0.91339 ], [ 0., 5.8510, -1.1153, 5.8806, -1.3277, -0.19762, 0.002434, 0.23319, -0.004359, -0.29133, 0.007338, -0.16164, 0.13443 ], [ 0., -0.92533, 0.17639, 0.52445, -0.06726, -0.6130, 0.05206, 1.6107, -0.03011, -2.0124, 0.05071, -1.1166, 0.92859 ], [ 0., -3.7105, 0.70732, 2.1033, -0.26973, 6.1943, -1.3306, -0.39037, 0.007293, 0.48773, -0.01229, 0.27060, -0.22506 ], [ 0., 0.66951, -0.12762, -0.37947, 0.04867, 1.6334, -0.07625, -0.8962, 0.06604, 2.0594, -0.05189, 1.1426, -0.95026 ], [ 0., 2.3410, -0.44621, -1.3269, 0.17023, 5.7112, -0.26658, 6.3626, -1.3358, -0.62377, 0.01572, -0.34608, 0.28783 ], [ 0., -0.50862, 0.096954, 0.28827, -0.03697, -1.2409, 0.05792, 1.9303, -0.09067, -1.3642, 0.08856, -1.1742, 0.97652 ], [ 0., -1.4643, 0.27913, 0.82986, -0.10640, -3.5725, 0.16679, 5.5571, -0.26105, 6.5851, -1.3447, 0.40035, -0.33296 ], [ 0., 0.44672, -0.085151, -0.25320, 0.032479, 1.0899, -0.050867, -1.6953, 0.079624, 2.6426, -0.12460, 2.1912, -1.5796 ], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]]) Bp = ca.DM([[-11.886, 0., -11.886, 0.], [0.21376, 1.1940, 0., 268.53], [1.0000, 0., 0., 0.], [-0.14457, -1.2015, 0., -270.20], [-0.63619, 0.17683, 0., 39.769], [0.10062, 1.2215, 0., 274.71], [0.40347, -0.29604, 0., -66.578], [-0.072797, -1.2500, 0., -281.12], [-0.25455, 0.37861, 0., 85.147], [0.055305, 1.2845, 0., 288.88], [0.15921, -0.43798, 0., -98.503], [-0.048574, -2.4988, 0., -293.05], [0., 1.0000, 0., 0.]]) Bp_d = ca.DM([11.88589540, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]) # ca.DM([0., -0.028882, 0., 0., 0., -0.10716, 0.13604]) operating_point = ca.DM([ 0., 1.8571, 0., -1.7729, -1.9879, 1.7396, -1.1978, -1.7393, -1.2053, 1.7608, -1.7156, -2.8334, 0. ]) * 1 # [0.152070692682436] # Un-constraint lower_bounds_input = None lower_bounds_slew_rate = None lower_bounds_states = None upper_bounds_input = None upper_bounds_slew_rate = None upper_bounds_states = None # Actual constraints lower_bounds_input = ca.DM([0, 0, 0, 0]) # lower_bounds_slew_rate = ca.DM([-ca.inf, -ca.inf, -ca.inf, -ca.inf]) # lower_bounds_states = ca.DM([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # upper_bounds_input = ca.DM([2, 2, ca.inf, ca.inf]) # upper_bounds_slew_rate = ca.DM([1, 1, ca.inf, ca.inf]) upper_bounds_states = ca.DM([3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 2, 3]) * 5 # size1 and size2 represent the num of rows and columns in the Casadi lib, respectively num_states = Ap.size1() num_inputs = Bp.size2() # Initial conditions here are needed for the right dimensions in the MPC [x0, u0] = set_preismann_initial_conditions() [Q, R, S] = set_preismann_weight_matrices() initial_state_space_model = { "system_model": Ap, "b_matrix": Bp, "b_disturbance": Bp_d, "Q": Q, "R": R, "S": S, "x0": x0, "u0": u0, "operating_point": operating_point, "num_states": num_states, "num_inputs": num_inputs, "sim_type": simulation_type, "lower_bounds_input": lower_bounds_input, "lower_bounds_slew_rate": lower_bounds_slew_rate, "lower_bounds_states": lower_bounds_states, "upper_bounds_input": upper_bounds_input, "upper_bounds_slew_rate": upper_bounds_slew_rate, "upper_bounds_states": upper_bounds_states } print("Preismann model is constructed.") return initial_state_space_model
import casadi as ca from controller import mpc, mpco import numpy as np A = ca.DM([[1, 0], [0, 1.5]]) B = ca.DM([[0.1, 0], [0, 0.5]]) Hp = 40 Hu = 30 Q = ca.DM([[1, 0], [0, 3]]) Qb = mpc.blockdiag(Q, Hp) R = ca.DM([[1, 0], [0, 2]]) Rb = mpc.blockdiag(R, Hu) x0 = ca.DM([[10], [11]]) u_prev = ca.DM([-1, -3]) ref = ca.DM.ones(Hp * 2, 1) ref[1::A.size1()] = np.cumsum(ref[1::A.size1()]) / 20 + 2 mmpc = mpco.MpcObj(A, B, Hu, Hp, Q, R, x0, u_prev, ref) for i in range(0, 10): input("press key")
Q0 = np.full(n_level_nodes, 100.0) H0 = np.full(n_level_nodes, 0.0) for i in range(1, n_level_nodes): A = w / 2.0 * (H0[i - 1] - H_b[i - 1] + H0[i] - H_b[i]) P = w + H0[i - 1] - H_b[i - 1] + H0[i] - H_b[i] H0[i] = H0[i - 1] - dx * (P / A**3) * sabs(Q0[i]) * Q0[i] / C**2 # Symbols Q = ca.MX.sym("Q", n_level_nodes, T) H = ca.MX.sym("H", n_level_nodes, T) theta = ca.MX.sym("theta") # Left boundary condition Q_left = np.full(T + 1, 100) Q_left[T // 3:2 * T // 3] = 300.0 Q_left = ca.DM(Q_left).T # Hydraulic constraints Q_full = ca.vertcat(Q_left, ca.horzcat(Q0, Q)) H_full = ca.horzcat(H0, H) A_full = w * 0.5 * (H_full[1:, :] - H_b[1:] + H_full[:-1, :] - H_b[:-1]) A_full = ca.vertcat(w * (H_full[0, :] - H_b[0]), A_full, w * (H_full[-1, :] - H_b[-1])) P_full = w + (H_full[1:, :] - H_b[1:] + H_full[:-1, :] - H_b[:-1]) c = w * (H_full[:, 1:] - H_full[:, :-1]) / dt + (Q_full[1:, 1:] - Q_full[:-1, 1:]) / dx d = ((Q_full[1:-1, 1:] - Q_full[1:-1, :-1]) / dt + theta * (2 * Q_full[1:-1, :-1] / A_full[1:-1, :-1] * (sH(Q_full[1:-1, :-1]) * (Q_full[1:-1, :-1] - Q_full[0:-2, :-1]) / dx + (1 - sH(Q_full[1:-1, :-1])) *
def test_ctorDefaultUpperBound(self): '''Test Inequality ctor with default upper bound ''' i = ce.Inequality(cs.MX.sym('x', 3), lb=cs.DM([1, 2, 3])) nptest.assert_equal(i.lb, np.array(cs.DM([1, 2, 3]))) nptest.assert_equal(i.ub, np.array(cs.DM.inf(3)))
def hqpvschqp(params): result = {} n = params['n'] total_eq_constraints = params['total_eq_constraints'] total_ineq_constraints = params['total_ineq_constraints'] pl = params['pl'] #priority levels gamma = params['gamma'] rand_seed = params['rand_seed'] adaptive_method = params['adaptive_method'] #True n_eq_per_level = int(total_eq_constraints / pl) eq_last_level = n_eq_per_level + (total_eq_constraints - n_eq_per_level * pl) n_ineq_per_level = int(total_ineq_constraints / pl) ineq_last_level = n_ineq_per_level + (total_ineq_constraints - n_ineq_per_level * pl) # print(n_eq_per_level) # print(n_ineq_per_level) # print(eq_first_level) # print(ineq_first_level) count_hierarchy_failue = 0 count_same_constraints = 0 count_identical_solution = 0 count_geq_constraints = 0 hlp = lexopt_mosek() print("Using random seed " + str(rand_seed)) # n_eq_per_level = 6 # n_ineq_per_level = 6 np.random.seed( rand_seed) #for 45, 1, 8 HQP-l1 does not agree with the cHQP #15, 18, 11 for 8 priority levels x, x_dot = hlp.create_variable(n, [-1e6] * n, [1e6] * n) #high enough bounds to not matter A_eq_all = cs.DM(np.random.randn(params['eq_con_rank'], n)) A_extra = (A_eq_all.T @ cs.DM( np.random.randn(params['eq_con_rank'], total_eq_constraints - params['eq_con_rank']))).T A_eq_all = cs.vertcat(A_eq_all, A_extra) A_eq_all = A_eq_all.full() np.random.shuffle(A_eq_all) A_eq_all += np.random.randn(total_eq_constraints, n) * 1e-5 b_eq_all = np.random.randn(total_eq_constraints) #normalizing the each row vector row_vec_norm = [] for i in range(A_eq_all.shape[0]): row_vec_norm.append(cs.norm_1(A_eq_all[i, :])) # print(row_vec_norm) b_eq_all[i] /= row_vec_norm[i] for j in range(A_eq_all.shape[1]): A_eq_all[i, j] = A_eq_all[i, j].T / row_vec_norm[i] A_ineq_all = cs.DM(np.random.randn(params['ineq_con_rank'], n)) A_ineq_extra = (A_ineq_all.T @ cs.DM( np.random.randn(params['ineq_con_rank'], total_ineq_constraints - params['ineq_con_rank']))).T A_ineq_all = cs.vertcat(A_ineq_all, A_ineq_extra) A_ineq_all = A_ineq_all.full() np.random.shuffle(A_ineq_all) b_ineq_all = np.random.randn(total_ineq_constraints) b_ineq_all_lower = b_ineq_all - 1 # print(b_ineq_all) # print(b_ineq_all_lower) #normalizing the each row vector row_ineqvec_norm = [] for i in range(A_ineq_all.shape[0]): row_ineqvec_norm.append(cs.norm_1(A_ineq_all[i, :])) b_ineq_all[i] /= row_ineqvec_norm[i] b_ineq_all_lower[i] /= row_ineqvec_norm[i] for j in range(A_ineq_all.shape[1]): A_ineq_all[i, j] = A_ineq_all[i, j] / row_ineqvec_norm[i] A_eq = {} b_eq = {} A_eq_opti = {} b_eq_opti = {} A_ineq_opti = {} b_upper_opti = {} A_ineq = {} b_upper = {} b_lower = {} params = x params_init = [0] * n #create these tasks counter_eq = 0 counter_ineq = 0 # print(row_ineqvec_norm) for i in range(pl): if i != pl - 1: A_eq[i] = A_eq_all[counter_eq:counter_eq + n_eq_per_level, :] b_eq[i] = b_eq_all[counter_eq:counter_eq + n_eq_per_level] counter_eq += n_eq_per_level hlp.create_constraint( cs.mtimes(A_eq[i], x_dot) - b_eq[i], 'eq', i, {'b': 0}) A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq + n_ineq_per_level, :] b_upper[i] = b_ineq_all[counter_ineq:counter_ineq + n_ineq_per_level] b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq + n_ineq_per_level] counter_ineq += n_ineq_per_level hlp.create_constraint(A_ineq[i] @ x_dot, 'lub', i, { 'lb': b_lower[i], 'ub': b_upper[i] }) else: A_eq[i] = A_eq_all[counter_ineq:counter_ineq + eq_last_level, :] b_eq[i] = b_eq_all[counter_eq:counter_eq + eq_last_level] counter_eq += eq_last_level hlp.create_constraint( cs.mtimes(A_eq[i], x_dot) - b_eq[i], 'eq', i, {'b': 0}) A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq + ineq_last_level, :] b_upper[i] = b_ineq_all[counter_ineq:counter_ineq + ineq_last_level] b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq + ineq_last_level] counter_ineq += ineq_last_level hlp.create_constraint(A_ineq[i] @ x_dot, 'lub', i, { 'lb': b_lower[i], 'ub': b_upper[i] }) hlp.configure_constraints() hlp.compute_matrices([0] * n, [0] * n) hlp.configure_weighted_problem() hlp.configure_sequential_problem() try: hlp.solve_sequential_problem() result['slp_time'] = hlp.time_taken result['slp_status'] = True except: result['slp_status'] = False if not adaptive_method: hlp.time_taken = 0 sol = hlp.solve_weighted_method([gamma] * (pl - 2)) else: # tic = time.time() sol, hierarchical_failure = hqp.solve_adaptive_hqp3(params_init, [0] * n, gamma_init=gamma) # toc = time.time() - tic tp = 0 #true positive fp = 0 tn = 0 fn = 0 hlp_status = True result['hlp_status'] = hlp_status if not hlp_status: print("hlp-l1 failed") else: result['hlp_time'] = hlp.time_taken if adaptive_method: result['heuristic_prediction'] = len(hierarchical_failure) == 0 # if not adaptive_method: # return False, False, False, True, False, False # else: # return False, False, False, True, False, False, False #further analysis and comparison only if both hqp and chqp returned without failing lex_opt = True if hlp_status and result['slp_status']: for i in range(1, pl - 1): weighted_obj = sum(hlp.Mw_dict[i]['eq_slack'].level()) + sum( hlp.Mw_dict[i]['lub_slack'].level()) sequential_obj = hlp.optimal_slacks[i] if weighted_obj > sequential_obj + 1e-5: lex_opt = False if verbose: print("Lex norm unsatisfied!!!!!!!!!!!!!!!!! by " + str(weighted_obj - sequential_obj)) result['lex_opt'] = lex_opt return result
def solve(self, ): self.isSolved = True # prepare QP QSet, ASet, BSet, AeqSet, BeqSet = self.getQPset() if self.algorithm == 'end-derivative' and ASet is not None: mapMat = self.coeff2endDerivatives(AeqSet[0]) QSet, HSet, ASet, BSet = self.mapQP(QSet, ASet, BSet, AeqSet, BeqSet) elif self.algorithm == 'poly-coeff' or ASet is None: x_sym = ca.SX.sym('x', QSet[0].shape[0]) opts_setting = { 'ipopt.max_iter': 100, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.acceptable_tol': 1e-8, 'ipopt.acceptable_obj_change_tol': 1e-6 } else: print('unsupported algorithm') for dd in range(self.dim): print('soving {}th dimension ...'.format(dd)) if self.algorithm == 'poly-coeff' or ASet is None: obj = ca.mtimes([x_sym.T, QSet[dd], x_sym]) if ASet is not None: a_set = np.concatenate((ASet[dd], AeqSet[dd])) else: a_set = AeqSet[dd].copy() Ax_sym = ca.mtimes([a_set, x_sym]) if BSet is not None: b_set_u = np.concatenate( (BSet[dd], BeqSet[dd])) # Ax <= b_set_u b_set_l = np.concatenate( (-np.inf * np.ones(BSet[dd].shape), BeqSet[dd])) # Ax >= b_set_l else: b_set_u = BeqSet[dd] b_set_l = BeqSet[dd] nlp_prob = {'f': obj, 'x': x_sym, 'g': Ax_sym} solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting) try: result = solver( lbg=b_set_l, ubg=b_set_u, ) Phat_ = result['x'] flag_ = True except: Phat_ = None flag_ = False else: ## qpoases version qp = {} qp['h'] = ca.DM(QSet[dd]).sparsity() qp['a'] = ca.DM(ASet[dd]).sparsity() options_ = { 'print_time': False, "jit": True, 'verbose': False, 'print_problem': False, } solver_ = ca.conic('solver_', 'qpoases', qp, options_) try: result = solver_(h=QSet[dd], g=HSet[dd], a=ASet[dd], uba=BSet[dd]) dP_ = result['x'] dF_ = BeqSet[dd] Phat_ = solve(mapMat, np.concatenate((dF_, dP_))) flag_ = True except: dP_ = None flag_ = False # ## ipopt version # x_sym = ca.SX.sym('x', QSet[0].shape[0]) # opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6} # print(HSet[dd].shape) # obj = 0.5* ca.mtimes([x_sym.T, QSet[dd], x_sym]) + ca.mtimes([HSet[dd].reshape(1, -1), x_sym]) # Ax_sym = ca.mtimes([ASet[dd], x_sym]) # nlp_prob = {'f': obj, 'x': x_sym, 'g':Ax_sym} # solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting) # try: # result = solver(ubg=BSet[dd],) # dP_ = result['x'] # # print(dP_) # dF_ = BeqSet[dd] # Phat_ = solve(mapMat, np.concatenate((dF_, dP_))) # flag_ = True # except: # dP_ = None # flag_ = False if flag_: print("success !") P_ = np.dot(self.scaleMatBigInv(), Phat_) self.polyCoeffSet[dd] = P_.reshape(-1, self.N + 1).T print("done")
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 print(robot.fk(q0_1)[6]) print(robot2.fk(q0_2)[6]) #specifying the cartesian trajectory of the two robots as a function #of the progress variable rob1_start_pose = cs.DM([0.3, -0.24, 0.4]) rob1_end_pose = cs.DM([0.6, 0.2, 0.25]) traj1 = rob1_start_pose + cs.fmin(s_1, 1) * (rob1_end_pose - rob1_start_pose) rob2_start_pose = cs.DM([0.3, 0.54, 0.4]) rob2_end_pose = cs.DM([0.6, 0.1, 0.25]) traj2 = rob2_start_pose + cs.fmin(s_2, 1) * (rob2_end_pose - rob2_start_pose) #Highest priority: Hard constraints on joint velocity and joint position limits max_joint_vel = np.array([max_joint_vel] * 7) max_joint_acc = np.array([max_joint_acc] * 7) hqp.create_constraint(q_dot1, 'lub', priority=0,
def analyze_data(data): plt.figure(figsize=(20, 20)) plt.subplot(331) plt.title('fuel') plt.plot(data['t'], data['x'][:, 13]) plt.xlabel('t, sec') plt.ylabel('mass, kg') plt.grid() plt.subplot(332) #plt.title('velocity') plt.plot(data['t'], data['x'][:, 7], label='x') plt.plot(data['t'], data['x'][:, 8], label='y') plt.plot(data['t'], data['x'][:, 9], label='z') plt.xlabel('t, sec') plt.ylabel('body velocity, m/s') plt.grid() plt.legend() plt.subplot(333) euler = np.array( [np.array(ca.DM(so3.Euler.from_mrp(x))).reshape(-1) for x in data['x'][:, 3:7]]) plt.plot(data['t'], np.rad2deg(euler[:, 0]), label='roll') plt.plot(data['t'], np.rad2deg(euler[:, 1]), label='pitch') plt.plot(data['t'], np.rad2deg(euler[:, 2]), label='yaw') plt.legend() plt.grid() plt.xlabel('t, sec') plt.ylabel('euler angles, deg') #plt.title('euler') plt.subplot(334) #plt.title('angular velocity') plt.plot(data['t'], data['x'][:, 0], label='x') plt.plot(data['t'], data['x'][:, 1], label='y') plt.plot(data['t'], data['x'][:, 2], label='z') plt.xlabel('t, sec') plt.ylabel('angular velocity, rad/s') plt.grid() plt.legend() plt.subplot(335) #plt.title('trajectory [side]') plt.plot(data['x'][:, 10], -data['x'][:, 12]) plt.xlabel('North, m') plt.ylabel('Altitude, m') plt.axis('equal') plt.grid() plt.subplot(336) #plt.title('trajectory [top]') plt.plot(data['x'][:, 11], data['x'][:, 10]) plt.xlabel('East, m') plt.ylabel('North, m') plt.axis('equal') plt.grid() plt.subplot(337) #plt.title('control input') plt.plot(data['t'], data['u'][:, 0], label='mdot') plt.plot(data['t'], data['u'][:, 1], label='aileron') plt.plot(data['t'], data['u'][:, 2], label='elevator') plt.plot(data['t'], data['u'][:, 3], label='rudder') plt.xlabel('t, sec') plt.ylabel('control') plt.legend() plt.grid()
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs): engine_parameters = kwargs['engine_parameters'] atmosphere_parameters = kwargs['atmosphere_parameters'] init_mass = kwargs['init_mass'] has_steering_unit_vector_constraint = kwargs.get( 'has_steering_unit_vector_constraint', True) if init_mass is None: assert atmosphere_parameters is None # Must have mass for drag-acceleration assert engine_parameters is None # Must have mass for thrust-acceleration if phase_name == 'target': init_position = [ p.target_position_x, p.target_position_y, p.target_position_z ] init_velocity = [ p.target_velocity_x, p.target_velocity_y, p.target_velocity_z ] else: init_position = [p.launch_site_x, p.launch_site_y, p.launch_site_z] init_velocity = [0.0, 0.0, 0.0] duration = mocp.create_phase(phase_name, init=0.001, n_intervals=3) if init_mass is not None: # Total vessel mass mass = mocp.add_trajectory(phase_name, 'mass', init=init_mass) # ECEF position vector rx = mocp.add_trajectory(phase_name, 'rx', init=init_position[0]) ry = mocp.add_trajectory(phase_name, 'ry', init=init_position[1]) rz = mocp.add_trajectory(phase_name, 'rz', init=init_position[2]) r_vec = casadi.vertcat(rx, ry, rz) mocp.add_path_constraint(sumsqr(r_vec) > 0.99) # ECEF velocity vector vx = mocp.add_trajectory(phase_name, 'vx', init=init_velocity[0]) vy = mocp.add_trajectory(phase_name, 'vy', init=init_velocity[1]) vz = mocp.add_trajectory(phase_name, 'vz', init=init_velocity[2]) v_vec = casadi.vertcat(vx, vy, vz) if engine_parameters is not None: launch_site_param = DM( [p.launch_site_x, p.launch_site_y, p.launch_site_z]) init_up_direction = normalize(launch_site_param) # ECEF steering unit vector ux = mocp.add_trajectory(phase_name, 'ux', init=init_up_direction[0]) uy = mocp.add_trajectory(phase_name, 'uy', init=init_up_direction[1]) uz = mocp.add_trajectory(phase_name, 'uz', init=init_up_direction[2]) u_vec = casadi.vertcat(ux, uy, uz) if has_steering_unit_vector_constraint: # u is a unit vector mocp.add_constraint(sumsqr(mocp.start(u_vec)) == 1.0) omega_x = mocp.add_trajectory(phase_name, 'omega_x', init=0.0) omega_y = mocp.add_trajectory(phase_name, 'omega_y', init=0.0) omega_z = mocp.add_trajectory(phase_name, 'omega_z', init=0.0) omega_vec = casadi.vertcat(omega_x, omega_y, omega_z) alpha_x = mocp.add_trajectory(phase_name, 'alpha_x', init=0.0) alpha_y = mocp.add_trajectory(phase_name, 'alpha_y', init=0.0) alpha_z = mocp.add_trajectory(phase_name, 'alpha_z', init=0.0) alpha_vec = casadi.vertcat(alpha_x, alpha_y, alpha_z) du_dt = casadi.cross(omega_vec, u_vec) + u_vec * 10 * (1.0 - sumsqr(u_vec)) mocp.set_derivative(ux, du_dt[0]) mocp.set_derivative(uy, du_dt[1]) mocp.set_derivative(uz, du_dt[2]) mocp.set_derivative(omega_x, alpha_x) mocp.set_derivative(omega_y, alpha_y) mocp.set_derivative(omega_z, alpha_z) mocp.add_path_constraint(dot(u_vec, omega_vec) == 1e-20) mocp.add_mean_objective(1e-6 * sumsqr(alpha_vec)) mocp.add_mean_objective(1e-6 * sumsqr(omega_vec)) mocp.add_path_output('u_mag_err', sumsqr(u_vec) - 1) throttle = mocp.add_trajectory(phase_name, 'throttle', init=1.0) throttle_rate = mocp.add_trajectory(phase_name, 'throttle_rate', init=0.0) r_squared = sumsqr(r_vec) + 1e-8 v_squared = sumsqr(v_vec) + 1e-8 airspeed = v_squared**0.5 orbital_radius = r_squared**0.5 up_direction = r_vec / orbital_radius altitude = orbital_radius - 1.0 planet_angular_velocity = casadi.DM( [0.0, -p.body_angular_rate, 0.0] ) # KSP coordinates: planet angular velocity points in negative y direction a_gravity = -(r_squared**( -1.5)) * r_vec # Gravitational acceleration (mu=1 is omitted) a_coriolis = -2 * cross(planet_angular_velocity, v_vec) # Coriolis acceleration a_centrifugal = -cross(planet_angular_velocity, cross(planet_angular_velocity, r_vec)) # Centrifugal acceleration a_vec = a_gravity + a_coriolis + a_centrifugal # Total acceleration on vehicle # Atmosphere if atmosphere_parameters is not None: atmosphere_fraction = casadi.exp(-altitude / atmosphere_parameters.scale_height) air_density = atmosphere_parameters.air_density_MSL * atmosphere_fraction dynamic_pressure = 0.5 * air_density * v_squared mocp.add_path_output('dynamic_pressure', dynamic_pressure) # Drag force drag_force_vector = (-0.5 * atmosphere_parameters.drag_area * atmosphere_parameters.air_density_MSL ) * atmosphere_fraction * airspeed * v_vec a_vec += (drag_force_vector / mass) # Engine thrust if engine_parameters is not None: if atmosphere_parameters is None: exhaust_velocity = engine_parameters.exhaust_velocity_vacuum else: exhaust_velocity = engine_parameters.exhaust_velocity_vacuum + \ atmosphere_fraction * (engine_parameters.exhaust_velocity_sea_level - engine_parameters.exhaust_velocity_vacuum) engine_mass_flow = engine_parameters.max_mass_flow * throttle thrust = exhaust_velocity * engine_mass_flow mocp.add_path_output('thrust', thrust) thrust_acceleration = thrust / mass mocp.add_path_output('thrust_acceleration', thrust_acceleration) thrust_force_vector = thrust * u_vec a_vec += (thrust_force_vector / mass) ## Differential equations if engine_parameters is not None: mocp.set_derivative(mass, -engine_mass_flow) mocp.set_derivative(throttle, throttle_rate) elif init_mass is not None: mocp.set_derivative(mass, 0.0) # Atmospheric coast: mass is constant for i in range(3): mocp.set_derivative(r_vec[i], v_vec[i]) mocp.set_derivative(v_vec[i], a_vec[i]) ## Constraints # Duration is positive and bounded mocp.add_constraint(duration > 0.004) if phase_name == 'target': mocp.add_constraint(duration < 20.0) else: mocp.add_constraint(duration < 10.0) # Mass is positive if init_mass is not None: mocp.add_path_constraint(mass > 1e-6) if engine_parameters is not None: # Do not point down mocp.add_path_constraint(dot(up_direction, u_vec) > -0.2) # Throttle limits mocp.add_path_constraint(throttle < 0.90) mocp.add_path_constraint(throttle > 0.05) # Throttle rate reduction mocp.add_mean_objective(1e-5 * throttle_rate**2) # Q alpha constraint if (atmosphere_parameters is not None) and (engine_parameters is not None): lateral_airspeed = casadi.sqrt( casadi.sumsqr(v_vec - (dot(v_vec, u_vec) * u_vec)) + 1e-8) lateral_dynamic_pressure = 0.5 * air_density * lateral_airspeed * airspeed # Same as Q * sin(alpha), but without trigonometry mocp.add_path_output('lateral_dynamic_pressure', lateral_dynamic_pressure) mocp.add_mean_objective( (lateral_dynamic_pressure / atmosphere_parameters.lateral_dynamic_pressure_limit)**8) ## Outputs mocp.add_path_output('altitude', altitude) mocp.add_path_output('airspeed', airspeed) mocp.add_path_output('vertical_speed', dot(up_direction, v_vec)) mocp.add_path_output( 'horizontal_speed_ECEF', casadi.norm_2(v_vec - (dot(up_direction, v_vec) * (up_direction)))) if engine_parameters is not None: mocp.add_path_output( 'AoA', casadi.acos(dot(v_vec, u_vec) / airspeed) * 180.0 / pi) mocp.add_path_output( 'pitch', 90.0 - casadi.acos(dot(up_direction, u_vec)) * 180.0 / pi) variables = locals().copy() RocketStagePhase = collections.namedtuple('RocketStagePhase', sorted(list(variables.keys()))) return RocketStagePhase(**variables)
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 _convertToDmIfNotDmStruct(val): return val if isinstance(val, DMStruct) else cs.DM(val)
def run(self, x0=[0, 0, 0, 0]): """ Run simulator with specified system dynamics and control function. """ print("Running simulation....") sim_loop_length = int( self.total_sim_time / self.dt) + 1 # account for 0th t = np.array([0]) y_vec = np.array([x0]).T u_vec = np.array([0]) # Start figure if ANIM: fig = plt.figure() ax1 = fig.add_subplot(2, 1, 1) ax2 = fig.add_subplot(2, 1, 2) for i in range(sim_loop_length): # Translate data to ca.DM x = ca.DM(4, 1).full() x = y_vec[:, -1] try: # Get control input and obtain next state u = self.controller(x) x_next = self.dynamics(x, u) except RuntimeError: print( "Uh oh, your simulator crashed due to unstable dynamics.\n \ Retry with new controller parameters.") exit() # Store data t = np.append(t, t[-1] + self.dt) y_vec = np.append(y_vec, np.array(x_next), axis=1) u_vec = np.append(u_vec, np.array(u)) # Get plot window values: if self.plt_window != float("inf"): l_wnd = 0 if int(i + 1 - self.plt_window / self.dt) < 1 else int( i + 1 - self.plt_window / self.dt) else: l_wnd = 0 if ANIM: ax1.clear() plt.subplot(211) plt.plot(t[l_wnd:-1], y_vec[0, l_wnd:-1], 'r--', t[l_wnd:-1], y_vec[1, l_wnd:-1], 'b--') plt.legend(["x1", "x2"]) plt.ylabel("X1 [m] / X2 [m/s]") plt.ylim([-0.5, 11]) plt.title("Pendulum on Cart - Ref: " + str(self.model.x_d) + " [m]") if ANIM: ax2.clear() rad_to_deg_x3 = 180 / np.pi * y_vec[2, l_wnd:-1] rad_to_deg_x4 = 180 / np.pi * y_vec[3, l_wnd:-1] plt.subplot(212) plt.plot(t[l_wnd:-1], rad_to_deg_x3, 'go', t[l_wnd:-1], rad_to_deg_x4, 'k--') plt.xlabel("Time [s]") plt.ylabel("X3 [deg] / X4 [deg/s]") plt.legend(["x3", "x4"]) if ANIM: plt.pause(0.01) plt.subplot(211) plt.grid(True) plt.subplot(212) plt.grid(True) if ANIM: plt.show() else: plt.savefig('sim_nosampling.pdf') return t, y_vec, u_vec
HPRC = False Model = 'quad' #'car_w_trailers' #'car_model' #'youBot_model' OL_time = 0 #Open loop time taken. Xg = np.array([2, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0]) if FILE_WRITE: filename = "/home/naveed/Dropbox/Research/Data/WAFR20/Quad/MPC_SH_1_wo_obs_modified1.csv" file = open(filename, "a") file.write('epsilon' + ',' + 'Average Cost' + ',' + 'Cost variance' + ',' + 'Average Time' + '\n') if Model == 'quad': robot = quad(dt=0.1) #open-loop optimisation gain R = c.DM([[5, 0, 0, 0], [0, 10, 0, 0], [0, 0, 10, 0], [0, 0, 0, 10]]) Q = c.DM.eye(robot.nx) Q[0, 0] = 10 Q[1, 1] = 10 Q[2, 2] = 10 Qf = 1000 * c.DM.eye(robot.nx) def blockPrint(): sys.stdout = open(os.devnull, 'w') # Restore def enablePrint(): sys.stdout = sys.__stdout__
def getModel(self, q, dq, u): q = ca.reshape(q, 5, 1) dq = ca.reshape(dq, 5, 1) p0 = ca.MX(self.p0[0, 0]) p1 = self.l[0, 0] * ca.sin(q[0, 0]) + p0 p2 = self.l[1, 0] * ca.sin(q[1, 0]) + p1 p3 = self.l[2, 0] * ca.sin(q[2, 0]) + p2 p4 = self.l[3, 0] * ca.sin(q[3, 0]) + p2 p5 = self.l[4, 0] * ca.sin(q[4, 0]) + p4 c1 = self.l[0, 0] * ca.sin(q[0, 0]) / 2 + p0 c2 = self.l[1, 0] * ca.sin(q[1, 0]) / 2 + p1 c3 = self.l[2, 0] * ca.sin(q[2, 0]) / 2 + p2 c4 = self.l[3, 0] * ca.sin(q[3, 0]) / 2 + p2 c5 = self.l[4, 0] * ca.sin(q[4, 0]) / 2 + p4 dc1 = self.l[0, 0] * ca.cos(q[0, 0]) * dq[0, 0] / 2 dc2 = self.l[1, 0] * ca.cos( q[1, 0]) * dq[1, 0] / 2 + self.l[0, 0] * ca.cos(q[0, 0]) * dq[0, 0] dc3 = self.l[2, 0] * ca.cos( q[2, 0]) * dq[2, 0] / 2 + self.l[1, 0] * ca.cos( q[1, 0]) * dq[1, 0] + self.l[0, 0] * ca.cos(q[0, 0]) * dq[0, 0] dc4 = self.l[3, 0] * ca.cos( q[3, 0]) * dq[3, 0] / 2 + self.l[1, 0] * ca.cos( q[1, 0]) * dq[1, 0] + self.l[0, 0] * ca.cos(q[0, 0]) * dq[0, 0] dc5 = (self.l[4, 0] * ca.cos(q[4, 0]) * dq[4, 0] / 2 + self.l[3, 0] * ca.cos(q[3, 0]) * dq[3, 0] + self.l[1, 0] * ca.cos(q[1, 0]) * dq[1, 0] + self.l[0, 0] * ca.cos(q[0, 0]) * dq[0, 0]) dC = ca.reshape(ca.vertcat(dc1, dc2, dc3, dc4, dc5), 5, 1) ddc1 = (self.l[0, 0] * ca.sin(q[0, 0]) * (-dq[0, 0]**2) / 2) ddc2 = (self.l[1, 0] * ca.sin(q[1, 0]) * (-dq[1, 0]**2) / 2) + (self.l[0, 0] * ca.sin(q[0, 0]) * (-dq[0, 0]**2)) ddc3 = ((self.l[2, 0] * ca.sin(q[2, 0]) * (-dq[2, 0]**2) / 2) + (self.l[1, 0] * ca.sin(q[1, 0]) * (-dq[1, 0]**2)) + (self.l[0, 0] * ca.sin(q[0, 0]) * (-dq[0, 0]**2))) ddc4 = ((self.l[3, 0] * ca.sin(q[3, 0]) * (-dq[3, 0]**2) / 2) + (self.l[1, 0] * ca.sin(q[1, 0]) * (-dq[1, 0]**2)) + (self.l[0, 0] * ca.sin(q[0, 0]) * (-dq[0, 0]**2))) ddc5 = ((self.l[4, 0] * ca.sin(q[0, 0]) * (-dq[4, 0]**2) / 2) + (self.l[3, 0] * ca.sin(q[3, 0]) * (-dq[3, 0]**2)) + (self.l[1, 0] * ca.sin(q[1, 0]) * (-dq[1, 0]**2)) + (self.l[0, 0] * ca.sin(q[0, 0]) * (-dq[0, 0]**2))) P = ca.MX(5, 5) P[0, :] = p0 P[1, 1:4:1] = p1 P[2, 2:4:1] = p2 P[3, 3:4:1] = p3 P[4, 4:4:1] = p4 # print(P.shape) G = ca.MX(5, 5) G[0, 0] = c1 G[0:1:1, 1] = c2 G[0:2:1, 2] = c3 G[0:3:1, 3] = c4 G[:, 4] = c5 # print(G.shape) ddC = ca.reshape(ca.vertcat(ddc1, ddc2, ddc3, ddc4, ddc5), 5, 1) # print(ddC.shape) U = ca.reshape(ca.vertcat(0., self.u), 5, 1) # print(U.shape) M = ca.MX(self.m.reshape(5, 1)) # print(M) I = ca.DM([[self.i[0], self.i[1], self.i[2], self.i[3], self.i[4]], [0., self.i[1], self.i[2], self.i[3], self.i[4]], [0., 0., self.i[2], self.i[3], self.i[4]], [0., 0., 0., self.i[3], self.i[4]], [0., 0., 0., 0., self.i[4]]]) # print(I) iI = ca.inv(I) # print(iI) Q = U + ca.mtimes((G - P), M * (self.gravity - ddC)) ddq = ca.mtimes(ca.MX(iI), Q) # print(ddq) return [p0, p1, p2, p3, p4, p5], [c1, c2, c3, c4, c5], dC, ddq
def pv_schedule(Tf, Nperday, x0, B, pv_lambda, p_in, patient_volume, dict_opts): N = int(Tf * Nperday) dt = Tf/N # set maximal treatment volume, default is 500 ml if 'max_treatment_volume' in dict_opts.keys(): max_treatment_volume = dict_opts['max_treatment_volume'] else: max_treatment_volume = 500 # maximal fractional blood removal max_fraction = max_treatment_volume / patient_volume # Allowed treatment times if 'allowed_days' in dict_opts.keys() or 'allowed_hours' in dict_opts.keys() or 'forbidden_days' in dict_opts.keys(): allowed_arr = allowed_generator(Nperday, dict_opts, N, dt) else: allowed_arr = [1]*N # initial value for control, default is zero control if 'u_start' in dict_opts.keys(): u_start = dict_opts['u_start'] else: u_start = [ca.DM(0.)] * len(allowed_arr) # obtaining objective if 'objective' in dict_opts.keys() and dict_opts['objective'] == 'integer_end_point': objective = 'integer_end_point' elif 'objective' in dict_opts.keys() and dict_opts['objective'] == 'heuristic': objective = 'heuristic' else: # default objective = 'relaxed_int_u' if objective == 'integer_end_point': # model formulation and integrator integrator_function, integrator_function_2 = model_integrator(N, dt, Tf, Nperday, B, max_fraction, pv_lambda, two_stage=True) else: # model formulation and integrator integrator_function = model_integrator(N, dt, Tf, Nperday, B, max_fraction, pv_lambda) integrator_function_2 = None if objective == 'heuristic': x1_opt, x2_opt, x3_opt, q_opt, u, tgrid, error_flag = pv_heuristic_alg(Tf, N, np.array(x0), integrator_function, p_in, max_fraction, B, allowed_arr) return x1_opt, x2_opt, x3_opt, q_opt, u, tgrid, {}, allowed_arr, error_flag else: # NLP based problem # maximal number of donations, default is none if 'u_max' in dict_opts.keys(): u_max = dict_opts['u_max'] else: u_max = None Q, w, w0, g, lbw, ubw, lbg, ubg, discrete = nlp_builder(N, Nperday, 0.05, B, x0, max_fraction, allowed_arr, integrator_function, integrator_function_2, p_in, u_start, u_max) # build NLP # Solve problem using NLP solver if 'obj_factor' in dict_opts.keys(): obj_factor = dict_opts['obj_factor'] print('Using objective factor') else: obj_factor = 10 # problem formulation nlp_prob = {'f': obj_factor*Q, 'x': w, 'g': g} # define and run NLP solver if objective == 'integer_end_point': bonmin_options = {'variable_selection': 'most-fractional', 'tree_search_strategy':'dive'} # options used in paper # bonmin_options = {'variable_selection': 'nlp-strong-branching', 'tree_search_strategy':'dive'} # bonmin_options = {} nlp_solver = ca.nlpsol('nlp_solver', 'bonmin', nlp_prob, {"discrete": discrete, "bonmin": bonmin_options}); else: # objective == 'relaxed_int_u' ipopt_opts = {} nlp_solver = ca.nlpsol('nlp_solver', 'ipopt', nlp_prob, {"ipopt": ipopt_opts}); sol = nlp_solver(x0=ca.vertcat(*w0), lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) # Integrate solution object to obtain trajectories x1_opt, x2_opt, x3_opt, q_opt, u_opt, tgrid = integrate_nlp_sol(sol, x0, allowed_arr, N, dt, Nperday, Tf, integrator_function, integrator_function_2, max_fraction, p_in) return x1_opt, x2_opt, x3_opt, q_opt, u_opt, tgrid, sol, allowed_arr, 0