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