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