def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def construct_parameters(self):
entries = []
for label, child in self.children.items():
entries_child = []
for name, par in child._parameters.items():
entries_child.append(entry(name, shape=par.shape))
entries.append(entry(label, struct=struct(entries_child)))
self._par_struct = struct(entries)
return struct_symMX(self._par_struct)
def construct_parameters(self):
entries = []
for label, child in self.children.items():
entries_child = []
for name, par in child._parameters.items():
entries_child.append(entry(name, shape=par.shape))
entries.append(entry(label, struct=struct(entries_child)))
self._par_struct = struct(entries)
return struct_symMX(self._par_struct)
def setup_nlp_p_fix(V, model):
# fixed system parameters
p_fix = cas.struct_symSX([(
cas.entry('ref', struct=V), # tracking reference for cost function
cas.entry('weights',
struct=model.variables) # weights for cost function
)])
return p_fix
def create_belief_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Non-linear constraints
[g, lbg, ubg] = cls._create_belief_nonlinear_constraints(model, V)
# Objective function
J = cls._create_belief_objective_function(model, V)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Warm start
# 'warm_start_init_point': 'yes',
# Termination
'max_iter': 1500,
'tol': 1e-6,
'constr_viol_tol': 1e-5,
'compl_inf_tol': 1e-4,
# Acceptable termination
'acceptable_tol': 1e-3,
'acceptable_iter': 5,
'acceptable_obj_change_tol': 1e-2,
# NLP
# 'fixed_variable_treatment': 'make_constraint',
# Quasi-Newton
'hessian_approximation': 'limited-memory',
'limited_memory_max_history': 5,
'limited_memory_max_skipping': 1}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def make_stage_constraint_struct(model):
# make entry list to check if not empty
entry_list = [cas.entry('collocation', shape =model.dynamics(model.variables, model.parameters).size())]
if list(model.constraints.keys()): # check if not empty
entry_list.append(cas.entry('path_constraints', struct = model.constraints))
# stage constraints structure -- necessary for interleaving
stage_constraints = cas.struct_symSX(entry_list)
return stage_constraints
def setup_nlp_p(V, model):
cost = setup_nlp_cost()
p_fix = setup_nlp_p_fix(V, model)
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0'])
])
return P
def generate_parameterization_settings(self, options):
[
periodic, initial_conditions, param_initial_conditions,
param_terminal_conditions, terminal_inequalities,
integral_constraints
] = operation.get_operation_conditions(options)
xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))])
xi_bounds = {}
xi_bounds['xi_0'] = [0.0, 0.0]
xi_bounds['xi_f'] = [0.0, 0.0]
V_pickle_initial = None
plot_dict_pickle_initial = None
V_pickle_terminal = None
plot_dict_pickle_terminal = None
if param_initial_conditions:
xi_bounds['xi_0'] = [0.0, 1.0]
if options['trajectory']['type'] == 'compromised_landing':
xi_0 = options['landing']['xi_0_initial']
xi_bounds['xi_0'] = [xi_0, xi_0]
V_pickle_initial, plot_dict_pickle_initial = self.__get_V_pickle(
options, 'initial')
if param_terminal_conditions:
xi_bounds['xi_f'] = [0.0, 1.0]
V_pickle_terminal, plot_dict_pickle_terminal = self.__get_V_pickle(
options, 'terminal')
if param_terminal_conditions and param_initial_conditions:
for theta in struct_op.subkeys(V_pickle_initial, 'theta'):
diff = V_pickle_terminal['theta',
theta] - V_pickle_initial['theta',
theta]
if theta != 't_f':
if (float(diff) != 0.0):
raise ValueError(
'Parameters of initial and terminal trajectory are not identical.'
)
xi_dict = {}
xi_dict['V_pickle_initial'] = V_pickle_initial
xi_dict['plot_dict_pickle_initial'] = plot_dict_pickle_initial
xi_dict['V_pickle_terminal'] = V_pickle_terminal
xi_dict['plot_dict_pickle_terminal'] = plot_dict_pickle_terminal
xi_dict['xi'] = xi
xi_dict['xi_bounds'] = xi_bounds
self.__xi_dict = xi_dict
return None
def _create_struct_from_dict(dictionary):
entries = []
for key, data in dictionary.items():
if isinstance(data, dict):
stru = _create_struct_from_dict(data)
entries.append(entry(str(key), struct=stru))
else:
if isinstance(data, list):
sh = len(data)
else:
sh = data.shape
entries.append(entry(key, shape=sh))
return struct(entries)
def get_phase_fix_theta(variables_dict):
entry_list = []
for name in list(variables_dict['theta'].keys()):
if name == 't_f':
entry_list.append(cas.entry('t_f', shape=(2, 1)))
else:
entry_list.append(
cas.entry(name, shape=variables_dict['theta'][name].shape))
theta = cas.struct_symSX(entry_list)
return theta
def _create_struct_from_dict(dictionary):
entries = []
for key, data in dictionary.items():
if isinstance(data, dict):
stru = _create_struct_from_dict(data)
entries.append(entry(str(key), struct=stru))
else:
if isinstance(data, list):
sh = len(data)
else:
sh = data.shape
entries.append(entry(key, shape=sh))
return struct(entries)
def create_simple_plan(x0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
# Regularize controls
J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and non-linear control constraints
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
g = ca.vertcat(g)
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
opts = {}
#opts['hessian_approximation'] = 'limited-memory'
opts['linear_solver'] = 'ma57'
# Create a solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = x0
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def get_collocation_variables_struct(self, variables_dict, u_param):
entry_list = [
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa'])
]
if 'xl' in list(variables_dict.keys()):
entry_list += [cas.entry('xl', struct=variables_dict['xl'])]
if u_param == 'poly':
entry_list += [cas.entry('u', struct=variables_dict['u'])]
return cas.struct_symMX(entry_list)
def get_collocation_variables_struct(self, variables_dict):
if 'xl' in list(variables_dict.keys()):
coll_var = cas.struct_symSX([
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa']),
cas.entry('xl', struct=variables_dict['xl'])
])
else:
coll_var = cas.struct_symSX([
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa']),
])
return coll_var
def generate_variable_struct(variable_list):
structs = {}
for name in list(variable_list.keys()):
structs[name] = cas.struct_symSX([
cas.entry(variable_list[name][i][0],
shape=variable_list[name][i][1])
for i in range(len(variable_list[name]))
])
variable_struct = cas.struct_symSX([
cas.entry(name, struct=structs[name])
for name in list(variable_list.keys())
])
return variable_struct, structs
def generate_system_parameters(options):
# extract parametric options
parametric_options = options['params']
parameters_dict = {}
# generate nested casadi structure for system parameters
parameters_dict['theta0'] = struct_op.generate_nested_dict_struct(parametric_options)
parameters_dict['phi'] = generate_optimization_parameters()
parameters = cas.struct_symSX([
cas.entry('theta0', struct= parameters_dict['theta0']),
cas.entry('phi', struct= parameters_dict['phi'])]
)
return parameters, parameters_dict
def get_component_cost_structure(component_costs):
list_of_entries = []
for name in list(component_costs.keys()):
list_of_entries += [cas.entry(name)]
component_cost_struct = cas.struct_symMX(list_of_entries)
return component_cost_struct
def create_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Force the catcher to always look forward
# lbx['U', :, 'theta'] = ubx['U', :, 'theta'] = 0
# Non-linear constraints
[g, lbg, ubg] = cls._create_nonlinear_constraints(model, V)
# Objective function
J = cls._create_objective_function(model, V, warm_start)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Acceptable termination
'acceptable_iter': 5}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def gen(NX, NU, M):
"""gen Generates a casadi struct containing the symbolic optimization parameters
x_cur (the initial state), x_ref (the reference states) and u_cur (the reference controls).
x_cur is a <NX>x<1> vector, x_ref is a <NX>x<M+1> matrix, u_ref is a <NU>x<M> matrix
:param NX: Number of reference state variables
:param NU: Number of reference control variables
:param M: Prediction horizon length
:returns: A casadi struct_symSX object
"""
params = struct_symSX([
entry('x_cur', shape=NX),
entry('x_ref', shape=(NX, M + 1)),
entry('u_ref', shape=(NU, M))
])
return params
def get_structure(self, cstr_type):
cstr_list = self.get_list(cstr_type)
entry_list = []
for cstr in cstr_list:
joined_name = cstr.name
local = cas.entry(joined_name, shape=cstr.expr.shape)
entry_list.append(local)
return cas.struct_symSX(entry_list)
def setup_integral_output_structure(nlp_numerics_options, integral_outputs):
nk = nlp_numerics_options['n_k']
entry_tuple = ()
# interval outputs
entry_tuple += (cas.entry('int_out',
repeat=[nk + 1],
struct=integral_outputs), )
if nlp_numerics_options['discretization'] == 'direct_collocation':
d = nlp_numerics_options['collocation']['d']
entry_tuple += (cas.entry('coll_int_out',
repeat=[nk, d],
struct=integral_outputs), )
Integral_outputs_struct = cas.struct_symMX([entry_tuple])
return Integral_outputs_struct
def setup_output_structure(nlp_numerics_options, model_outputs, form_outputs):
# create outputs
# n_o_t_e: !!! outputs are defined at nodes where both state and algebraic variables
# are defined. In the radau case, algebraic variables are not defined on the interval
# nodes, note however that algebraic variables might be discontinuous so the same
# holds for the outputs!!!
nk = nlp_numerics_options['n_k']
entry_tuple = ()
if nlp_numerics_options['discretization'] == 'direct_collocation':
# extract collocation parameters
d = nlp_numerics_options['collocation']['d']
scheme = nlp_numerics_options['collocation']['scheme']
if scheme != 'radau':
# define outputs on interval and collocation nodes
entry_tuple += (
cas.entry('outputs', repeat=[nk], struct=model_outputs),
cas.entry('coll_outputs', repeat=[nk, d],
struct=model_outputs),
)
else:
# define outputs on collocation nodes
entry_tuple += (cas.entry('coll_outputs',
repeat=[nk, d],
struct=model_outputs), )
elif nlp_numerics_options['discretization'] == 'multiple_shooting':
# define outputs on interval nodes
entry_tuple += (cas.entry('outputs', repeat=[nk],
struct=model_outputs), )
Outputs = cas.struct_symSX([entry_tuple] +
[cas.entry('final', struct=form_outputs)])
return Outputs
def gen(NX, NU, M, stateBounds=None, controlBounds=None):
"""gen Generates a casadi struct containing the symbolic optimization variables required
for direct multiple shooting. x is a <NX>x<M+1> matrix, u is a <NU>x<M> matrix.
:param NX: Number of state variables
:param NU: Number of control variables
:param M: Prediction horizon length
:returns: A casadi struct_symSX object
"""
# decision (free) variables
variables = struct_symSX(
[entry('x', shape=(NX, M + 1)),
entry('u', shape=(NU, M))])
# symbolic bounds
bx = ca.SX.sym('bx', NX)
bu = ca.SX.sym('bu', NU)
# bounds struct, must be identical to variables struct in dimensions and keys
bounds = struct_SX([
entry('x', expr=ca.repmat(bx, 1, M + 1)),
entry('u', expr=ca.repmat(bu, 1, M))
])
boundsFun = ca.Function('varBoundsFun', [bx, bu], [bounds.cat])
if stateBounds is None:
stateBounds = np.multiply(np.ones((2, NX)),
np.array((-np.inf, np.inf), ndmin=2).T)
if controlBounds is None:
controlBounds = np.multiply(np.ones((2, NU)),
np.array((-np.inf, np.inf), ndmin=2).T)
lbw = boundsFun(stateBounds[0, :], controlBounds[0, :])
ubw = boundsFun(stateBounds[1, :], controlBounds[1, :])
return variables, lbw, ubw
def make_entry_list(eqs_dict, ineqs_dict):
# make entry list for all non-empty dicts
entry_list = []
if eqs_dict: # check if not empty
# equality constraint struct
eq_struct = cas.struct_symSX([
cas.entry(name, shape=eqs_dict[name].size())
for name in list(eqs_dict.keys())
])
entry_list.append(cas.entry('equality', struct=eq_struct))
if ineqs_dict: # check if not empty
# inequality constraint struct
ineq_struct = cas.struct_symSX([
cas.entry(name, shape=ineqs_dict[name].size())
for name in list(ineqs_dict.keys())
])
entry_list.append(cas.entry('inequality', struct=ineq_struct))
return entry_list
def create_simple_plan(x0, F, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final position
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
def test_affine_constraint():
var = struct_symSX([entry('x', shape=(2, 1))])
linear_coefficient = np.eye(2) * 2
affine_coefficient = np.ones((2, ))
lower_bound = np.zeros((2, 1))
upper_bound = np.ones((2, 1))
constr = AffineConstraint.gen(var, linear_coefficient, affine_coefficient,
lower_bound, upper_bound)
g, lb, ub = constr[0]
assert (lb == (lower_bound - affine_coefficient)).all()
assert (ub == (upper_bound - affine_coefficient)).all()
assert '@1=2, [(@1*x_0), (@1*x_1)]' == str(g)
assert np.equal(Function('g_fun', [var['x']], [g])([2, 2]), [4, 4]).all()
def generate_optimization_parameters():
# variable system parameters
p_dec = cas.struct_symSX([(
cas.entry('gamma'), # force homotopy variable
cas.entry('tau'), # tether drag homotopy variable
cas.entry('iota'), # induction homotopy variable
cas.entry('psi'), # power homotopy variable
cas.entry('eta'), # nominal landing homotopy variable
cas.entry('nu'), # compromised landing homotopy variable
cas.entry('upsilon'), # transition homotopy variable
)])
optimization_parameters = p_dec
return optimization_parameters
def construct_constraints(self, variables, parameters):
entries = []
for child in self.children.values():
for name, constraint in child._constraints.items():
expression = self._substitute_symbols(
constraint[0], variables, parameters)
entries.append(entry(child._add_label(name), expr=expression))
self._con_struct = struct(entries)
constraints = struct_MX(entries)
self._lb, self._ub = constraints(0), constraints(0)
self._constraint_shutdown = {}
for child in self.children.values():
for name, constraint in child._constraints.items():
self._lb[child._add_label(name)] = constraint[1]
self._ub[child._add_label(name)] = constraint[2]
if constraint[3]:
self._constraint_shutdown[
child._add_label(name)] = constraint[3]
return constraints, self._lb, self._ub
def construct_constraints(self, variables, parameters):
entries = []
for child in self.children.values():
for name, constraint in child._constraints.items():
expression = self._substitute_symbols(constraint[0], variables,
parameters)
entries.append(entry(child._add_label(name), expr=expression))
self._con_struct = struct(entries)
constraints = struct_MX(entries)
self._lb, self._ub = constraints(0), constraints(0)
self._constraint_shutdown = {}
for child in self.children.values():
for name, constraint in child._constraints.items():
self._lb[child._add_label(name)] = constraint[1]
self._ub[child._add_label(name)] = constraint[2]
if constraint[3]:
self._constraint_shutdown[child._add_label(
name)] = constraint[3]
return constraints, self._lb, self._ub
def build_integrator(self, options, model):
print('Building integrator...')
# get model variables
variables = model.variables
# construct the DAE variables
x = cas.struct_SX([cas.entry('xd', expr = variables['xd'])]) # differential states
z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives
cas.entry('xa', expr = variables['xa']), # algebraic variables
cas.entry('xl', expr = variables['xl']), # lifted variables
])
p = cas.struct_SX([cas.entry('u', expr = variables['u']), # dae parameters
cas.entry('theta', expr = variables['theta']),
cas.entry('phi', expr = model.parameters)])
# scale xddot with t_f
time_scaled_variables = scale_xddot(variables)
# model equations
alg = model.dynamics(time_scaled_variables, model.parameters)
ode = variables['xddot']
# create dae
dae = {'x': x.cat, 'z': z.cat, 'p': p.cat, 'alg': alg,'ode': ode}
# system dynamics
f = cas.Function('f', [x, z, p], [ode, alg], ['x', 'z', 'p'], ['ode', 'alg'])
# create integrator and rootfinder
if cas.sprank(cas.jacobian(alg,z)) < z.cat.size()[0]: # check dae index
raise ValueError('jacobian of dynamics is structurally rank-deficient: DAE is not of index 1!')
else:
# create integrator
I = cas.integrator('I', options['integrator']['type'], dae, {'tf': 1.0 / self.__N_sim})
# create rootfinder
g = cas.Function('g',[z.cat,x.cat,p.cat],[alg])
G = cas.rootfinder('G', 'newton', g, {'linear_solver': 'csparse'})
self.__integrator = I
self.__rootfinder = G
self.__variables_dict = model.variables_dict
self.__phi = model.parameters
self.__dae = dae
self.__f = f
self.__x = x
self.__z = z
self.__p = p
def setup_nlp_p(V, model):
cost = setup_nlp_cost()
p_fix = setup_nlp_p_fix(V, model)
use_vortex_linearization = 'lin' in model.parameters_dict.keys()
if use_vortex_linearization:
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0']),
cas.entry('lin', struct=V)
])
else:
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0'])
])
return P
def __get_sstruct(dictionary):
"""Convert a dictionary to a casadi struct with the same structure and entries for 0th, 1st and 2nd derivatives
:type dictionary: dict
:param dictionary: variables to be put into the struct
:rtype: casadi.struct_symSX
"""
struct_list = []
for key in list(dictionary.keys()):
if key[0] != 'd':
struct_list += [(key, dictionary[key].shape)]
sub_struct = ct.struct_symSX([
ct.entry(struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
dsub_struct = ct.struct_symSX([
ct.entry('d' + struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
ddsub_struct = ct.struct_symSX([
ct.entry('dd' + struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
sstruct = ct.struct_symSX([
ct.entry('var', struct=sub_struct),
ct.entry('dvar', struct=dsub_struct),
ct.entry('ddvar', struct=ddsub_struct)
])
return sstruct
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(["x_b", "y_b", "z_b", "vx_b", "vy_b", "vz_b", "x_c", "y_c", "phi"])
# Control
control = cat.struct_symSX(["v", "w", "psi"])
# Observation
observation = cat.struct_SX(
[
cat.entry("x_b", expr=state["x_b"]),
cat.entry("y_b", expr=state["y_b"]),
cat.entry("z_b", expr=state["z_b"]),
cat.entry("x_c", expr=state["x_c"]),
cat.entry("y_c", expr=state["y_c"]),
cat.entry("phi", expr=state["phi"]),
]
)
# Belief state (mu, Sigma)
belief = cat.struct_symSX([cat.entry("m", struct=state), cat.entry("S", shapestruct=(state, state))])
# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX(
[
cat.entry("m", struct=state),
def _construct_upd_z_nlp(self):
# construct variables
self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct),
entry('z_ij', struct=self.q_ij_struct)])
var = struct_symMX(self._var_struct_updz)
z_i = self.q_i_struct(var['z_i'])
z_ij = self.q_ij_struct(var['z_ij'])
# construct parameters
self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct),
entry('x_j', struct=self.q_ij_struct),
entry('l_i', struct=self.q_i_struct),
entry('l_ij', struct=self.q_ij_struct),
entry('t'), entry('T'), entry('rho'),
entry('par', struct=self.par_struct)])
par = struct_symMX(self._par_struct_updz)
x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j'])
l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij'])
t, T, rho = par['t'], par['T'], par['rho']
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# construct constraints
constraints, lb, ub = [], [], []
for con in self.constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_i[label, name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_i.items():
if s.getName() == sym.getName():
c = substitute(c, sym, par['par', name])
constraints.append(c)
lb.append(con[1])
ub.append(con[2])
self.lb_updz, self.ub_updz = lb, ub
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label, name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name in q_ij.keys():
x = x_j[str(nghb), child.label, name]
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
# construct problem
prob, compile_time = self.father.create_nlp(var, par, obj,
constraints, self.options)
self.problem_upd_z = prob
def create_plan_pc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final means
m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g = []
lbg = []
ubg = []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v
lbx['U',:,'v'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
def shift_trajectory(state, u):
##########################################################
## state and u in form (state, N)
state_ = np.concatenate((state.T[1:], state.T[-1:]))
u_ = np.concatenate((u.T[1:], u.T[-1:]))
return u_, state_
if __name__ == '__main__':
T = 0.2 # sampling time [s]
N = 100 # prediction horizon
rob_diam = 0.3 # [m]
v_max = 0.6
omega_max = np.pi / 4.0
states = ca_tools.struct_symSX([(ca_tools.entry('x'), ca_tools.entry('y'),
ca_tools.entry('theta'))])
x, y, theta = states[...]
n_states = states.size
controls = ca_tools.struct_symSX([(ca_tools.entry('v'),
ca_tools.entry('omega'))])
v, omega = controls[...]
n_controls = controls.size
## rhs
rhs = ca_tools.struct_SX(states)
rhs['x'] = v * ca.cos(theta)
rhs['y'] = v * ca.sin(theta)
rhs['theta'] = omega
#ulb = np.array([0.01, 299])
#xlb = np.array([0.5, 0.8, 0])
#xub = np.array([2.5, 2.0, 400])
#<<ENDCHUNK>>
# Make optimizers.
x0 = np.array([.05 * cs, .75 * Ts, .5 * hs])
u0 = np.array([Tcs, Fs])
umax = np.array([.05 * Tcs, .15 * Fs])
dumax = .2 * umax
xlb = np.array([-np.inf, -np.inf, -np.inf])
xub = np.array([np.inf, np.inf, np.inf])
# Create variables struct.
var = ctools.struct_symSX([(
ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("u", shape=(Nu, ), repeat=Nt),
ctools.entry("Du", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)
# Create parameters struct.
par = ctools.struct_symSX([
ctools.entry("x_sp", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("u_sp", shape=(Nu, ), repeat=Nt),
ctools.entry("uprev", shape=(Nu, 1)),
ctools.entry("Ad", repeat=Nt, shape=(Nx, Nx)),
ctools.entry("Bd", repeat=Nt, shape=(Nx, Nu)),
ctools.entry("fd", repeat=Nt, shape=(Nx, 1))
lh_x = l.hessian('state') # l_xx
lg_u = l.gradient('control') # l_u
lh_u = l.hessian('control') # l_uu
lgj_ux = ca.SXFunction('l_ux_fun', # l_ux
[state, control, dt_sym],
[ca.jacobian(l.grad('control'), state)])
# ----------------------------------------------------------------------------
# Optimization
# ----------------------------------------------------------------------------
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=n_sim + 1, struct=state),
cat.entry('U', repeat=n_sim, struct=control)
)
])
# Multiple shooting constraints
g = []
for k in range(n_sim):
g.append(V['X', k + 1] - F([V['X', k], V['U', k], dt])[0])
g = ca.vertcat(g)
# Objective
[final_cost] = lf([V['X', n_sim]])
J = final_cost
# Regularize controls
def get_costs_struct(V):
costs_struct = cas.struct_symSX([
cas.entry("tracking_cost"),
cas.entry("ddq_regularisation_cost"),
cas.entry("u_regularisation_cost"),
cas.entry("fictitious_cost"),
cas.entry("theta_regularisation_cost"),
cas.entry("slack_cost")
] + [cas.entry(name + '_cost') for name in struct_op.subkeys(V, 'phi')] + [
cas.entry("time_cost"),
cas.entry("power_cost"),
cas.entry("nominal_landing_cost"),
cas.entry("transition_cost"),
cas.entry("compromised_battery_cost"),
cas.entry("tracking_problem_cost"),
cas.entry("power_problem_cost"),
cas.entry("general_problem_cost"),
cas.entry("objective")
])
return costs_struct
lfunc = (casadi.mtimes(x.T, x) + casadi.mtimes(u.T, u))
l = casadi.Function("l", [x, u], [lfunc])
Pffunc = casadi.mtimes(x.T, x)
Pf = casadi.Function("Pf", [x], [Pffunc])
# Bounds on u.
uub = 1
ulb = -.75
# Make optimizers.
x0 = np.array([0, 1])
# Create variables struct.
var = ctools.struct_symSX([(
ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("xc", shape=(Nx, Nc), repeat=Nt),
ctools.entry("u", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)
# Adjust the relevant constraints.
for t in range(Nt):
varlb["u", t, :] = ulb
varub["u", t, :] = uub
# Now build up constraints and objective.
obj = casadi.SX(0)
con = []
import casadi.tools as CT
import numpy
from LLT_solver import setupImplicitFunction
import geometry
import pylab
# geomRootChord = geomRoot[0]
# geomRootY = geomRoot[1]
# geomRootAinc = numpy.radians(geomRoot[2])
# geomTipChord = geomTip[0]
# geomTipY = geomTip[1]
# geomTipAinc = numpy.radians(geomTip[2])
n = 50
dvs = CT.struct_ssym(['operAlpha',
CT.entry('chordLoc',shape=n),
CT.entry('aIncGeometricLoc',shape=n),
CT.entry('An',shape=n)])
thetaLoc = numpy.linspace(1,n,n)*numpy.pi/(2.0*n)
bref = 10.0
yLoc = 0.5*bref*numpy.cos(thetaLoc)
chordLoc = dvs['chordLoc']
aIncGeometricLoc = dvs['aIncGeometricLoc']
aeroCLaRoot = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
aeroCLaTip = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
#aeroCLaRoot = numpy.array([ -28.2136, 16.4140, 0.9568,-0.4000])
#aeroCLaTip = numpy.array([ -28.2136, 16.4140, 0.9568, -0.4000])
clPolyLoc = numpy.zeros((4,n)) #setup lift slope curves for each station
for i in range(n):
def setup_nlp_cost():
cost = cas.struct_symSX([(
cas.entry('tracking'),
cas.entry('regularisation'),
cas.entry('ddq_regularisation'),
cas.entry('gamma'),
cas.entry('iota'),
cas.entry('psi'),
cas.entry('tau'),
cas.entry('eta'),
cas.entry('nu'),
cas.entry('upsilon'),
cas.entry('fictitious'),
cas.entry('power'),
cas.entry('t_f'),
cas.entry('theta'),
cas.entry('nominal_landing'),
cas.entry('compromised_battery'),
cas.entry('transition'),
)])
return cost
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(['x_b','y_b','vx_b','vy_b',
'x_c','y_c','phi'])
# Control
control = cat.struct_symSX(['v','w'])
# Observation
observation = cat.struct_SX([
cat.entry('x_b', expr = state['x_b']),
cat.entry('y_b', expr = state['y_b']),
cat.entry('x_c', expr = state['x_c']),
cat.entry('y_c', expr = state['y_c']),
cat.entry('phi', expr = state['phi'])
])
# Belief state (mu, Sigma)
belief = cat.struct_symSX([
cat.entry('m',struct=state),
cat.entry('S',shapestruct=(state,state))
])
# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX([
cat.entry('m',struct=state),
def create_plan_pc(b0, BF, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final coordinate
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
J += 1e1 * ca.trace(bk_next['S']) # uncertainty
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# State constraints
# catcher can look within the upper hemisphere
lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
# Control constraints
# 0 <= F
lbx['U',:,'F'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Take care of the time
#lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
# Simulation ends when the ball touches the ground
#lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
def setup_nlp_v(nlp_numerics_options, model, formulation, Collocation):
# extract necessary inputs
variables_dict = model.variables_dict
nk = nlp_numerics_options['n_k']
# check if phase fix and adjust theta accordingly
if nlp_numerics_options['phase_fix']:
theta = get_phase_fix_theta(variables_dict)
else:
theta = variables_dict['theta']
# define interval struct entries for controls and states
entry_tuple = (
cas.entry('xd', repeat=[nk + 1], struct=variables_dict['xd']),
cas.entry('u', repeat=[nk], struct=variables_dict['u']),
)
# add additional variables according to provided options
if nlp_numerics_options['discretization'] == 'direct_collocation':
# add algebraic variables at interval except for radau case
if nlp_numerics_options['collocation']['scheme'] != 'radau':
if nlp_numerics_options['lift_xddot']:
entry_tuple += (
cas.entry(
'xddot', repeat=[nk], struct=variables_dict['xddot']
), # depends on implementation (e.g. not present for radau collocation)
)
if nlp_numerics_options['lift_xa']:
entry_tuple += (
cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
) # depends on implementation (e.g. not present for radau collocation)
if 'xl' in list(variables_dict.keys()):
entry_tuple += (
cas.entry('xl',
repeat=[nk],
struct=variables_dict['xl']),
) # depends on implementation (e.g. not present for radau collocation)
# add collocation node variables
d = nlp_numerics_options['collocation'][
'd'] # interpolating polynomial order
coll_var = Collocation.get_collocation_variables_struct(variables_dict)
entry_tuple += (cas.entry('coll_var', struct=coll_var, repeat=[nk,
d]), )
elif nlp_numerics_options['discretization'] == 'multiple_shooting':
# add slack variables for inequalities
if nlp_numerics_options[
'slack_constraints'] == True and model.constraints_dict[
'inequality']:
entry_tuple += (cas.entry(
'us', repeat=[nk],
struct=model.constraints_dict['inequality']), )
# multiple shooting: add algebraic variables at interval if lifted
if nlp_numerics_options['lift_xddot']:
entry_tuple += (
cas.entry(
'xddot', repeat=[nk], struct=variables_dict['xddot']
), # depends on implementation (e.g. not present for radau collocation)
)
if nlp_numerics_options['lift_xa']:
entry_tuple += (
cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
) # depends on implementation (e.g. not present for radau collocation)
if 'xl' in list(variables_dict.keys()):
entry_tuple += (
cas.entry('xl', repeat=[nk], struct=variables_dict['xl']),
) # depends on implementation (e.g. not present for radau collocation)
# add global entries
entry_list = [entry_tuple]
entry_list += [
cas.entry('theta', struct=theta),
cas.entry('phi', struct=model.parameters_dict['phi']),
cas.entry('xi', struct=formulation.xi_dict['xi'])
]
# generate structure
V = cas.struct_symMX(entry_list)
return V
