def eval_opt_ineq_N(self, z, p): x = z[:self.n_x] hyp_w = p[self.n_x+self.N_ineq*self.d_ineq+self.N_ineq : self.n_x+self.N_ineq*self.d_ineq+self.N_ineq+self.n_x] hyp_b = p[self.n_x+self.N_ineq*self.d_ineq+self.N_ineq+self.n_x] t_opt = z[0:2] R_opt = np.array([ [ca.cos(z[2]), -ca.sin(z[2])], [ca.sin(z[2]), ca.cos(z[2])] ]) obca = [] obca_d = [] obca_norm = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ self.n_x + j : self.n_x + j + self.n_ineq[i] ] mu = z[ self.n_x + self.N_ineq + i*self.m_ineq : self.n_x + self.N_ineq + (i+1)*self.m_ineq ] obca.append( ca.mtimes( ca.transpose(self.G), mu) + ca.mtimes( ca.transpose(ca.mtimes(A, R_opt)), Lambda ) ) obca_d.append( -ca.dot(self.g, mu) + ca.dot(ca.mtimes(A, t_opt)-b, Lambda) ) obca_norm.append( ca.dot(ca.mtimes( ca.transpose(A), Lambda), ca.mtimes( ca.transpose(A), Lambda)) ) j += self.n_ineq[i] opt_ineq = ca.vcat( [ca.vcat(obca), ca.vcat(obca_d), ca.vcat(obca_norm)] ) opt_ineq = ca.vcat( [opt_ineq, ca.dot(hyp_w, x) - hyp_b] ) return opt_ineq
def eval_ws_eq(self, z, p): t_ws = p[0:2] R_ws = np.array( [ [ca.cos(p[2]), -ca.sin(p[2])], [ca.sin(p[2]), ca.cos(p[2])] ] ) obca_d = [] obca = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ j : j + self.n_ineq[i] ] mu = z[ self.N_ineq + i*self.m_ineq : self.N_ineq + (i+1)*self.m_ineq ] d = z[ self.N_ineq + self.M_ineq + i ] obca_d.append( -ca.dot(self.g, mu) + ca.dot( ca.mtimes(A, t_ws)-b, Lambda ) - d) obca.append( ca.mtimes(self.G.T, mu) + ca.mtimes( ca.mtimes(A, R_ws).T, Lambda ) ) j += self.n_ineq[i] ws_eq = ca.vcat( [ca.vcat(obca_d), ca.vcat(obca)] ) return ws_eq
def error_contour_lag(self, z, p):
theta = z[self.index["theta_position"]]
px = z[self.index['x_position']]
py = z[self.index['y_position']]
pz = z[self.index['z_position']]
rx = casadi.polyval(p[self.index["px"]], theta)
ry = casadi.polyval(p[self.index["py"]], theta)
rz = casadi.polyval(p[self.index["pz"]], theta)
drx = casadi.polyval(p[self.index["pdx"]], theta)
dry = casadi.polyval(p[self.index["pdy"]], theta)
drz = casadi.polyval(p[self.index["pdz"]], theta)
tangent_normalized = self.calculate_tangent(drx, dry, drz)
# set point
s = casadi.vcat([rx, ry, rz])
pos = casadi.vcat([px, py, pz])
r = s - pos
# lag
lag = casadi.dot(r, tangent_normalized)
e_lag = lag**2
# contour
contour = r - (lag * tangent_normalized)
e_contour = casadi.dot(contour, contour)
return e_contour, e_lag
def trajectory_2pts(initial_state, final_state):
'''
Find point-to-point trajectory
'''
assert np.shape(initial_state) == np.shape(final_state)
ntrailers = len(initial_state) - 3
flat = get_flat_maps(ntrailers)
args = flat.flat_derivs.reshape((-1,1))
state_expr = vcat((
flat.positions[0][0], # x
flat.positions[0][1], # y
flat.phi,
*flat.theta
))
inp_expr = vcat((
flat.u1,
flat.u2,
))
state_fun = Function('state', [args], [state_expr])
inp_fun = Function('input', [args], [inp_expr])
flat_val1 = eval_flat_derivs(initial_state, flat)
flat_val2 = eval_flat_derivs(final_state, flat)
poly1 = make_poly_bv(flat_val1[:,0], flat_val2[:,0])
poly2 = make_poly_bv(flat_val1[:,1], flat_val2[:,1])
t = np.linspace(0, 1, 1000)
poly = make_poly_bv([0, 0], [1, 0])
s = polyval(t, poly)
nderivs = flat.flat_derivs.shape[0] - 1
out1_vals = get_poly_derivatives(s, poly1, nderivs)
out2_vals = get_poly_derivatives(s, poly2, nderivs)
out_vals = np.concatenate([out1_vals, out2_vals])
state_vals = state_fun(out_vals)
state_vals = np.array(state_vals.T, float)
for i in range(ntrailers):
fix_conitnuity(state_vals[:,3+i], 2*np.pi)
inp_vals = inp_fun(out_vals)
inp_vals = np.array(inp_vals.T, float)
traj = {
'ntrailers': ntrailers,
't': t,
'x': state_vals[:,0],
'y': state_vals[:,1],
'phi': state_vals[:,2],
'theta': state_vals[:,3:].T,
'u1': inp_vals[:,0],
'u2': inp_vals[:,1]
}
return traj
def get_state_trans_func(x, u, dt):
pos, θ, vel, ω, m, J, r, μ = x[0:2], x[2], x[3:5], x[5], x[6], x[7], x[
8:10], x[10]
fx, fy, tr = u[0], u[1], u[2]
pos_new = pos + vel * dt
# position of the center of mass.
θ_new = θ + ω * dt
# angular velocity of the center of mass
#θ_new = cs.mod(θ_new, 2*pi)
vel_new = vel - μ / m * vel * dt + 1 / m * cs.vcat([fx, fy]) * dt
ω_new = ω - 1/J*cs.dot(r,cs.vcat([cs.sin(θ),cs.cos(θ)]))*fx*dt + \
1/J*cs.dot(r,cs.vcat([cs.cos(θ),-cs.sin(θ)]))*fy*dt + 1/J*tr*dt
x_new = cs.vcat([pos_new, θ_new, vel_new, ω_new, m, J, r, μ])
f = cs.Function('f', [x, u], [x_new], ['x', 'u'], ['x_new'])
return f
def _grid_intg_fine(self, stage, expr, grid, refine):
"""Evaluate expression at extra fine integrator discretization points."""
if stage._method.poly_coeff is None:
msg = "No polynomal coefficients for the {} integration method".format(
stage._method.intg)
raise Exception(msg)
N, M, T = stage._method.N, stage._method.M, stage._method.T
expr_f = Function('expr', [stage.x, stage.z, stage.u], [expr])
sub_expr = []
time = self.sol.value(stage._method.control_grid)
total_time = []
for k in range(N):
t0 = time[k]
dt = (time[k + 1] - time[k]) / M
tlocal = np.linspace(0, dt, refine + 1)
ts = DM(tlocal[:-1]).T
for l in range(M):
total_time.append(t0 + tlocal[:-1])
coeff = stage._method.poly_coeff[k * M + l]
tpower = vcat([ts**i for i in range(coeff.shape[1])])
if stage._method.poly_coeff_z:
coeff_z = stage._method.poly_coeff_z[k * M + l]
tpower_z = vcat([ts**i for i in range(coeff_z.shape[1])])
z = coeff_z @ tpower_z
else:
z = nan
sub_expr.append(expr_f(coeff @ tpower, z, stage._method.U[k]))
t0 += dt
ts = tlocal[-1]
total_time.append(time[k + 1])
tpower = vcat([ts**i for i in range(coeff.shape[1])])
if stage._method.poly_coeff_z:
tpower_z = vcat([ts**i for i in range(coeff_z.shape[1])])
z = coeff_z @ tpower_z
else:
z = nan
sub_expr.append(
expr_f(stage._method.poly_coeff[-1] @ tpower, z,
stage._method.U[-1]))
res = self.sol.value(hcat(sub_expr))
time = np.hstack(total_time)
return time, res
def p_control(x, pos_gain, rot_gain):
fVec = -pos_gain * x[:2]
torque = -rot_gain * (x[2] - pi)
# Target θ is pi.
u = cs.vcat([fVec, torque])
p_control = cs.Function('p_control', [x], [u], ['x'], ['u'])
return p_control
def fk_rpy_casadi(self, q):
T = self.fk_casadi(q)
s = ca.sqrt(T[0, 0] * T[0, 0] + T[0, 1] * T[0, 1])
r_x = ca.arctan2(-T[1, 2], T[2, 2])
r_y = ca.arctan2(T[0, 2], s)
r_z = ca.arctan2(-T[0, 1], T[2, 2])
return ca.vcat([T[:3, 3], r_x, r_y, r_z])
def exitForEquation(self, tree):
logger.debug('exitForEquation')
f = self.for_loops.pop()
if len(f.values) > 0:
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat([ca.vec(self.get_mx(e)) for e in tree.equations])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body', all_args, [expr])
indexed_symbols_full = []
for k in indexed_symbols:
s = f.indexed_symbols[k]
orig_symbol = self.nodes[self.current_class][s.tree.name]
indexed_symbol = orig_symbol[s.indices]
if s.transpose:
indexed_symbol = ca.transpose(indexed_symbol)
indexed_symbols_full.append(indexed_symbol)
Fmap = F.map("map", self.map_mode, len(f.values),
list(range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
self.src[tree] = res[0].T
else:
self.src[tree] = ca.MX()
def __init__(self,name,syms,expression):
# info
self.name = name
self.sizes = (syms[0].numel(),expression.numel(),sum([s.numel() for s in syms[1:]]))
# use a single parameter for the expression (because julia sucks sometimes)
self.param_sym = oc.MX.sym("P",self.sizes[2])
self.param_names = [s.name() for s in syms[1:]]
expression_ = cs.substitute(expression,cs.vcat(syms[1:]),self.param_sym)
# expression evaluation
self.eval = cs.Function('eval',[syms[0],self.param_sym],[expression_]).expand()
# collect the sx version of the symbols
sx_syms = self.eval.sx_in()
self.main_sym = sx_syms[0]
# expression jacobian
jacobian = cs.jacobian(expression_,syms[0])
self.eval_jac = cs.Function('eval_jac',[syms[0],self.param_sym],[jacobian]).expand()
# hessian of each of the elements of the expression
split_eval = [cs.Function('eval',[syms[0],self.param_sym],[expression_[i]]).expand() for i in range(expression_.shape[0]) ]
hessian = [cs.hessian(split_eval[i](*sx_syms),sx_syms[0])[0] for i in range(expression_.shape[0])]
self.eval_hes = [cs.Function('eval_hes'+str(i),sx_syms,[hessian[i]]).expand() for i in range(expression.shape[0])]
# location of the compiled library
self.lib_path = None
def generate_model(self, configuration):
""" Logic to construct a model, and smooth representation on BSpline basis """
self.n = configuration['grid_size']
self.K = configuration['basis_number']
self.s = configuration['model_form']['state']
n_ps = configuration['model_form']['parameters']
# setup fine time grid
self.ts = ca.MX.sym("t", self.n, 1)
self.observation_times = np.linspace(*configuration['time_span'][:2],
self.n)
# determine knots and build basis functions
if configuration['knot_function'] is None:
knots = casbasis.choose_knots(self.observation_times, self.K - 2)
else:
knots = configuration['knot_function'](self.observation_times,
self.K - 2,
configuration['dataset'])
self.basis_fns = casbasis.basis_functions(knots)
self.basis = ca.vcat([b(self.ts) for b in self.basis_fns]).reshape(
(self.n, self.K))
self.tssx = ca.SX.sym("t", self.n, 1)
# define basis matrix and gradient matrix
phi = ca.Function('phi', [self.ts], [self.basis])
self.phi = np.array(phi(self.observation_times))
bjac = ca.vcat([
ca.diag(ca.jacobian(self.basis[:, i], self.ts))
for i in range(self.K)
]).reshape((self.n, self.K))
self.basis_jacobian = np.array(
ca.Function('bjac', [self.ts], [bjac])(self.observation_times))
# create the objects that define the smooth, model parameters
self.cs = [ca.SX.sym("c_" + str(i), self.K, 1) for i in range(self.s)]
self.xs = [self.phi @ ci for ci in self.cs]
self.xdash = self.basis_jacobian @ ca.hcat(self.cs)
self.ps = [ca.SX.sym("p_" + str(i)) for i in range(n_ps)]
# model function derived from input model function
self.model = ca.Function(
"model", [self.tssx, *self.cs, *self.ps],
[ca.hcat(configuration['model'](self.tssx, self.xs, self.ps))])
def build_solvers(self, config):
for i, (objective,
model) in enumerate(zip(self.objectives, self.models)):
problem = {
'f': objective.objective,
'x': ca.vcat(objective.input_list),
'p': ca.vcat([objective.rho, objective.alpha]),
'g': ca.vcat(model.ps),
}
options = {
'ipopt': {
'print_level': 3,
},
}
self.solvers.append(
ca.nlpsol(f"solver{i}", 'ipopt', problem, options))
opts = self.prep_solver(config, model)
self.solve_opts.append((i, opts))
def create_objective(self, model):
self.obj_1 = sum(
w / len(ov) * ca.sumsqr(self.densities * (ov - cm @ ca.interp1d(
model.observation_times,
om(model.observation_times, model.ps, *
(model.xs[j] for j in oj)), self.observation_times)))
for om, oj, ov, w, cm in zip(
self.observation_model, self.observation_vector,
self.observations, self.weightings, self.collocation_matrices))
self.obj_2 = sum(
ca.sumsqr((model.xdash[:, i] - model.model(
model.observation_times, *model.cs, *model.ps)[:, i]))
for i in range(model.s)) / model.n
self.regularisation = ca.sumsqr(
(ca.vcat(model.ps) - ca.vcat(self.regularisation_vector)) /
(1 + ca.vcat(self.regularisation_vector)))
self.objective = self.obj_1 + self.rho * self.obj_2 + self.alpha * self.regularisation
def _grid_integrator(self, stage, expr, grid):
"""Evaluate expression at (N*M + 1) integrator discretization points."""
sub_expr = []
time = []
for k in range(stage._method.N):
for l in range(stage._method.M):
sub_expr.append(
stage._method.eval_at_integrator(stage, expr, k, l))
time.append(stage._method.integrator_grid[k])
sub_expr.append(stage._method.eval_at_control(stage, expr, -1))
res = [self.sol.value(elem) for elem in sub_expr]
time = self.sol.value(vcat(time))
return time, np.array(res)
def ControlInput(ref_trajectories, opti_vars, k):
"""
Übersetzt durch Maschinenparameter parametrierte
Führungsgrößenverläufe in optimierbare control inputs
"""
control = []
for key in ref_trajectories.keys():
control.append(ref_trajectories[key](opti_vars, k))
control = cs.vcat(control)
return control
def is_signal(self, expr):
"""Does the expression represent a signal (does it depend on time)?
Returns
-------
res : bool
"""
return depends_on(
expr,
vertcat(self.x, self.u, self.t,
vcat(self.variables['control'] +
self.variables['states'])))
def error_velocity(self, z, p):
dpx = z[self.index['x_velocity']]
dpy = z[self.index['y_velocity']]
dpz = z[self.index['z_velocity']]
theta = z[self.index["theta_position"]]
drx = casadi.polyval(p[self.index["pdx"]], theta)
dry = casadi.polyval(p[self.index["pdy"]], theta)
drz = casadi.polyval(p[self.index["pdz"]], theta)
desired_velocity = casadi.polyval(p[self.index["pv"]], theta)
tangent_normalized = self.calculate_tangent(drx, dry, drz)
global_velocity = casadi.vcat([dpx, dpy, dpz])
projected_velocity = casadi.dot(global_velocity, tangent_normalized)
e_velocity = (desired_velocity - projected_velocity)**2
return e_velocity
def ode(chi, T, dt):
T_surge = T[0] + T[1]
M_yaw = (T[0] - T[1]) * self.D_back
u_dot = -(-chi[1] * chi[2]) * m - (Xu * chi[0]) + T_surge
v_dot = -(chi[0] * chi[2]) * m - (Yv * chi[1])
r_dot = -(Nr * chi[2]) + M_yaw
x_dot = ca.cos(chi[5]) * chi[0] - ca.sin(chi[5]) * chi[1]
y_dot = ca.sin(chi[5]) * chi[0] + ca.cos(chi[5]) * chi[1]
psi_dot = chi[2]
chi_dot = ca.vcat([
u_dot * Mu_inv, v_dot * Mv_inv, r_dot * Mr_inv, x_dot, y_dot,
psi_dot
])
chi_1 = chi + dt * chi_dot
return chi_1
def eval_ws_ineq(self, z, p): ws_ineq = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) Lambda = z[ j : j + self.n_ineq[i] ] ws_ineq.append( ca.dot( ca.mtimes(A.T, Lambda), ca.mtimes(A.T, Lambda) ) ) j += self.n_ineq[i] return ca.vcat( ws_ineq )
def solve(self, x0):
xs = ca.vcat([state for state in self.states])
us = ca.vcat([control for control in self.controls])
dyns = ca.vcat([
var[1:] - (var[:-1] + self.state_der[var] * self.dt)
for var in self.states
])
cons = ca.vcat(
[state[0] - x0[i] for i, state in enumerate(self.states)])
obs = ca.vcat([(o - self.objectives[o]) * self.o_scales[o]
for o in self.objectives.keys()])
nlp = {\
'x': ca.vertcat(xs, us, self.dt),
'f': ca.sumsqr(obs),
'g': ca.vertcat(dyns, cons)
}
n_g = nlp['g'].shape[0]
self.lbg = [0] * n_g
self.ubg = [0] * n_g
x00 = ca.vcat([self.initial[state] for state in self.states])
u00 = ca.vcat([self.initial[control] for control in self.controls])
x0 = ca.vertcat(x00, u00, self.initial[self.dt])
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}})
r = S(x0=x0, lbx=self.lbx, ubx=self.ubx, lbg=self.lbg, ubg=self.ubg)
x_opt = r['x']
n_state_vars = len(self.states) * self.n
n_control_vars = len(self.controls) * self.n
states = np.array(x_opt[:n_state_vars])
controls = np.array(x_opt[n_state_vars:n_state_vars + n_control_vars])
times = np.array(x_opt[-self.n + 1:])
for i, state in enumerate(self.states):
self.set_inital(state, states[i * self.n:self.n * (i + 1)])
for i, control in enumerate(self.controls):
self.set_inital(control,
controls[(self.n - 1) * i:(i + 1) * (self.n - 1)])
self.set_inital(self.dt, times)
return states, controls, times
def ControlInput(reference,opti_vars,k):
"""
Übersetzt durch Maschinenparameter parametrierte
Führungsgrößenverläufe in optimierbare control inputs
"""
if reference == []:
return []
control = []
for ref in reference:
control.append(ref(opti_vars,k))
control = cs.vcat(control)
return control
def collocation_coeff(tau):
[C, D] = collocation_interpolators(tau)
d = len(tau)
tau = [0] + tau
F = [None] * (d + 1)
# Construct polynomial basis
for j in range(d + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
F[j] = pint(1.0)
return (hcat(C[1:]), vcat(D), hcat(F[1:]))
def get_link_relative_transform_casadi(self, qi):
""" Link transform according to the Denavit-Hartenberg convention.
Casadi compatible function.
"""
if self.joint_type == JointType.revolute:
a, alpha, d, theta = self.dh.a, self.dh.alpha, self.dh.d, qi
elif self.joint_type == JointType.prismatic:
a, alpha, d, theta = self.dh.a, self.dh.alpha, qi, self.dh.theta
c_t, s_t = ca.cos(theta), ca.sin(theta)
c_a, s_a = ca.cos(alpha), ca.sin(alpha)
row1 = ca.hcat([c_t, -s_t * c_a, s_t * s_a, a * c_t])
row2 = ca.hcat([s_t, c_t * c_a, -c_t * s_a, a * s_t])
row3 = ca.hcat([0, s_a, c_a, d])
row4 = ca.hcat([0, 0, 0, 1])
return ca.vcat([row1, row2, row3, row4])
def generate_cost_function(self):
initial_state = cd.SX.sym('initial_state',
self._controller.model.number_of_states, 1)
state_reference = cd.SX.sym('state_reference',
self._controller.model.number_of_states, 1)
input_reference = cd.SX.sym('input_reference',
self._controller.model.number_of_inputs, 1)
static_casadi_parameters = cd.vertcat(initial_state, state_reference,
input_reference)
constraint_weights = cd.SX.sym('constraint_weights',
self._controller.number_of_constraints,
1)
input_all_steps = cd.SX.sym(
'input_all_steps',
self._controller.model.number_of_inputs * self._controller.horizon,
1)
cost = cd.SX.sym('cost', 1, 1)
cost = 0
current_state = initial_state
for i in range(1, self._controller.horizon + 1):
input = input_all_steps[(i - 1) *
self._controller.model.number_of_inputs:i *
self._controller.model.number_of_inputs]
current_state = self._controller.model.get_next_state(
current_state, input)
cost = cost + self._controller.stage_cost(
current_state, input, i, state_reference, input_reference)
cost = cost + self._controller.generate_cost_constraints(
current_state, input, constraint_weights)
# Function created to bypass the parameters, needs to be adjusted but works for now
static_casadi_parameters = cd.vertcat(
static_casadi_parameters,
cd.vcat([e for e in cd.symvar(cost) if "obstacle_" in e.name()]))
(cost_function, cost_function_derivative_combined) = \
ccg.setup_casadi_functions_and_generate_c(static_casadi_parameters,input_all_steps,constraint_weights,cost,\
self._controller.location)
return (cost_function, cost_function_derivative_combined)
def exitForStatement(self, tree):
logger.debug('exitForStatement')
f = self.for_loops.pop()
if len(f.values) > 0:
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat(
[ca.vec(self.get_mx(e.right)) for e in tree.statements])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body', all_args, [expr])
indexed_symbols_full = []
for k in indexed_symbols:
s = f.indexed_symbols[k]
orig_symbol = self.nodes[self.current_class][s.tree.name]
indexed_symbol = orig_symbol[s.indices]
if s.transpose:
indexed_symbol = ca.transpose(indexed_symbol)
indexed_symbols_full.append(indexed_symbol)
Fmap = F.map("map", self.map_mode, len(f.values),
list(range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
# Split into a list of statements
variables = [
assignment.left for statement in tree.statements
for assignment in self.get_mx(statement)
]
all_assignments = []
for i in range(len(f.values)):
for j, variable in enumerate(variables):
all_assignments.append(Assignment(variable, res[0][j,
i].T))
self.src[tree] = all_assignments
else:
self.src[tree] = []
def add_inf_constraints(self, stage, opti, c, k, l, meta):
coeff = stage._method.poly_coeff[k * self.M + l]
degree = coeff.shape[1]-1
basis = BSplineBasis([0]*(degree+1)+[1]*(degree+1),degree)
tscale = self.T / self.N / self.M
tpower = vcat([tscale**i for i in range(degree+1)])
coeff = coeff * repmat(tpower.T,stage.nx,1)
# TODO: bernstein transformation as function of degree
Poly_to_Bernstein_matrix_4 = DM([[1,0,0,0,0],[1,1.0/4, 0, 0, 0],[1, 1.0/2, 1.0/6, 0, 0],[1, 3.0/4, 1.0/2, 1.0/4, 0],[1, 1, 1, 1, 1]])
state_coeff = Poly_to_Bernstein_matrix_4 @ coeff.T
statesize = [0] + [elem.nnz() for elem in stage.states]
statessizecum = np.cumsum(statesize)
subst_from = stage.states
state_coeff_split = horzsplit(state_coeff,statessizecum)
subst_to = [BSpline(basis,coeff) for coeff in state_coeff_split]
c_spline = reinterpret_expr(c, subst_from, subst_to)
opti.subject_to(self.eval_at_control(stage, c_spline, k), meta=meta)
def solve(self):
xs = ca.vcat([state for state in self.states])
us = ca.vcat([control for control in self.controls])
dyns = ca.vcat([var[1:] - (var[:-1] + self.state_der[var] * self.dt) for var in self.states])
cons = ca.vcat([cons[0] - self.constraints[cons] for cons in self.constraints.keys()])
obs = ca.vcat([(o - self.objectives[o]) * self.o_scales[o] for o in self.objectives.keys()])
nlp = {\
'x': ca.vertcat(xs, us, self.dt),
'f': ca.sumsqr(obs),
'g': ca.vertcat(dyns, cons)
}
n_g = nlp['g'].shape[0]
lbg = [0] * n_g
ubg = [0] * n_g
x00 = ca.vcat([self.initial[state] for state in self.states])
u00 = ca.vcat([self.initial[control] for control in self.controls])
x0 = ca.vertcat(x00, u00, self.initial[self.dt])
x_mins = [self.min_lims[state] for state in self.states] * self.n
x_maxs = [self.max_lims[state] for state in self.states] * self.n
u_mins = [self.min_lims[control] for control in self.controls] * (self.n - 1)
u_maxs = [self.max_lims[control] for control in self.controls] * (self.n - 1)
lbx = x_mins + u_mins + list(np.zeros(self.n-1))
ubx = x_maxs + u_maxs + list(np.ones(self.n-1) * self.max_t)
S = ca.nlpsol('S', 'ipopt', nlp)
r = S(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
x_opt = r['x']
n_state_vars = len(self.states) * self.n
n_control_vars = len(self.controls) * self.n
states = np.array(x_opt[:n_state_vars])
controls = np.array(x_opt[n_state_vars:n_state_vars + n_control_vars])
times = np.array(x_opt[-self.n+1:])
return states, controls, times
def exitForEquation(self, tree):
f = self.for_loops.pop()
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat([ca.vec(self.get_mx(e)) for e in tree.equations])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body_' + f.name, all_args, [expr])
indexed_symbols_full = [
self.nodes[f.indexed_symbols[k].tree.name][
f.indexed_symbols[k].indices - 1] for k in indexed_symbols
]
Fmap = F.map("map", "serial", len(f.values),
list(range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
self.src[tree] = res[0].T
def __init__(self,name,syms,expression,type='auto'):
# info
self.name = name
self.sizes = (syms[0].numel(),expression.numel(),sum([s.numel() for s in syms[1:]]))
self.main_sym = syms[0]
# use a single parameter for the expression (because julia sucks sometimes)
self.param_sym = oc.MX.sym("P",self.sizes[2])
self.param_names = [s.name() for s in syms[1:]]
expression_ = cs.substitute(expression,cs.vcat(syms[1:]),self.param_sym)
# expression evaluation
self.eval = cs.Function('eval',[self.main_sym,self.param_sym],[expression_])
# expression jacobian
jacobian = cs.jacobian(expression_,self.main_sym)
self.jcb_nnz = jacobian.nnz()
self.jcb_sparsity = jacobian.sparsity().get_triplet()
self.eval_jac = cs.Function('eval_jac',[self.main_sym,self.param_sym],[jacobian])
# hessian of each of the elements of the expression
hessian = [cs.hessian(expression_[i],self.main_sym)[0] for i in range(expression_.shape[0])]
self.hes_nnz = [hessian[i].nnz() for i in range(expression_.shape[0])]
self.hes_sparsity = [hessian[i].sparsity().get_triplet() for i in range(expression_.shape[0])]
self.eval_hes = [cs.Function('eval_hes'+str(i),[self.main_sym,self.param_sym],[hessian[i]]) for i in range(expression.shape[0])]
# check type
self.type = 'Linear'
for n in self.hes_nnz:
if n > 0:
self.type = 'Quadratic'
break
if self.type == 'Quadratic' and self.type not in ['auto','Auto','Quadratic','quadratic']:
raise NameError('LinQuadForJulia: the function is declared '+type+' but it results Quadratic.')
if self.type == 'Linear' and self.type not in ['auto','Auto','Linear','linear']:
raise NameError('LinQuadForJulia: the function is declared '+type+' but it results Linear.')
def exitForStatement(self, tree):
logger.debug('exitForStatement')
f = self.for_loops.pop()
if len(f.values) > 0:
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat([ca.vec(self.get_mx(e.right)) for e in tree.statements])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body', all_args, [expr])
indexed_symbols_full = []
for k in indexed_symbols:
s = f.indexed_symbols[k]
orig_symbol = self.nodes[self.current_class][s.tree.name]
indexed_symbol = orig_symbol[s.indices]
if s.transpose:
indexed_symbol = ca.transpose(indexed_symbol)
indexed_symbols_full.append(indexed_symbol)
Fmap = F.map("map", self.map_mode, len(f.values), list(
range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
# Split into a list of statements
variables = [assignment.left for statement in tree.statements for assignment in self.get_mx(statement)]
all_assignments = []
for i in range(len(f.values)):
for j, variable in enumerate(variables):
all_assignments.append(Assignment(variable, res[0][j, i].T))
self.src[tree] = all_assignments
else:
self.src[tree] = []
def get_obs_func(x, u):
pos, θ, vel, ω, m, J, r, μ = x[0:2], x[2], x[3:5], x[5], x[6], x[7], x[
8:10], x[10]
fx, fy, tr = u[0], u[1], u[2]
px_robot = pos[0] + r[0] * cs.cos(θ) - r[1] * cs.sin(θ)
py_robot = pos[1] + r[0] * cs.sin(θ) + r[1] * cs.cos(θ)
vx_robot = vel[0] - ω * (r[0] * cs.sin(θ) + r[1] * cs.cos(θ))
vy_robot = vel[1] + ω * (r[0] * cs.cos(θ) - r[1] * cs.sin(θ))
α_robot = 1/J*tr \
- 1/J*(r[0]*cs.sin(θ) + r[1]*cs.cos(θ))*fx \
+ 1/J*(r[0]*cs.cos(θ) - r[1]*cs.sin(θ))*fy
ax_robot = 1/m*(fx - μ*vel[0]) \
- α_robot*(r[0]*cs.sin(θ) + r[1]*cs.cos(θ)) \
- (ω**2)*(r[0]*cs.cos(θ) - r[1]*cs.sin(θ))
ay_robot = 1/m*(fy - μ*vel[1]) \
+ α_robot*(r[0]*cs.cos(θ) - r[1]*cs.sin(θ)) \
- (ω**2)*(r[0]*cs.sin(θ) + r[1]*cs.cos(θ))
y = cs.vcat([
px_robot, py_robot, θ, vx_robot, vy_robot, ω, ax_robot, ay_robot,
α_robot
])
h = cs.Function('h', [x, u], [y], ['x', 'u'], ['y'])
return h
def exitForEquation(self, tree):
logger.debug('exitForEquation')
f = self.for_loops.pop()
if len(f.values) > 0:
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat([ca.vec(self.get_mx(e)) for e in tree.equations])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body', all_args, [expr])
indexed_symbols_full = []
for k in indexed_symbols:
s = f.indexed_symbols[k]
indices = s.indices
try:
i = self.model.delay_states.index(k.name())
except ValueError:
orig_symbol = self.nodes[self.current_class][s.tree.name]
else:
# We are missing a similarly shaped delayed symbol. Make a new one with the appropriate shape.
delay_symbol = self.model.delay_arguments[i]
# We need to figure out the shape of the expression that
# we are delaying. The symbols that can occur in the delay
# expression should have been encountered before this
# iteration of the loop. The assert statement below covers
# this.
delay_expr_args = free_vars + all_args[:len(indexed_symbols_full)+1]
assert set(ca.symvar(delay_symbol.expr)).issubset(delay_expr_args)
f_delay_expr = ca.Function('delay_expr', delay_expr_args, [delay_symbol.expr])
f_delay_map = f_delay_expr.map("map", self.map_mode, len(f.values), list(
range(len(free_vars))), [])
[res] = f_delay_map.call(free_vars + [f.values] + indexed_symbols_full)
res = res.T
# Make the symbol with the appropriate size, and replace the old symbol with the new one.
orig_symbol = _new_mx(k.name(), *res.size())
assert res.size1() == 1 or res.size2() == 1, "Slicing does not yet work with 2-D indices"
indices = slice(None, None)
model_input = next(x for x in self.model.inputs if x.symbol.name() == k.name())
model_input.symbol = orig_symbol
self.model.delay_arguments[i] = DelayArgument(res, delay_symbol.duration)
indexed_symbol = orig_symbol[indices]
if s.transpose:
indexed_symbol = ca.transpose(indexed_symbol)
indexed_symbols_full.append(indexed_symbol)
Fmap = F.map("map", self.map_mode, len(f.values), list(
range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
self.src[tree] = res[0].T
else:
self.src[tree] = ca.MX()
