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
