def __init__(self, env_name='Pushing2D-v1', num_nets=1, mpc_params=None): self.env = gym.make(env_name) self.task_horizon = TASK_HORIZON self.agent = Agent(self.env) mpc_params['use_gt_dynamics'] = False self.model = PENN(num_nets, STATE_DIM, len(self.env.action_space.sample()), LR) self.cem_policy = MPC(self.env, PLAN_HORIZON, self.model, POPSIZE, NUM_ELITES, MAX_ITERS, **mpc_params, use_random_optimizer=False) self.random_policy = MPC(self.env, PLAN_HORIZON, self.model, POPSIZE, NUM_ELITES, MAX_ITERS, **mpc_params, use_random_optimizer=True) self.random_policy_no_mpc = RandomPolicy( len(self.env.action_space.sample()))
def __init__(self, env_name='Pushing2D-v1', mpc_params=None): self.env = gym.make(env_name) self.env.seed(1024) self.task_horizon = TASK_HORIZON self.agent = Agent(self.env, self.env.control_noise != 0.) # Does not need model self.warmup = False mpc_params['use_gt_dynamics'] = True self.cem_policy = MPC(self.env, PLAN_HORIZON, None, POPSIZE, NUM_ELITES, MAX_ITERS, **mpc_params, use_random_optimizer=False) self.random_policy = MPC(self.env, PLAN_HORIZON, None, POPSIZE, NUM_ELITES, MAX_ITERS, **mpc_params, use_random_optimizer=True)
def __init__(self): self.env = gym.make('Pushing2D-v1') self.task_hor = TASK_HORIZON self.agent = Agent(self.env) self.model = PENN(NUM_NETS, STATE_DIM, ACTION_DIM, LR) self.policy = MPC(self.env, NUM_PARTICLES, PLAN_HOR, self.model, POPSIZE, NUM_ELITES, MAX_ITERS)
def __init__(self, window, dt=0.01): self.use_mpc = True self.clock = sf.Clock() self.window = window self.dt = dt self.time = 0 self.mpc_horizon = 10 self.counter = -1 spring_damper = SpringDamper(window, pos_x=0.9, pos_y=0.5) spring_damper.set_properties(k=10., m=1., c=0.1, x0=0.5) spring_damper.integration_type = spring_damper.RUNGE_KUTTA pendulum = Pendulum(window) pendulum.set_properties(m=1.0, g=-9.8, L=0.25, k=0.1) pendulum.set_position(0.5, 0.5) pendulum.set_rotation(np.pi * 0.95) pendulum.integration_type = pendulum.RUNGE_KUTTA pendulum.nonlinear_mode = True cart = CartPole(window) cart.set_properties(m=0.2, M=0.5, I=0.006, g=-9.8, l=0.25, b=0.5) cart.set_position(0.5, 0.5) cart.set_rotation(np.pi * 0.99) cart.nonlinear_mode = True cart.integration_type = cart.RUNGE_KUTTA self.mpc = MPC(dt, self.mpc_horizon) # This works for springdamper and pendulum R = np.matrix(np.eye(2) * 1e-5) Q = np.matrix(np.matrix([[100, 0], [0.0, 1.0]])) T = np.matrix(np.eye(2) * 1e-5) self.mpc.set_cost_weights(Q, R, T) self.mpc.Y_prime = np.matrix(np.zeros(shape=(2, self.mpc_horizon))) self.mpc.Y_prime[0, :] = 0.7 self.mpc.C = np.matrix(np.eye(2)) # cartpole # R = np.matrix(np.eye(4)*0.001) # Q = np.matrix([[100, 0, 0, 0], # [0, 0, 0, 0], # [0, 0, 100, 0], # [0, 0, 0, 0]]) # # self.mpc.set_cost_weights(Q, R) # self.mpc.set_constraints(cart.cons) # # self.mpc.Y_prime = np.matrix(np.zeros(shape=(4, self.mpc_horizon))) # self.mpc.Y_prime[0,:] = np.pi # self.mpc.Y_prime[2,:] = 0.5 # self.C = np.matrix([[1,0, 1, 0]]) # self.bodies = [spring_damper, pendulum] self.bodies = [spring_damper] self.mpc.set_body(self.bodies[0])
def __init__(self, env_name='Pushing2D-v1', mpc_params=None): self.env = gym.make(env_name) self.task_horizon = TASK_HORIZON self.agent = Agent(self.env) # Does not need model self.warmup = False mpc_params['use_gt_dynamics'] = True if (mpc_params['use_mpc']): self.cem_policy = MPC(self.env, PLAN_HORIZON, None, POPSIZE, NUM_ELITES, MAX_ITERS, INITIAL_MU, INITIAL_SIGMA, **mpc_params, use_random_optimizer=False) self.random_policy = MPC(self.env, PLAN_HORIZON, None, POPSIZE, NUM_ELITES, MAX_ITERS, INITIAL_MU, INITIAL_SIGMA, **mpc_params, use_random_optimizer=True) else: self.cem_policy = CEMPolicy( self.env, len(self.env.action_space.low), INITIAL_MU, INITIAL_SIGMA, PLAN_HORIZON, POPSIZE, NUM_ELITES, MAX_ITERS, self.env.action_space.high, self.env.action_space.low, mpc_params['use_gt_dynamics']) self.random_policy = RandomPolicy(self.env, len(self.env.action_space.low), INITIAL_MU, INITIAL_SIGMA, PLAN_HORIZON, POPSIZE, MAX_ITERS, self.env.action_space.high, self.env.action_space.low, mpc_params['use_gt_dynamics'])
def __init__(self, A, B, C, Q, R, RD, umin, umax, N): self.A = A self.B = B self.C = C self.num_outputs = C.shape[0] self.num_inputs = B.shape[1] if self.num_inputs == self.num_outputs == 1: self.mtype = 0 else: self.mtype = 1 self.mpc = MPC(A, B, C, Q, R, RD, umin, umax, N)
def __init__(self, env_name='Pushing2D-v1', num_nets=1, mpc_params=None): self.env = gym.make(env_name) # self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') self.device = torch.device('cpu') self.task_horizon = TASK_HORIZON # Tensorboard logging. self.timestamp = datetime.now().strftime('%Y-%m-%d_%H-%M-%S') self.environment_name = "pusher" self.logdir = 'logs/%s/%s' % (self.environment_name, self.timestamp) self.summary_writer = SummaryWriter(self.logdir) self.agent = Agent(self.env) mpc_params['use_gt_dynamics'] = False self.model = PENN(num_nets, STATE_DIM, len(self.env.action_space.sample()), LR, self.device, self.summary_writer, self.timestamp, self.environment_name) self.cem_policy = MPC(self.env, PLAN_HORIZON, self.model, POPSIZE, NUM_ELITES, MAX_ITERS, use_random_optimizer=False, **mpc_params) self.random_policy = MPC(self.env, PLAN_HORIZON, self.model, POPSIZE, NUM_ELITES, MAX_ITERS, use_random_optimizer=True, **mpc_params) self.random_policy_no_mpc = RandomPolicy( len(self.env.action_space.sample()))
def __init__(self, A, B, C, Q, R, RD, umin, umax, N): self.A = A self.B = B self.C = C self.num_outputs = C.shape[0] self.num_inputs = B.shape[1] if self.num_inputs == self.num_outputs == 1: self.mtype = 0 else: self.mtype = 1 self.mpc = MPC(A, B, C, Q, R, RD, umin, umax, N) plt.rcParams['savefig.facecolor'] = 'xkcd:black'
def __init__(self, chain_length=4, alpha=pi / 4): super(Brush, self).__init__() # Add parts self.add(Silane(), label='silane') self.add(Initiator(), label='initiator') self.add(mb.recipes.Polymer(MPC(alpha=alpha), n=chain_length, port_labels=('up', 'down')), label='pmpc') self.add(CH3(), label='methyl') mb.force_overlap(self['initiator'], self['initiator']['down'], self['silane']['up']) mb.force_overlap(self['pmpc'], self['pmpc']['down'], self['initiator']['up']) mb.force_overlap(self['methyl'], self['methyl']['up'], self['pmpc']['up']) # Make self.port point to silane.bottom_port self.add(self['silane']['down'], label='down', containment=False)
def __init__(self, moos_community, moos_port): """Initiates MOOSComms, sets the callbacks and runs the loop""" super(mpcMOOS, self).__init__() self.server = moos_community self.port = moos_port self.name = 'mpcMOOS' self.lock = threading.Lock() self.set_on_connect_callback(self.__on_connect) self.set_on_mail_callback(self.__on_new_mail) self.add_active_queue('path_queue', self.on_path) self.add_message_route_to_active_queue('path_queue', 'PATH_X') self.add_message_route_to_active_queue('path_queue', 'PATH_Y') self.add_active_queue('control_queue', self.on_vessel_state) self.add_message_route_to_active_queue('control_queue', 'VESSEL_STATE') self.path_x = [] self.path_y = [] self.mpc = MPC() print('MPC created') self.run(self.server, self.port, self.name)
P = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R)) # Instantiate controller max_fz = 4 * Quadcopter_orginal.m * Quadcopter_orginal.g max_tau = (max_fz / 2) * (17.5 / 100) tau_z = 0.01 ctl = MPC( model=Quadcopter_orginal, dynamics=Quadcopter_orginal.discrete_time_dynamics, # Linear MPC Q=Q, R=R, P=P, horizon=0.5, ulb=[-max_fz, -max_tau, -max_tau, -tau_z], uub=[max_fz, max_tau, max_tau, tau_z], xlb=[ -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.pi / 2, -np.pi / 2, -np.pi / 2, -np.inf, -np.inf, -np.inf ], xub=[ np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.pi / 2, np.pi / 2, np.pi / 2, np.inf, np.inf, np.inf ], integrator=False) ctl.set_reference(x_sp=np.array([0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Q2-1 ctl.set_constant_control([0, 0, 0, 0]) sim_env = EmbeddedSimEnvironment( model=Quadcopter_orginal,
from mpc import MPC from control_room import ControlRoom import cvxpy as cp from cvxpy.atoms.elementwise.abs import abs as cp_abs import random random.seed(3) # construct MPC class mpc = MPC( ctr_mu=ctr_mu, n_slots=n_slots, ad_opportunities_params=ad_opportunities_params, ad_opportunities_rate_initial=ad_opportunities_rate_initial, b_star_params=b_star_params, b_star_initial=b_star_initial, ctr_params=ctr_params, ctr_initial=ctr_initial, cov=cov, bid_price_initial=bid_price_initial, bid_uncertainty_initial=bid_uncertainty_initial ) # 0. Initialize campaign without MPC informed bidding for i in range(100): ad_data = mpc.simulate_data() cost = ad_data["cost"] # Update historic cost data past_costs = mpc.update_history(past_costs, cost)
# pendulum = Pendulum(window) # pendulum.set_properties(m=1.0, g=-9.8, L=0.25, k=0.1) # pendulum.set_position(0.5, 0.5) # pendulum.set_rotation(np.pi * 0.95) # pendulum.integration_type = pendulum.RUNGE_KUTTA # pendulum.nonlinear_mode = True # # cart = CartPole(window) # cart.set_properties(m=0.2, M=0.5, I=0.006, g=-9.8, l=0.25, b=0.5) # cart.set_position(0.5, 0.5) # cart.set_rotation(np.pi * 0.99) # cart.nonlinear_mode = True # cart.integration_type = cart.RUNGE_KUTTA mpc = MPC(dt, mpc_horizon) # This works for springdamper and pendulum R = np.matrix(np.eye(2) * 1.0e-4) Q = np.matrix(np.matrix([[100, 0], [0.0, 0.0]])) T = np.matrix(np.eye(2) * 1e-4) mpc.set_cost_weights(Q, R, T) mpc.Y_prime = np.matrix(np.zeros(shape=(2, mpc_horizon))) mpc.Y_prime[0, :] = 0.7 mpc.C = np.matrix(np.eye(2)) mpc.set_body(spring_damper) Xs = np.zeros(shape=(mpc.body.get_state().shape[0], time.shape[0])) Us = np.zeros(shape=(mpc.body.u.shape[0], time.shape[0]))
import time import numpy as np import serial.tools.list_ports import scipy.linalg as linalg import matplotlib.pyplot as plt from analyzer import EncoderAnalyzer, ImageAnalyzer from motor_control import SabreControl from cart_pole import CartPole import serial.tools.list_ports from mpc import MPC mpc = MPC(0.5, 0, 0, 0) ports = list(serial.tools.list_ports.comports()) print(ports) for p in ports: print(p) if p[2] == "USB VID:PID=268b:0201 SNR=160045F45953": sabre_port = p[0] elif p[2] == "USB VID:PID=2341:0043 SNR=75334323935351D0D022": ard_port = p[0] motor = SabreControl(port=sabre_port) encoder = EncoderAnalyzer(port=ard_port) #image = ImageAnalyzer(0,show_image=True) cart = CartPole(motor, encoder, encoder, encoder) cart.zeroAngleAnalyzer() #encoder.setAngleZero()
def __init__(self): """Whole project is run from this constructor """ # +++++++++++++++ Init +++++++++++++++ # self.keep_run = 1 # used in run for daemon loop, reset by SIGTERM self.ret_code = 0 # return code to command line (10 = shutdown) # +++++++++++++++ Objects +++++++++++++++ # # Each inner object will get reference to PyBlaster as self.main. # exceptions in child threads are put here self.ex_queue = queue.Queue() self.log = Log(self) self.settings = Settings(self) self.settings.parse() PB_GPIO.init_gpio(self) self.led = LED(self) self.buttons = Buttons(self) self.lirc = Lirc(self) self.dbhandle = DBHandle(self) self.cmd = Cmd(self) self.mpc = MPC(self) self.bt = RFCommServer(self) self.alsa = AlsaMixer(self) self.i2c = I2C(self) self.usb = UsbDrive(self) # +++++++++++++++ Init Objects +++++++++++++++ # # Make sure to run init functions in proper order! # Some might depend upon others ;) self.led.init_leds() self.dbhandle.dbconnect() self.mpc.connect() self.bt.start_server_thread() self.alsa.init_alsa() self.i2c.open_bus() self.usb.start_uploader_thread() # +++++++++++++++ Daemoninze +++++++++++++++ # self.check_pidfile() self.daemonize() self.create_pidfile() # +++++++++++++++ Daemon loop +++++++++++++++ # self.led.show_init_done() self.buttons.start() self.lirc.start() self.run() # +++++++++++++++ Finalize +++++++++++++++ # self.log.write(log.MESSAGE, "Joining threads...") # join remaining threads self.usb.join() self.bt.join() self.buttons.join() self.lirc.join() self.mpc.join() self.led.join() self.log.write(log.MESSAGE, "leaving...") # cleanup self.mpc.exit_client() self.delete_pidfile() PB_GPIO.cleanup(self)
import mpc.MPC as MPC if __name__ == '__main__': MpcMachine = MPC()
pendulum = Pendulum() # Get the system discrete-time dynamics A, B, Bw, C = pendulum.get_discrete_system_matrices_at_eq() # Solve the ARE for our system to extract the terminal weight matrix P Q = np.eye(4) * 1 R = np.eye(1) * 1 P = np.eye(4) * 1 # Instantiate controller ctl = MPC(model=pendulum, dynamics=pendulum.discrete_time_dynamics, horizon=7, Q=Q, R=R, P=P, ulb=-5, uub=5, xlb=[-2, -10, -np.pi / 2, -np.pi / 2], xub=[12, 10, np.pi / 2, np.pi / 2]) # Solve without disturbance ctl.set_reference(x_sp=np.array([10, 0, 0, 0])) sim_env_full = EmbeddedSimEnvironment( model=pendulum, dynamics=pendulum.pendulum_linear_dynamics_with_disturbance, controller=ctl.mpc_controller, time=6) sim_env_full.run([0, 0, 0, 0]) # Solve witho disturbance
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05) obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05) obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07) obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08) obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06) map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9]) """ Get configured do-mpc modules: """ vehicle = simple_bycicle_model(length=0.12, width=0.06, Ts=0.05, reference_path=reference_path) model = vehicle.model controller = MPC(vehicle) mpc = controller.mpc simulator = Simulator(vehicle).simulator # Compute speed profile ay_max = 4.0 # m/s^2 a_min = -0.1 # m/s^2 a_max = 0.5 # m/s^2 SpeedProfileConstraints = { 'a_min': a_min, 'a_max': a_max, 'v_min': 0.0, 'v_max': 1.0, 'ay_max': ay_max }
save(folder + 'ftocp_sPred.npy', ftocp.sPred) save(folder + 'ftocp_xPred.npy', ftocp.xPred) save(folder + 'ftocp_sPred.npy', ftocp.sPred) pdb.set_trace() if newCost == 1: Q = 10**(-4) * np.diag([1, 10, 1, 1]) R = 10**(-4) * np.diag([1, 0.1]) maxIt = 41 else: maxIt = 1 if tracking == 0: mpc = MPC(N_mpc, n, d, Q, R, Qf, xlb, xub, ulb, uub, dt, xref) else: Q = 100 * np.diag([1, 1, 1, 1]) R = np.diag([1, 1]) mpc = MPC_tracking(N_mpc, n, d, Q, R, Qf, xlb, xub, ulb, uub, dt) mpc.addTraj(ftocp.xPred, ftocp.uPred, ftocp.sPred, region) tMax = 600 pltFlag = 0 IC = [] IC.append(np.array([0.0, 0.0, 0.0, 0.0])) # 0 IC.append(np.array([0.0, -0.03, 0.0, 0.0])) # 1 IC.append(np.array([0.005, 0.0, 0.0, 0.0])) # 2 IC.append(np.array([0.0, 0.0, 0.0, 0.2])) # 3
fig, ax = plt.subplots(nrows=2, ncols=1) ax[0].plot(t_des, q_des[0].T, 'o') ax[0].plot(t_fine, y_fine[0].T) ax[0].set_title('Desired Position') ax[1].plot(t_des, q_des[1].T, 'o') ax[1].plot(t_fine, y_fine[1].T) ax[1].set_title('Desired Angle') plt.show() # initialize MPC controller = MPC(m1=m1, m2=m2, l=l, g=g, u_max=u_max, d_max=d_max, tf=tf, N=N, Q=Q, Qf=Qf, R=R, verbose=verbose) # cartpole dynamics used in integration def qddot(x, u): q1ddot = (l*m2*np.sin(x[1])*x[3]**2 + u + \ m2*g*np.cos(x[1])*np.sin(x[1]))/ \ (m1 + m2*(1 - np.cos(x[1])**2)) q2ddot = -1*(l*m2*np.cos(x[1])*np.sin(x[1])*x[3]**2 \ + u*np.cos(x[1]) + (m1 + m2)*g*np.sin(x[1])) / \ (l*m1 + l*m2*(1 - np.cos(x[1])**2))