"""
-----------------------------------------------
do-mpc: Definition of the do-mpc configuration
-----------------------------------------------
"""
# Import the user defined modules
import template_model
import template_optimizer
import template_observer
import template_simulator
# Create the objects for each module
model_1 = template_model.model()
# Create an optimizer object based on the template and a model
optimizer_1 = template_optimizer.optimizer(model_1)
# Create an observer object based on the template and a model
observer_1 = template_observer.observer(model_1)
# Create a simulator object based on the template and a model
simulator_1 = template_simulator.simulator(model_1)
# Create a configuration
configuration_1 = core_do_mpc.configuration(model_1, optimizer_1, observer_1,
simulator_1)
# Set up the solvers
configuration_1.setup_solver()
"""
----------------------------
do-mpc: MPC loop
----------------------------
"""
def getOptControl(self, waypoint, obstacles, pose, twist):
#weights
w = [80, 80, 1, 5e3]
# Create the objects for each module
model_1 = template_model.model(waypoint, obstacles, pose, twist, w)
# Create an optimizer object based on the template and a model
optimizer_1 = template_optimizer.optimizer(model_1)
# Create an observer object based on the template and a model
observer_1 = template_observer.observer(model_1)
# Create a simulator object based on the template and a model
simulator_1 = template_simulator.simulator(model_1)
# Create a configuration
configuration_1 = core_do_mpc.configuration(model_1, optimizer_1,
observer_1, simulator_1)
# Set up the solvers
configuration_1.setup_solver()
"""
----------------------------
do-mpc: MPC loop
----------------------------
"""
x = pose
Y_ref = waypoint
lam = w[3]
gamma = 1
#print "obstacles", len(obstacles.a)
#V=lam/((gamma+((xo[0]-x[0])**2)/(rx**2)+((xo[1]-x[1])**2)/(ry**2)))
#print "no. of obstacles", len(obstacles.a)
if len(obstacles.a):
V = 0
for i in xrange(len(obstacles.a)):
if obstacles.a[i]:
rx = obstacles.a[i]
ry = obstacles.b[i]
phi = atan(obstacles.m[i])
xo = obstacles.centroid[i]
R = [[cos(x[2]), -sin(x[2])], [sin(x[2]), cos(x[2])]]
xo = np.matmul(R, xo) + pose[0:2]
V = V + (lam) / ((gamma +
(((xo[0] - x[0]) * cos(phi) +
(xo[1] - x[1]) * sin(phi))**2) /
(rx**2) +
(((xo[1] - x[0]) * sin(phi) -
(xo[1] - x[1]) * cos(phi))**2) /
(ry**2)))
#print "cost",i, V
print "total cost", V
print "x cost:", w[0] * (x[0] - Y_ref[0])**2, "y cost:", w[1] * (
x[1] - Y_ref[1])**2, "theta cost:", w[2] * (x[2] - Y_ref[2])**2
#a1=ax.scatter(x[0],x[1])
#a2=ax.arrow(x[0],x[1],3*math.cos(x[2]),3*math.sin(x[2]), head_width=0.08, head_length=0.00002)
#plt.pause(0.1)
#count =0
"""
----------------------------
do-mpc: Optimizer
----------------------------
"""
# Make one optimizer step (solve the NLP)
start = time.time()
configuration_1.make_step_optimizer()
end = time.time()
#print "optimization time:",end-start
print "u_mpc: ", configuration_1.optimizer.u_mpc
#x=move(configuration_1.simulator.x0_sim,configuration_1.optimizer.u_mpc,1)
#a1=ax.scatter(x[0],x[1])
#a2=ax.arrow(x[0],x[1],3*math.cos(x[2]),3*math.sin(x[2]), head_width=0.08, head_length=0.00002)
#plt.pause(0.1)
"""
----------------------------
do-mpc: Simulator
----------------------------
"""
# Simulate the system one step using the solution obtained in the optimization
configuration_1.make_step_simulator()
"""
----------------------------
do-mpc: Observer
----------------------------
"""
# Make one observer step
configuration_1.make_step_observer()
"""
------------------------------------------------------
do-mpc: Prepare next iteration and store information
------------------------------------------------------
"""
# Store the information
configuration_1.store_mpc_data()
# Set initial condition constraint for the next iteration
configuration_1.prepare_next_iter()
"""
------------------------------------------------------
do-mpc: Plot MPC animation if chosen by the user
------------------------------------------------------
"""
# Plot animation if chosen in by the user
#data_do_mpc.plot_animation(configuration_1)
#print "dist:", dist(configuration_1.simulator.xf_sim, Y_ref)
#if dist(configuration_1.simulator.xf_sim, Y_ref)<=5:
# break
"""
------------------------------------------------------
do-mpc: Plot the closed-loop results
------------------------------------------------------
"""
#plt.show()
#data_do_mpc.plot_mpc(configuration_1)
# Export to matlab if wanted
#data_do_mpc.export_to_matlab(configuration_1)
return configuration_1.optimizer.u_mpc
""" ----------------------------------------------- do-mpc: Definition of the do-mpc configuration ----------------------------------------------- """ # Import the user defined modules import template_model import template_optimizer import template_observer import template_simulator # Create the objects for each module model_1 = template_model.model() # Create an optimizer object based on the template and a model optimizer_1 = template_optimizer.optimizer(model_1) # Create an observer object based on the template and a model observer_1 = template_observer.observer(model_1) # Create a simulator object based on the template and a model simulator_1 = template_simulator.simulator(model_1) # Create a configuration configuration_1 = core_do_mpc.configuration(model_1, optimizer_1, observer_1, simulator_1) # Set up the solvers configuration_1.setup_solver() """ ---------------------------- do-mpc: MPC loop ----------------------------
# Import the user defined modules
import template_model
import template_optimizer
import template_observer
import template_simulator
from step_simulator import step_simulator
from step_simulator import compare
from vars import *
configurations = []
for zone in zones:
# Create the objects for each module
vars()['model_' + zone] = template_model.model(zone)
# Create an optimizer object based on the template and a model
vars()['optimizer_' + zone] = template_optimizer.optimizer(
vars()['model_' + zone], zonenumber[zone])
# Create an observer object based on the template and a model
vars()['observer_' + zone] = template_observer.observer(vars()['model_' +
zone])
# Create a simulator object based on the template and a model
vars()['simulator_' + zone] = template_simulator.simulator(
vars()['model_' + zone], zonenumber[zone])
# Create a configuration
vars()['configuration_' + zone] = core_do_mpc.configuration(
vars()['model_' + zone],
vars()['optimizer_' + zone],
vars()['observer_' + zone],
vars()['simulator_' + zone])
# Set up the solvers
vars()['configuration_' + zone].setup_solver()