def state_contraints(self, robot, U): constraintVar = c.MX( 2 * (robot.nx - 1) * self.N, self.T) #skipping orientation. 2* to include min and max U = c.reshape(U, robot.nu * self.N, self.T) X = c.MX(robot.nx * self.N, self.T + 1) X[:, 0] = np.reshape(self.X0, (robot.nx * self.N, )) for i in range(self.T): for n in range(self.N): X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics( X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i], U[robot.nu * n + 1, i], U[robot.nu * n + 2, i]) constraintVar[2 * (robot.nx - 1) * n:2 * (robot.nx - 1) * (n + 1), i] = c.blockcat( [[ self.Xmax - X[robot.nx * n:robot.nx * (n + 1) - 1, i + 1] ], [ X[robot.nx * n:robot.nx * (n + 1) - 1, i + 1] - self.Xmin ]]) constraintVar = c.reshape(constraintVar, 1, 2 * (robot.nx - 1) * self.N * self.T) return constraintVar
def formulateNLP(self, walker): Xk = ca.MX([-0.3, 0.7, 0.0, -0.5, -0.6, 0., 0., 0., 0., 0.]) Xk = ca.reshape(Xk, 10, 1) self.g += [Xk] self.lbg += [-np.pi / 2] * 10 self.ubg += [np.pi / 2] * 10 for k in range(walker.N): if k == walker.N - 1: Xk = ca.MX([-0.6, -0.5, 0.0, 0.7, -0.3, 0., 0., 0., 0., 0.]) Xk = ca.reshape(Xk, 10, 1) self.g += [Xk] self.lbg += [-np.pi / 2] * 10 self.ubg += [np.pi / 2] * 10 # New NLP variable for the control Uk = ca.MX.sym('U_' + str(k), 4, 1) # Uk = ca.MX([0.]) self.w += [Uk] self.lbw += [-walker.tau_max] * 4 self.ubw += [walker.tau_max] * 4 self.w0 += [0.] * 4 # Integrate till the end of the interval Fk = walker.F(x0=Xk, u=Uk) Xk = Fk['xf'] self.J = self.J + Fk['j']
def extract_solution(sol, net, nt, nx): 'Extract NumPy arrays from CasADi NLP solution' _, A, producers, _, _, configs = net() n_ctrls = len(producers) n_confg = len(configs) offset = 0 alpha = np.array( cas.reshape(sol['x'][offset:offset + (nt - 1) * n_confg], nt - 1, n_confg)) offset += (nt - 1) * n_confg u = np.array( cas.reshape(sol['x'][offset:offset + (nt - 1) * n_ctrls], nt - 1, n_ctrls)) offset += (nt - 1) * n_ctrls xi_p, xi_m = [], [] for line in range(len(A)): xi_p += [ np.array(cas.reshape(sol['x'][offset:offset + nt * nx], nx, nt)) ] offset += nt * nx xi_m += [ np.array(cas.reshape(sol['x'][offset:offset + nt * nx], nx, nt)) ] offset += nt * nx return u, alpha, xi_p, xi_m
def boundary_constraints(U_var, T): U_tot = casadi.reshape(U_var, 2, T) X_tot = find_X_tot(U_var, T) c = casadi.MX(2, T) _, X_max = robot_params() for i in range(T): c[:, i] = X_max - X_tot[0:2, i] c = casadi.reshape(c, 2 * T, 1) return c
def setState(self, r, q, pe, fe): self.q = ca.reshape(q, 1, 1) self.r = ca.reshape(r, 2, 1) self.pe = ca.reshape(pe, 2, 1) self.fe = ca.reshape(fe, 2, 1) self.kinematic_constraint = self.kinematic_model(r=self.r, q=self.q, pe=self.pe) self.dynamic_constraints = self.dynamic_model(r=self.r, pe=self.pe, f=self.fe)
def def_opt_problem(obstacles): horizon_status = ca.SX.sym('horizon_status', 5, N) initial_status = ca.SX.sym('initial_status', 3, 1) horizon_ref = ca.SX.sym('horizon_ref', 3, N) obstacle_path = ca.SX.sym('dynamic_obj', 3, obstacles.shape[1]) Q = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) R = np.array([[1.0, 0.0], [0.0, 1.0]]) obj = 0 constraint = [] state_trans_5_to_3 = def_state_trans() f_sl_err_3_3_to_3 = def_func_err() next_state = initial_status last_input = np.zeros((2, 1)) for i in range(N): E = f_sl_err_3_3_to_3(horizon_status[:3, i], horizon_ref[:, i]) dU = horizon_status[3:, i] - last_input # E = horizon_status[:3, i] - horizon_ref[:, i] obj = obj + ca.mtimes([E.T, Q, E]) + ca.mtimes([dU.T, R, dU]) for obs_id in range(int(obstacles.shape[1] / N)): dx = horizon_status[0, i] - obstacle_path[0, i + N * obs_id] dy = horizon_status[1, i] - obstacle_path[1, i + N * obs_id] constraint.append(dx * dx / 0.25 + dy * dy / 0.04) for j in range(3): constraint.append(horizon_status[j, i] - next_state[j]) next_state = state_trans_5_to_3(horizon_status[:, i]) last_input = horizon_status[3:, i] nlp_prob = {} nlp_prob['f'] = obj nlp_prob['x'] = ca.reshape(horizon_status, -1, 1) nlp_prob['p'] = ca.vertcat(ca.reshape(horizon_ref, -1, 1), ca.reshape(initial_status, -1, 1), ca.reshape(obstacle_path, -1, 1)) # nlp_prob['p'] = ca.vertcat(ca.reshape(horizon_ref, -1, 1), ca.reshape(initial_status, -1, 1)) nlp_prob['g'] = ca.vertcat(*constraint) # ipot设置 opts_setting = { 'ipopt.max_iter': 500, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.acceptable_tol': 1e-5, 'ipopt.acceptable_obj_change_tol': 1e-6 } opts_setting = {} return ca.nlpsol('cost_func', 'ipopt', nlp_prob, opts_setting)
def acceleration_limit(U_var, T): U_tot = casadi.reshape(U_var, 2, T) a_max = 0.05 v_max = 0.05 c = casadi.MX(2, T - 1) for i in range(T - 1): c[0, i] = a_max**2 - casadi.mtimes( (U_tot[0, i + 1] - U_tot[0, i]).T, U_tot[0, i + 1] - U_tot[0, i]) c[1, i] = v_max**2 - casadi.mtimes( (U_tot[1, i + 1] - U_tot[1, i]).T, U_tot[1, i + 1] - U_tot[1, i]) # c[:,T] = casadi.DM([[0.1],[0.1]]) c = casadi.reshape(c, 2 * (T - 1), 1) return c
def acceleration_limit(S, S_i, K): c = casadi.MX(1,K) #constraint variable S = casadi.reshape(S,1,K) S_prev = S_i #initial condition i=0; for i in range(K): c[0,i] = casadi.mtimes((S[i]-S_prev).T,(S[i]-S_prev)) - 1.414**2 S_prev = S[i] c = casadi.reshape(c,1,K) return c
def solve(theta_value): global x0 solution = solver(lbx=lbX, ubx=ubX, lbg=lbg, ubg=ubg, p=theta_value, x0=x0) if solver.stats()["return_status"] != "Solve_Succeeded": raise Exception("Solve failed with status {}".format( solver.stats()["return_status"])) x0 = solution["x"] Q_res = ca.reshape(x0[:Q.size1() * Q.size2()], Q.size1(), Q.size2()) H_res = ca.reshape(x0[Q.size1() * Q.size2():], H.size1(), H.size2()) d = {} d["Q_0"] = np.array(Q_left).flatten() for i in range(n_level_nodes): d[f"Q_{i + 1}"] = np.array(ca.horzcat(Q0[i], Q_res[i, :])).flatten() d[f"H_{i + 1}"] = np.array(ca.horzcat(H0[i], H_res[i, :])).flatten() results[theta_value] = d
def U_upper_bound(self, robot): ones = c.DM.ones(robot.nu, self.T) ub = c.blockcat([[robot.vel_max * ones[0, :]], [robot.ang_vel_max * ones[1, :]]]) ub = c.reshape(ub, robot.nu * self.T, 1) return ub
def solve_K(self, robot, X, U): K = c.DM.zeros(robot.nu * robot.nx, self.T + 1) _, A, B = robot.proc_model() for i in range(self.T, 0, -1): At = A(X[:, i], U[:, i - 1]) Bt = B(X[:, i], U[:, i - 1]) #print "At:", At #print "Bt:", Bt if i == self.T: P = self.Wxf else: P = c.mtimes(c.mtimes(At.T, P), At) - c.mtimes( c.mtimes(c.mtimes(At.T, P), Bt), K_mat) + self.Wx K_mat = c.mtimes(c.inv(self.Wu + c.mtimes(c.mtimes(Bt.T, P), Bt)), c.mtimes(c.mtimes(Bt.T, P), At)) K[:, i] = c.reshape(K_mat, robot.nu * robot.nx, 1) return K
def J_qi(T, Q, R, P_f, N_pred, states, controls, rhs, func_type=1): # kriegen die Anzahl der Zeiler("shape" ist ein Tuple) n_states = states.shape[0] # p 22. # Eingangsanzahl n_controls = controls.shape[0] # nonlinear mapping function f(x,u) f = Function('f', [states, controls], [rhs]) # Decision variables (controls) u_pwm U = SX.sym('U', n_controls, N_pred) # parameters (which include the !!!initial and the !!!reference state of # the robot!!!reference input) P = SX.sym('P', n_states + n_states + n_controls) # number of x is always N+1 -> 0..N X = SX.sym('X', n_states, (N_pred + 1)) X[:, 0] = P[0:n_states] obj = 0 # compute predicted states and cost function for k in range(N_pred): st = X[:, k] con = U[:, k] f_value = f(st, con) # print(f_value) st_next = st + (T * f_value) X[:, k + 1] = st_next obj = obj + (st - P[n_states:2 * n_states]).T @ Q @ ( st - P[n_states:2 * n_states]) + ( con - P[2 * n_states:2 * n_states + n_controls]).T @ R @ ( con - P[2 * n_states:2 * n_states + n_controls]) # print(obj) st = X[:, N_pred] obj = obj + (st - P[n_states:2 * n_states]).T @ P_f @ ( st - P[n_states:2 * n_states]) # create a dense matrix of state constraints. Size of g: n_states * (N_pred+1) +1 g = SX.zeros(n_states * (N_pred + 1) + 1, 1) # Constrainsvariable spezifizieren for i in range(N_pred + 1): for j in range(n_states): g[n_states * i + j] = X[j, i] g[n_states * (N_pred + 1)] = (X[:, N_pred] - P[n_states:2 * n_states]).T @ P_f @ ( X[:, N_pred] - P[n_states:2 * n_states]) OPT_variables = reshape(U, N_pred, 1) nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g, 'p': P} opts = {} opts['print_time'] = False opts['ipopt'] = { 'max_iter': 100, 'print_level': 0, 'acceptable_tol': 1e-8, 'acceptable_obj_change_tol': 1e-6 } # print(opts) return f, nlpsol("solver", "ipopt", nlp_prob, opts)
def U_lower_bound(self, robot): ones = c.DM.ones(self.N * self.T) lb = c.blockcat([[-robot.vel_max * ones], [-robot.vel_max * ones], [-robot.ang_vel_max * ones]]) lb = c.reshape(lb, robot.nu * self.N * self.T, 1) return lb
def cost_func_SSP(self, robot, U, X0): cost = 0 U = c.reshape(U, robot.nu, self.T) X = c.MX(robot.nx, self.T + 1) X[:, 0] = X0 for i in range(self.T): cost = (cost + c.mtimes(c.mtimes(U[:, i].T, self.R), U[:, i]) + c.mtimes(c.mtimes((self.Xg - X[:, i]).T, self.Q), (self.Xg - X[:, i]))) X_temp = X[0:2, i] if params.OBSTACLES: obstacle_cost = params.M*(c.exp(-(c.mtimes(c.mtimes((params.c_obs_1 - X_temp).T, params.E_obs_1),(params.c_obs_1 - X_temp))))+ \ c.exp(-(c.mtimes(c.mtimes((params.c_obs_2 - X_temp).T, params.E_obs_2),(params.c_obs_2 - X_temp)))) + \ c.exp(-(c.mtimes(c.mtimes((params.c_obs_3 - X_temp).T, params.E_obs_3),(params.c_obs_3 - X_temp)))) + \ c.exp(-(c.mtimes(c.mtimes((params.c_obs_4 - X_temp).T, params.E_obs_4),(params.c_obs_4 - X_temp))))) cost = cost + obstacle_cost X[:, i + 1] = robot.kinematics(X[:, i], U[:, i]) cost = cost + c.mtimes(c.mtimes((self.Xg - X[:, self.T]).T, self.Qf), (self.Xg - X[:, self.T])) return cost
def solve_BSP(self, robot, X0, cov_X0, Ui, U_guess): #Belief space planning opti = c.Opti() U = opti.variable(robot.nu * self.T * self.N, 1) opti.set_initial(U, U_guess) opti.minimize(self.cost_func_BSP(robot, U, X0, cov_X0)) #control constraints opti.subject_to(U <= self.U_upper_bound(robot)) opti.subject_to(U >= self.U_lower_bound(robot)) #state constraints opti.subject_to(self.state_contraints(robot, U) > 0) opts = {} opts['ipopt.print_level'] = 0 opti.solver('ipopt', opts) sol = opti.solve() U_opti = sol.value(U) U_opti = c.reshape(U_opti, robot.nu * self.N, self.T) return U_opti
def solve_SSP(self, robot, X0, Ui, U_guess): opti = c.Opti() U = opti.variable(robot.nu * self.T, 1) opti.set_initial(U, U_guess) opti.minimize(self.cost_func_SSP(robot, U, X0)) #control constraints opti.subject_to(U <= self.U_upper_bound(robot)) opti.subject_to(U >= self.U_lower_bound(robot)) #state constraints #opti.subject_to(self.state_contraints(robot,U) > 0) p_opts = {} p_opts['ipopt.print_level'] = 0 s_opts = {"max_iter": 100} opti.solver('ipopt', p_opts, s_opts) try: sol = opti.solve() U_opti = sol.value(U) except RuntimeError: U_opti = opti.debug.value(U) print "debug value used." # sol = opti.solve() # U_opti = sol.value(U) U_opti = c.reshape(U_opti, robot.nu, self.T) return U_opti
def test_glibcbug(self): self.message("former glibc error") A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) f_in = daeIn(t=t, x=q, p=p) f_out = daeOut(ode=mul(c.reshape(p,3,3),q)) f=SXFunction("f", f_in,f_out) opts = {} opts["fsens_err_con"] = True opts["steps_per_checkpoint"] = 1000 opts["t0"] = 0 opts["tf"] = te integrator = Integrator("integrator", "cvodes", f, opts) q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend = integrator({'x0':q0, 'p':par})["xf"] qe=integrator.jacobian("p","xf") qe = qe({'x0':q0,'p':par})['jac'] qef=MXFunction("qef", [q0,par],[qe]) qef.setInput(A,0) qef.setInput(B.ravel(),1) qef.evaluate()
def __init__(self, traj, is_var=True): """ Converts an existing trajectory into a symbolic trajectory that can be used to express casadi symbolic terms. Node poses are linearized around their current values. """ self.linearized_traj = traj self.node_poses = [] if is_var: # Optimization variable x: One col of a 6-coefficient twist for every node pose: # (Casadi is col-major) self.xvec = casadi.MX.sym('x', len(traj.node_poses) * 6, 1) self.x = casadi.reshape(self.xvec, 6, len(traj.node_poses)) # Generate symbols for node poses: for i in range(len(traj.node_poses)): self.node_poses.append( casadi_pose.linearizedPose(traj.node_poses[i], self.x[:, i])) else: for i in range(len(traj.node_poses)): self.node_poses.append(casadi_pose.constant( traj.node_poses[i])) # Generate symbols for increments between poses: self.node_increments = [] for i in range(len(traj.node_poses) - 1): self.node_increments.append((self.node_poses[i].inverse() * self.node_poses[i + 1]).asTwist())
def obstacle_cost(U_var, T): U_tot = casadi.reshape(U_var, 2, T) X_tot = find_X_tot(U_var, T) obs = env_setup() obs = casadi.DM(obs) cost = 0 for i in range(T): x = X_tot[0, i] y = X_tot[1, i] x0 = obs[0, 0] y0 = obs[0, 1] r0 = obs[0, 2] x1 = obs[1, 0] y1 = obs[1, 1] r1 = obs[1, 2] x2 = obs[2, 0] y2 = obs[2, 1] r2 = obs[2, 2] cost = cost + r0 * 5000 * casadi.exp(-((x - x0)**2 + (y - y0)**2 - r0**2)) cost = cost + r1 * 5000 * casadi.exp(-((x - x1)**2 + (y - y1)**2 - r1**2)) cost = cost + r2 * 5000 * casadi.exp(-((x - x2)**2 + (y - y2)**2 - r2**2)) return cost
def dm_quicksort_bw(x,I=None): if I is None: I = list(range(x.numel())) else: assert len(I) == x.numel() x = cs.reshape(x,-1,1) if x.numel() <= 1: return [x,I] else: pivot = x[0] i = 0 for j in range(x.numel()-1): if x[j+1] > pivot: I[j+1],I[i+1] = I[i+1], I[j+1] x[j+1],x[i+1] = x[i+1], x[j+1] i += 1 I[0],I[i] = I[i],I[0] x[0],x[i] = x[i],x[0] [x_fp, I_fp] = dm_quicksort_bw(x[:i],I[:i]) [x_sp, I_sp] = dm_quicksort_bw(x[i+1:],I[i+1:]) return [cs.vertcat(x_fp,x[i],x_sp), I_fp + [I[i]] + I_sp]
def test_glibcbug(self): self.message("former glibc error") A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) f_in = daeIn(t=t, x=q, p=p) f_out = daeOut(ode=mul(c.reshape(p,3,3),q)) f=SXFunction(f_in,f_out) f.init() integrator = Integrator("cvodes",f) integrator.setOption("fsens_err_con", True) integrator.setOption("steps_per_checkpoint",1000) integrator.setOption("t0",0) integrator.setOption("tf",te) integrator.init() q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend, = integratorOut(integrator.call(integratorIn(x0=q0,p=par)),"xf") qe=integrator.jacobian("p","xf") qe.init() qe = qe.call(integratorIn(x0=q0,p=par))[0] qef=MXFunction([q0,par],[qe]) qef.init() qef.setInput(A,0) qef.setInput(B.ravel(),1) qef.evaluate()
def reshape(a, newshape): """Gives a new shape to an array without changing its data.""" if not is_casadi_type(a): return _onp.reshape(a, newshape) else: return _cas.reshape(a, newshape)
def test_glibcbug(self): self.message("former glibc error") A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 t=ssym("t") q=ssym("q",3,1) p=ssym("p",9,1) f_in = daeIn(t=t, x=q, p=p) f_out = daeOut(ode=mul(c.reshape(p,3,3),q)) f=SXFunction(f_in,f_out) f.init() integrator = CVodesIntegrator(f) integrator.setOption("fsens_err_con", True) integrator.setOption("steps_per_checkpoint",1000) integrator.setOption("t0",0) integrator.setOption("tf",te) integrator.init() q0 = MX("q0",3,1) par = MX("p",9,1) qend,_,_,_ = integrator.call([q0,par]) qe=integrator.jacobian(INTEGRATOR_P,INTEGRATOR_XF) qe.init() qe = qe.call([q0,par])[0] qef=MXFunction([q0,par],[qe]) qef.init() qef.input(0).set(A) qef.input(1).set(B.ravel()) qef.evaluate()
def randDM(self, n, m=1, sparsity=1, valuegenerator=lambda: random.normal(0, 1), symm=False): if sparsity < 1: spp = self.randDM(n, m, sparsity=1, valuegenerator=lambda: random.uniform(0, 1), symm=symm) spm = (spp < sparsity) spm = sparsify(spm) ret = DM(spm.sparsity(), array([valuegenerator() for i in range(spm.nnz())])) if symm: return (ret + ret.T) / 2 else: return ret else: ret = casadi.reshape(DM([valuegenerator() for i in range(n * m)]), n, m) if symm: return (ret + ret.T) / 2 else: return ret
def asTwist(self): acosarg = (casadi.trace(self.R) - 1) / 2 theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg)) w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]), (self.R[0, 2] - self.R[2, 0]), (self.R[1, 0] - self.R[0, 1])) return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def FIM_t(xpdot, b, criterion): ''' computes FIM at a given time. b is the bonary variable which selects or not the time point ''' n_x = 2 n_theta = 4 FIM_sample = np.zeros([n_theta, n_theta]) FIM_0 = cad.inv((((10.0 - 0.001)**2) / 12) * cad.SX.eye(n_theta)) FIM_sample += FIM_0 for i in range(np.shape(xpdot)[0] - 1): xp_r = cad.reshape(xpdot[i + 1], (n_x, n_theta)) # vv = np.zeros([ntheta[0], ntheta[0], 1 + N]) # for i in range(0, 1 + N): FIM_sample += b[i] * xp_r.T @ np.linalg.inv( np.array([[0.01, 0], [0, 0.05]]) ) @ xp_r # + np.linalg.inv(np.array([[0.01, 0, 0, 0], [0, 0.05, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0.2]])) # FIM = solve(FIM1, SX.eye(FIM1.size1())) # [Q, R] = qr(FIM1.expand()) if criterion == 'D_optimal': # objective = -cad.log(cad.det(FIM_sample) + 1e-10) objective = -2 * cad.sum1(cad.log(cad.diag( cad.chol(FIM_sample)))) # by Cholesky factorisation # objective = -cad.log((cad.det(FIM_sample)**2)) # objective = -cad.det(FIM_sample) elif criterion == 'A_optimal': objective = -cad.log(cad.trace(FIM_sample) + 1e-10) return objective
def T2W(T,p,dp): """ w_101 = T2W(T_10,p,dp) """ R = T2R(T) dR = c.reshape(c.mul(c.jacobian(R,p),dp),(3,3)) return invskew(c.mul(R.T,dR))
def lower_bound(K): steer_angle_max = m.pi/2 ones = casadi.DM.ones(1,K) lb = -steer_angle_max*ones lb = casadi.reshape(lb,K,1) return lb
def upper_bound(K): steer_angle_max = m.pi/2 ones = casadi.DM.ones(1,K) ub = steer_angle_max*ones ub = casadi.reshape(ub,K,1) return ub
def state_contraints(self, robot, U): constraintVar = c.MX( 2 * 2, self.T ) #skipping orientation & steer angle. 2* to include min and max U = c.reshape(U, robot.nu, self.T) X = c.MX(robot.nx, self.T + 1) X[:, 0] = self.X0 for i in range(self.T): X[:, i + 1] = robot.kinematics(X[:, i], U[:, i]) constraintVar[:, i] = c.blockcat([[self.Xmax - X[0:2, i + 1]], [X[0:2, i + 1] - self.Xmin]]) constraintVar = c.reshape(constraintVar, 1, 2 * 2 * self.T) return constraintVar
def T2W(T, p, dp): """ w_101 = T2W(T_10,p,dp) """ R = T2R(T) dR = c.reshape(c.mul(c.jacobian(R, p), dp), (3, 3)) return invskew(c.mul(R.T, dR))
def test_linear_system(self): self.message("Linear ODE") if not(scipy_available): return A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 Be=expm(B*te) t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) f=SXFunction("f", daeIn(t=t,x=q,p=p),daeOut(ode=mul(c.reshape(p,3,3),q))) opts = {} opts["fsens_err_con"] = True opts["reltol"] = 1e-15 opts["abstol"] = 1e-15 #opts["verbose"] = True opts["steps_per_checkpoint"] = 10000 opts["t0"] = 0 opts["tf"] = te integrator = Integrator("integrator", "cvodes", f, opts) q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend = integrator({'x0':q0, 'p':par})["xf"] qe=MXFunction("qe", [q0,par],[qend]) qendJ=integrator.jacobian("x0","xf") qendJ = qendJ({'x0':q0,'p':par})['jac'] qeJ=MXFunction("qeJ", [q0,par],[qendJ]) qendJ2=integrator.jacobian("x0","xf") qendJ2 = qendJ2({'x0':q0,'p':par})['jac'] qeJ2=MXFunction("qeJ2", [q0,par],[qendJ2]) qe.setInput(A,0) qe.setInput(vec(B),1) qe.evaluate() self.checkarray(dot(Be,A)/1e3,qe.getOutput()/1e3,"jacobian('x0','xf')") qeJ.setInput(A,0) qeJ.setInput(vec(B),1) qeJ.evaluate() self.checkarray(qeJ.getOutput()/1e3,Be/1e3,"jacobian('x0','xf')") qeJ2.setInput(A,0) qeJ2.setInput(vec(B),1) qeJ2.evaluate() return # this should return identical zero H=qeJ.jacobian(0,0) #H.setOption("ad_mode","reverse") H.setInput(A,0) H.setInput(vec(B),1) H.evaluate() print array(H.getOutput())
def reduce_matvec(e, v): """ Reduces the MX graph e of linear operations on p into a matrix-vector product. This reduces the number of nodes required to represent the linear operations. """ Af = ca.Function('Af', [ca.MX()], [ca.jacobian(e, v)]) A = Af(ca.DM()) return ca.reshape(ca.mtimes(A, v), e.shape)
def get_stage_cost_func(x, u, p, Cs, Cu, x_target, dt): P = cs.reshape(p, 11, 11) cost = 0.5 * cs.trace(P @ Cs) cost += 0.5 * cs.dot(x[:6] - x_target, Cs[:6, :6] @ (x[:6] - x_target)) cost += 0.5 * cs.dot(u, Cu @ u) cost *= dt c_stage = cs.Function('c_stage', [x, u, p], [cost], ['x', 'u', 'p'], ['cost']) return c_stage
def test_issue298(self): self.message("Issue #298") a = DMatrix(4,1) b = c.reshape(a,2,2) self.assertEqual(type(a),type(b)) a = IMatrix(4,1) b = DMatrix(a) self.assertTrue(isinstance(b,DMatrix)) a = DMatrix(4,1) self.assertRaises(RuntimeError,lambda : IMatrix(a))
def test_issue298(self): self.message("Issue #298") a = DM(4,1) b = c.reshape(a,2,2) self.assertEqual(type(a),type(b)) a = IM(4,1) b = DM(a) self.assertTrue(isinstance(b,DM)) a = DM(4,1) b = IM(a) self.assertTrue(isinstance(b,IM))
def randDM(self,n,m=1,sparsity=1,valuegenerator=lambda : random.normal(0,1),symm=False ): if sparsity < 1: spp = self.randDM(n,m,sparsity=1,valuegenerator=lambda : random.uniform(0,1) ,symm=symm) spm = (spp < sparsity) spm = sparsify(spm) ret = DM(spm.sparsity(),array([valuegenerator() for i in range(spm.nnz())])) if symm: return (ret + ret.T)/2 else: return ret else: ret = casadi.reshape(DM([valuegenerator() for i in range(n*m)]),n,m) if symm: return (ret + ret.T)/2 else: return ret
def test_linear_system(self): self.message("Linear ODE") if not(scipy_available): return A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 Be=expm(B*te) t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) dae = {'t':t, 'x':q, 'p':p, 'ode':mtimes(c.reshape(p,3,3),q)} opts = {} opts["fsens_err_con"] = True opts["reltol"] = 1e-15 opts["abstol"] = 1e-15 #opts["verbose"] = True opts["steps_per_checkpoint"] = 10000 opts["t0"] = 0 opts["tf"] = te integrator = casadi.integrator("integrator", "cvodes", dae, opts) q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend = integrator(x0=q0, p=par)["xf"] qe=Function("qe", [q0,par],[qend]) qendJ=integrator.jacobian("x0","xf") qendJ = qendJ(x0=q0,p=par)['dxf_dx0'] qeJ=Function("qeJ", [q0,par],[qendJ]) qendJ2=integrator.jacobian("x0","xf") qendJ2 = qendJ2(x0=q0,p=par)['dxf_dx0'] qeJ2=Function("qeJ2", [q0,par],[qendJ2]) qe_out = qe(A, vec(B)) self.checkarray(numpy.dot(Be,A)/1e3,qe_out/1e3,"jacobian('x0','xf')") qeJ_out = qeJ(A, vec(B)) self.checkarray(qeJ_out/1e3,Be/1e3,"jacobian('x0','xf')") qeJ2_out = qeJ2(A, vec(B)) return # this should return identical zero H=qeJ.jacobian(0,0) H_out = H(A, vec(B)) print array(H_out[0])
def casadi_vec2struct(s,vec): try: vec.sparsity() except: vec = C.DM(vec) if isinstance(s,OrderedDict): out = OrderedDict() sizes = [0] for f in s.keys(): n = casadi_struct2vec(s[f]).shape[0] sizes.append(sizes[-1]+n) comps = C.vertsplit(vec,sizes) for i,f in enumerate(s.keys()): out[f] = casadi_vec2struct(s[f],comps[i]) return out else: return C.reshape(vec,*s.shape)
def test_glibcbug(self): self.message("former glibc error") A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) dae = {'t':t, 'x':q, 'p':p, 'ode':mtimes(c.reshape(p,3,3),q)} opts = {} opts["fsens_err_con"] = True opts["steps_per_checkpoint"] = 1000 opts["t0"] = 0 opts["tf"] = te integrator = casadi.integrator("integrator", "cvodes", dae, opts) q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend = integrator(x0=q0, p=par)["xf"] qe=integrator.jacobian("p","xf") qe = qe(x0=q0,p=par)['dxf_dp'] qef=Function("qef", [q0,par],[qe]) qef_out = qef(A, B.ravel())
def test_bug_structSXMX(self): n = 2 x_sx = struct_symSX([entry("x", shape=n), entry("S", shape=(n, n))]) x_mx = struct_symSX([entry("x", shape=n), entry("S", shape=(n, n))]) X_sx = struct_SX(x_sx) X_sx["x"] = DMatrix(range(n)) X_sx["S"] = c.reshape(DMatrix(range(n, n + n * n)), n, n) X_mx = struct_MX(x_sx) X_mx["x"] = DMatrix(range(n)) X_mx["S"] = c.reshape(DMatrix(range(n, n + n * n)), n, n) self.checkarray(x_sx.struct.map[("S",)], c.reshape(DMatrix(range(n, n + n * n)), n, n)) self.checkarray(x_mx.struct.map[("S",)], c.reshape(DMatrix(range(n, n + n * n)), n, n)) self.checkarray(X_sx.cat, DMatrix(range(n + n * n))) self.checkarray(X_mx.cat, DMatrix(range(n + n * n))) for s, S in [(x_sx, struct_symSX), (x_mx, struct_symMX)]: h = S([entry("w", struct=s)]) hX = struct_SX(h) hX["w", "x"] = DMatrix(range(n)) hX["w", "S"] = c.reshape(DMatrix(range(n, n + n * n)), n, n) self.checkarray(h.struct.map[("w", "S")], c.reshape(DMatrix(range(n, n + n * n)), n, n)) self.checkarray(hX.cat, DMatrix(range(n + n * n))) self.checkarray(h.struct.map[("w",)], DMatrix(range(n + n * n))) self.checkarray(hX["w"], DMatrix(range(n + n * n))) hX["w"] = DMatrix(range(n + n * n)) self.checkarray(hX.cat, DMatrix(range(n + n * n))) n = 2 m = 3 x_sx = struct_symSX([entry("x", shape=n), entry("S", shape=(n, m))]) x_mx = struct_symSX([entry("x", shape=n), entry("S", shape=(n, m))]) X_sx = struct_SX(x_sx) X_sx["x"] = DMatrix(range(n)) X_sx["S"] = c.reshape(DMatrix(range(n, n + n * m)), n, m) X_mx = struct_MX(x_sx) X_mx["x"] = DMatrix(range(n)) X_mx["S"] = c.reshape(DMatrix(range(n, n + n * m)), n, m) self.checkarray(x_sx.struct.map[("S",)], c.reshape(DMatrix(range(n, n + n * m)), n, m)) self.checkarray(x_mx.struct.map[("S",)], c.reshape(DMatrix(range(n, n + n * m)), n, m)) self.checkarray(X_sx.cat, DMatrix(range(n + n * m))) self.checkarray(X_mx.cat, DMatrix(range(n + n * m))) n = 3 x_sx = struct_symSX([entry("x", shape=n), entry("S", shape=Sparsity.upper(n))]) x_mx = struct_symSX([entry("x", shape=n), entry("S", shape=Sparsity.upper(n))]) X_sx = struct_SX(x_sx) X_sx["x"] = DMatrix(range(n)) X_sx["S"] = DMatrix(Sparsity.upper(n), range(n, n + n * (n + 1) / 2)) X_mx = struct_MX(x_sx) X_mx["x"] = DMatrix(range(n)) X_mx["S"] = DMatrix(Sparsity.upper(n), range(n, n + n * (n + 1) / 2)) self.checkarray(x_sx.struct.map[("S",)], DMatrix(Sparsity.upper(n), range(n, n + n * (n + 1) / 2))) self.checkarray(x_mx.struct.map[("S",)], DMatrix(Sparsity.upper(n), range(n, n + n * (n + 1) / 2))) self.checkarray(X_sx.cat, DMatrix(range(n + n * (n + 1) / 2))) self.checkarray(X_mx.cat, DMatrix(range(n + n * (n + 1) / 2)))
def test_MX(self): x = MX.sym("x",2) y = MX.sym("y",2,2) f1 = MXFunction([x,y],[x+y[0,0],mul(y,x)]) f1.init() f2 = MXFunction([x,y],[mul(MX.zeros(0,2),x)]) f2.init() f3 = MXFunction([x,y],[MX.zeros(0,0),mul(y,x)]) f3.init() f4 = MXFunction([x,y],[MX.zeros(0,2),mul(y,x)]) f4.init() ndir = 2 in1 = [x,y] v1 = [DMatrix([1.1,1.3]),DMatrix([[0.7,1.5],[2.1,0.9]])] w=x[:] w[1]*=2 w2=x[:] w2[1]*=x[0] ww=x[:] ww[[0,1]]*=x wwf=x[:] wwf[[1,0]]*=x wwr=x[:] wwr[[0,0,1,1]]*=2 yy=y[:,:] yy[:,0] = x yy2=y[:,:] yy2[:,0] = x**2 yyy=y[:,:] yyy[[1,0],0] = x yyy2=y[:,:] yyy2[[1,0],0] = x**2 def remove_first(x): ret = DMatrix(x) if ret.numel()>0: ret[0,0] = DMatrix.sparse(1,1) return ret else: return ret def remove_last(x): ret = DMatrix(x) if ret.size()>0: ret[ret.sparsity().row()[-1],ret.sparsity().getCol()[-1]] = DMatrix.sparse(1,1) return ret else: return x spmods = [lambda x: x , remove_first, remove_last] # TODO: sparse seeding for inputs,values,out, jac in [ (in1,v1,x,DMatrix.eye(2)), (in1,v1,x.T,DMatrix.eye(2)), (in1,v1,x**2,2*c.diag(x)), (in1,v1,(x**2).attachAssert(True),2*c.diag(x)), (in1,v1,(x**2).T,2*c.diag(x)), (in1,v1,c.reshape(x,(1,2)),DMatrix.eye(2)), (in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)), (in1,v1,x+y.nz[0],DMatrix.eye(2)), (in1,v1,x+y[0,0],DMatrix.eye(2)), (in1,v1,x+x,2*DMatrix.eye(2)), (in1,v1,x**2+x,2*c.diag(x)+DMatrix.eye(2)), (in1,v1,x*x,2*c.diag(x)), (in1,v1,x*y.nz[0],DMatrix.eye(2)*y.nz[0]), (in1,v1,x*y[0,0],DMatrix.eye(2)*y[0,0]), (in1,v1,x[0],DMatrix.eye(2)[0,:]), (in1,v1,(x**2)[0],horzcat([2*x[0],MX.sparse(1,1)])), (in1,v1,x[0]+x[1],DMatrix.ones(1,2)), (in1,v1,vertcat([x[1],x[0]]),sparse(DMatrix([[0,1],[1,0]]))), (in1,v1,vertsplit(x,[0,1,2])[1],sparse(DMatrix([[0,1]]))), (in1,v1,vertcat([x[1]**2,x[0]**2]),blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])), (in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX.sparse(1,1),2*x[1]]])), (in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX.sparse(1,1),6*x[1]**5]])), (in1,v1,horzcat([x[1],x[0]]).T,sparse(DMatrix([[0,1],[1,0]]))), (in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])), (in1,v1,diagcat([x[1]**2,y,x[0]**2]), blockcat( [[MX.sparse(1,1),2*x[1]]] + ([[MX.sparse(1,1),MX.sparse(1,1)]]*14) + [[2*x[0],MX.sparse(1,1)]] ) ), (in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])), (in1,v1,x[[0,1]],sparse(DMatrix([[1,0],[0,1]]))), (in1,v1,(x**2)[[0,1]],2*c.diag(x)), (in1,v1,x[[0,0,1,1]],sparse(DMatrix([[1,0],[1,0],[0,1],[0,1]]))), (in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX.sparse(1,1)],[2*x[0],MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]],[MX.sparse(1,1),2*x[1]]])), (in1,v1,wwr,sparse(DMatrix([[2,0],[0,2]]))), (in1,v1,x[[1,0]],sparse(DMatrix([[0,1],[1,0]]))), (in1,v1,x[[1,0],0],sparse(DMatrix([[0,1],[1,0]]))), (in1,v1,w,sparse(DMatrix([[1,0],[0,2]]))), (in1,v1,w2,blockcat([[1,MX.sparse(1,1)],[x[1],x[0]]])), (in1,v1,ww,2*c.diag(x)), (in1,v1,wwf,vertcat([x[[1,0]].T,x[[1,0]].T])), (in1,v1,yy[:,0],DMatrix.eye(2)), (in1,v1,yy2[:,0],2*c.diag(x)), (in1,v1,yyy[:,0],sparse(DMatrix([[0,1],[1,0]]))), (in1,v1,mul(y,x),y), (in1,v1,mul(x.T,y.T),y), (in1,v1,mul(y,x,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]), (in1,v1,mul(x.T,y.T,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]), (in1,v1,mul(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]), (in1,v1,mul(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]), (in1,v1,mul(y,x**2),y*2*vertcat([x.T,x.T])), (in1,v1,sin(x),c.diag(cos(x))), (in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)), (in1,v1,x*y[:,0],c.diag(y[:,0])), (in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])), (in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])), (in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])), (in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])), (in1,v1,inner_prod(x,x),(2*x).T), (in1,v1,inner_prod(x**2,x),(3*x**2).T), #(in1,v1,c.det(horzcat([x,DMatrix([1,2])])),DMatrix([-1,2])), not implemented (in1,v1,f1.call(in1)[1],y), (in1,v1,f1.call([x**2,y])[1],y*2*vertcat([x.T,x.T])), (in1,v1,f2.call(in1)[0],DMatrix.zeros(0,2)), (in1,v1,f2.call([x**2,y])[0],DMatrix.zeros(0,2)), (in1,v1,f3.call(in1)[0],DMatrix.zeros(0,2)), (in1,v1,f3.call([x**2,y])[0],DMatrix.zeros(0,2)), (in1,v1,f4.call(in1)[0],DMatrix.zeros(0,2)), (in1,v1,f4.call([x**2,y])[0],DMatrix.zeros(0,2)), #(in1,v1,f1.call([x**2,[]])[1],DMatrix.zeros(2,2)), #(in1,v1,f1.call([[],y])[1],DMatrix.zeros(2,2)), (in1,v1,vertcat([x,DMatrix.sparse(0,1)]),DMatrix.eye(2)), (in1,v1,(x**2).setSparse(sparse(DMatrix([0,1])).sparsity()),blockcat([[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]]])), (in1,v1,c.inner_prod(x,y[:,0]),y[:,0].T), (in1,v1,x.nz[IMatrix([[1,0]])]*y.nz[IMatrix([[0,2]])],blockcat([[MX.sparse(1,1),y.nz[0]],[y.nz[2],MX.sparse(1,1)]])), (in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX.sparse(1,1),y.nz[0]],[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),MX.sparse(1,1)],[y.nz[2],MX.sparse(1,1)]])), ]: print out fun = MXFunction(inputs,[out,jac]) fun.init() funsx = fun.expand() funsx.init() for i,v in enumerate(values): fun.setInput(v,i) funsx.setInput(v,i) fun.evaluate() funsx.evaluate() self.checkarray(fun.getOutput(0),funsx.getOutput(0)) self.checkarray(fun.getOutput(1),funsx.getOutput(1)) J_ = fun.getOutput(1) def vec(l): ret = [] for i in l: ret.extend(i) return ret storage2 = {} storage = {} vf_mx = None for f in [fun,fun.expand()]: f.init() d = f.derivative(ndir,ndir) d.init() num_in = f.getNumInputs() num_out = f.getNumOutputs() """# Fwd and Adjoint AD for i,v in enumerate(values): f.setInput(v,i) d.setInput(v,i) for d in range(ndir): f.setInput(DMatrix(inputs[0].sparsity(),random.random(inputs[0].size())),num_in+d*num_in + d) f.setAdjSeed(DMatrix(out.sparsity(),random.random(out.size())),num_in+d*num_in + 0) f.setFwdSeed(0,1,d) f.setAdjSeed(0,1,d) f.evaluate() for d in range(ndir): seed = array(f.getFwdSeed(0,d)).ravel() sens = array(f.getFwdSens(0,d)).ravel() self.checkarray(sens,mul(J_,seed),"Fwd %d %s" % (d,str(type(f)))) seed = array(f.getAdjSeed(0,d)).ravel() sens = array(f.getAdjSens(0,d)).ravel() self.checkarray(sens,mul(J_.T,seed),"Adj %d" %d) """ # evalThings for sym, Function in [(MX.sym,MXFunction),(SX.sym,SXFunction)]: if isinstance(f, MXFunction) and Function is SXFunction: continue if isinstance(f, SXFunction) and Function is MXFunction: continue # dense for spmod,spmod2 in itertools.product(spmods,repeat=2): fseeds = [[sym("f",spmod(f.getInput(i)).sparsity()) for i in range(f.getNumInputs())] for d in range(ndir)] aseeds = [[sym("a",spmod2(f.getOutput(i)).sparsity()) for i in range(f.getNumOutputs())] for d in range(ndir)] inputss = [sym("i",f.input(i).sparsity()) for i in range(f.getNumInputs())] with internalAPI(): res,fwdsens,adjsens = f.callDerivative(inputss,fseeds,aseeds,True) fseed = [DMatrix(fseeds[d][0].sparsity(),random.random(fseeds[d][0].size())) for d in range(ndir) ] aseed = [DMatrix(aseeds[d][0].sparsity(),random.random(aseeds[d][0].size())) for d in range(ndir) ] vf = Function(inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)])) vf.init() for i,v in enumerate(values): vf.setInput(v,i) offset = len(inputss) for d in range(ndir): vf.setInput(fseed[d],offset+0) for i in range(len(values)-1): vf.setInput(0,offset+i+1) offset += len(inputss) vf.setInput(aseed[d],offset+0) vf.setInput(0,offset+1) offset+=2 assert(offset==vf.getNumInputs()) vf.evaluate() offset = len(res) for d in range(ndir): seed = array(fseed[d]).ravel() sens = array(vf.getOutput(offset+0)).ravel() offset+=len(inputss) self.checkarray(sens,mul(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym))) seed = array(aseed[d]).ravel() sens = array(vf.getOutput(offset+0)).ravel() offset+=len(inputss) self.checkarray(sens,mul(J_.T,seed),"eval Adj %d %s" % (d,str([vf.getOutput(i) for i in range(vf.getNumOutputs())]))) assert(offset==vf.getNumOutputs()) # Complete random seeding random.seed(1) for i in range(vf.getNumInputs()): vf.setInput(DMatrix(vf.input(i).sparsity(),random.random(vf.input(i).size())),i) vf.evaluate() storagekey = (spmod,spmod2) if not(storagekey in storage): storage[storagekey] = [] storage[storagekey].append([vf.getOutput(i) for i in range(vf.getNumOutputs())]) # Added to make sure that the same seeds are used for SX and MX if Function is MXFunction: vf_mx = vf # Second order sensitivities for sym2, Function2 in [(MX.sym,MXFunction),(SX.sym,SXFunction)]: if isinstance(vf, MXFunction) and Function2 is SXFunction: continue if isinstance(vf, SXFunction) and Function2 is MXFunction: continue for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2): fseeds2 = [[sym2("f",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())] for d in range(ndir)] aseeds2 = [[sym2("a",vf_mx.output(i).sparsity()) for i in range(vf.getNumOutputs()) ] for d in range(ndir)] inputss2 = [sym2("i",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())] with internalAPI(): res2,fwdsens2,adjsens2 = vf.callDerivative(inputss2,fseeds2,aseeds2,True) vf2 = Function2(inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)])) vf2.init() random.seed(1) for i in range(vf2.getNumInputs()): vf2.setInput(DMatrix(vf2.input(i).sparsity(),random.random(vf2.input(i).size())),i) vf2.evaluate() storagekey = (spmod,spmod2) if not(storagekey in storage2): storage2[storagekey] = [] storage2[storagekey].append([vf2.getOutput(i) for i in range(vf2.getNumOutputs())]) # Remainder of eval testing for store,order in [(storage,"first-order"),(storage2,"second-order")]: for stk,st in store.items(): for i in range(len(st)-1): for k,(a,b) in enumerate(zip(st[0],st[i+1])): if b.numel()==0 and sparse(a).size()==0: continue if a.numel()==0 and sparse(b).size()==0: continue self.checkarray(sparse(a),sparse(b),("%s, output(%d)" % (order,k))+str(vf2.getInput(0))) for f in [fun.expand(),fun]: # jacobian() for mode in ["forward","reverse"]: f.setOption("ad_mode",mode) f.init() Jf=f.jacobian(0,0) Jf.init() for i,v in enumerate(values): Jf.setInput(v,i) Jf.evaluate() self.checkarray(Jf.getOutput(),J_) self.checkarray(DMatrix(Jf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(out)+str(mode)) self.checkarray(DMatrix(f.jacSparsity(),1),DMatrix(J_.sparsity(),1)) # Scalarized if out.isEmpty(): continue s_i = out.sparsity().row()[0] s_j = out.sparsity().getCol()[0] s_k = s_i*out.size2()+s_j fun = MXFunction(inputs,[out[s_i,s_j],jac[s_k,:].T]) fun.init() for i,v in enumerate(values): fun.setInput(v,i) fun.evaluate() J_ = fun.getOutput(1) for f in [fun,fun.expand()]: # gradient() for mode in ["forward","reverse"]: f.setOption("ad_mode",mode) f.init() Gf=f.gradient(0,0) Gf.init() for i,v in enumerate(values): Gf.setInput(v,i) Gf.evaluate() self.checkarray(Gf.getOutput(),J_,failmessage=("mode: %s" % mode)) #self.checkarray(DMatrix(Gf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(mode)+str(out)+str(type(fun))) H_ = None for f in [fun,fun.expand()]: # hessian() for mode in ["forward","reverse"]: f.setOption("ad_mode",mode) f.init() Hf=f.hessian(0,0) Hf.init() for i,v in enumerate(values): Hf.setInput(v,i) Hf.evaluate() if H_ is None: H_ = Hf.getOutput() self.checkarray(Hf.getOutput(),H_,failmessage=("mode: %s" % mode))
def toSX(a): return casadi.reshape(SXMatrix(a),a.shape[0],a.shape[1])
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 setUp(self): x=SX.sym("x") y=SX.sym("y") z=SX.sym("z") w=SX.sym("w") out=SX.sparse(6,1) out[0,0]=x out[2,0]=x+2*y**2 out[4,0]=x+2*y**3+3*z**4 out[5,0]=w inp=SX.sparse(6,1) inp[0,0]=x inp[2,0]=y inp[4,0]=z inp[5,0]=w sp = Sparsity(1,6,[0, 1, 1, 2, 2, 3, 4],[0, 0, 0, 0]).T spT = Sparsity(6,1,[0, 4],[0, 2, 4, 5]).T self.sxinputs = { "column" : { "dense": [vertcat([x,y,z,w])], "sparse": [inp] } , "row": { "dense": [vertcat([x,y,z,w]).T], "sparse": [inp.T] }, "matrix": { "dense": [c.reshape(vertcat([x,y,z,w]),2,2)], "sparse": [c.reshape(inp,3,2)] } } self.mxinputs = { "column" : { "dense": [MX.sym("xyzw",4,1)], "sparse": [MX.sym("xyzw",sp)] }, "row" : { "dense": [MX.sym("xyzw",1,4)], "sparse": [MX.sym("xyzw",spT)] }, "matrix": { "dense": [MX.sym("xyzw",2,2)], "sparse": [MX.sym("xyzw",c.reshape(inp,3,2).sparsity())] } } def temp1(xyz): X=MX.sparse(6,1) X[0,0]=xyz.nz[0] X[2,0]=xyz.nz[0]+2*xyz.nz[1]**2 X[4,0]=xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4 X[5,0]=xyz.nz[3] return [X] def temp2(xyz): X=MX.sparse(1,6) X[0,0]=xyz.nz[0] X[0,2]=xyz.nz[0]+2*xyz.nz[1]**2 X[0,4]=xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4 X[0,5]=xyz.nz[3] return [X] def testje(xyz): print vertcat([xyz.nz[0],xyz.nz[0]+2*xyz.nz[1]**2,xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4,xyz.nz[3]]).shape self.mxoutputs = { "column": { "dense": lambda xyz: [vertcat([xyz.nz[0],xyz.nz[0]+2*xyz.nz[1]**2,xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4,xyz.nz[3]])], "sparse": temp1 }, "row": { "dense": lambda xyz: [horzcat([xyz.nz[0],xyz.nz[0]+2*xyz.nz[1]**2,xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4,xyz.nz[3]])], "sparse": temp2 }, "matrix": { "dense": lambda xyz: [c.reshape(vertcat([xyz.nz[0],xyz.nz[0]+2*xyz.nz[1]**2,xyz.nz[0]+2*xyz.nz[1]**3+3*xyz.nz[2]**4,xyz.nz[3]]),(2,2))], "sparse": lambda xyz: [c.reshape(temp1(xyz)[0],(3,2))] } } self.sxoutputs = { "column": { "dense": [vertcat([x,x+2*y**2,x+2*y**3+3*z**4,w])], "sparse": [out] }, "row": { "dense": [vertcat([x,x+2*y**2,x+2*y**3+3*z**4,w]).T], "sparse": [out.T] }, "matrix" : { "dense": [c.reshape(vertcat([x,x+2*y**2,x+2*y**3+3*z**4,w]),2,2)], "sparse": [c.reshape(out,3,2)] } } self.jacobians = { "dense" : { "dense" : lambda x,y,z,w: array([[1,0,0,0],[1,4*y,0,0],[1,6*y**2,12*z**3,0],[0,0,0,1]]), "sparse" : lambda x,y,z,w: array([[1,0,0,0],[0,0,0,0],[1,4*y,0,0],[0,0,0,0],[1,6*y**2,12*z**3,0],[0,0,0,1]]) } , "sparse" : { "dense" : lambda x,y,z,w: array([[1,0,0,0,0,0],[1,0,4*y,0,0,0],[1,0,6*y**2,0,12*z**3,0],[0,0,0,0,0,1]]), "sparse" : lambda x,y,z,w: array([[1,0,0,0,0,0],[0,0,0,0,0,0],[1,0,4*y,0,0,0],[0,0,0,0,0,0],[1,0,6*y**2,0,12*z**3,0],[0,0,0,0,0,1]]) } }
def test_MX(self): x = MX.sym("x",2) y = MX.sym("y",2,2) f1 = Function("f1", [x,y],[x+y[0,0],mtimes(y,x)]) f2 = Function("f2", [x,y],[mtimes(MX.zeros(0,2),x)]) f3 = Function("f3", [x,y],[MX.zeros(0,0),mtimes(y,x)]) f4 = Function("f4", [x,y],[MX.zeros(0,2),mtimes(y,x)]) ndir = 2 in1 = [x,y] v1 = [DM([1.1,1.3]),DM([[0.7,1.5],[2.1,0.9]])] w=x[:] w[1]*=2 w2=x[:] w2[1]*=x[0] ww=x[:] ww[[0,1]]*=x wwf=x[:] wwf[[1,0]]*=x wwr=x[:] wwr[[0,0,1,1]]*=2 yy=y[:,:] yy[:,0] = x yy2=y[:,:] yy2[:,0] = x**2 yyy=y[:,:] yyy[[1,0],0] = x yyy2=y[:,:] yyy2[[1,0],0] = x**2 def remove_first(x): ret = DM(x) if ret.numel()>0: ret[0,0] = DM(1,1) return ret.sparsity() else: return ret.sparsity() def remove_last(x): ret = DM(x) if ret.nnz()>0: ret[ret.sparsity().row()[-1],ret.sparsity().get_col()[-1]] = DM(1,1) return ret.sparsity() else: return x spmods = [lambda x: x , remove_first, remove_last] # TODO: sparse seeding for inputs,values,out, jac in [ (in1,v1,x,DM.eye(2)), (in1,v1,x.T,DM.eye(2)), (in1,v1,x**2,2*c.diag(x)), (in1,v1,(x**2).attachAssert(True),2*c.diag(x)), (in1,v1,(x**2).T,2*c.diag(x)), (in1,v1,c.reshape(x,(1,2)),DM.eye(2)), (in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)), (in1,v1,x+y.nz[0],DM.eye(2)), (in1,v1,x+y[0,0],DM.eye(2)), (in1,v1,x+x,2*DM.eye(2)), (in1,v1,x**2+x,2*c.diag(x)+DM.eye(2)), (in1,v1,x*x,2*c.diag(x)), (in1,v1,x*y.nz[0],DM.eye(2)*y.nz[0]), (in1,v1,x*y[0,0],DM.eye(2)*y[0,0]), (in1,v1,x[0],DM.eye(2)[0,:]), (in1,v1,(x**2)[0],horzcat(*[2*x[0],MX(1,1)])), (in1,v1,x[0]+x[1],DM.ones(1,2)), (in1,v1,sin(repmat(x**2,1,3)),repmat(cos(c.diag(x**2))*2*c.diag(x),3,1)), (in1,v1,sin(repsum((x**2).T,1,2)),cos(x[0]**2+x[1]**2)*2*x.T), (in1,v1,vertcat(*[x[1],x[0]]),sparsify(DM([[0,1],[1,0]]))), (in1,v1,vertsplit(x,[0,1,2])[1],sparsify(DM([[0,1]]))), (in1,v1,vertcat(*[x[1]**2,x[0]**2]),blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])), (in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX(1,1),2*x[1]]])), (in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX(1,1),6*x[1]**5]])), (in1,v1,horzcat(*[x[1],x[0]]).T,sparsify(DM([[0,1],[1,0]]))), (in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])), (in1,v1,diagcat(*[x[1]**2,y,x[0]**2]), blockcat( [[MX(1,1),2*x[1]]] + ([[MX(1,1),MX(1,1)]]*14) + [[2*x[0],MX(1,1)]] ) ), (in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])), (in1,v1,x[[0,1]],sparsify(DM([[1,0],[0,1]]))), (in1,v1,(x**2)[[0,1]],2*c.diag(x)), (in1,v1,x[[0,0,1,1]],sparsify(DM([[1,0],[1,0],[0,1],[0,1]]))), (in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX(1,1)],[2*x[0],MX(1,1)],[MX(1,1),2*x[1]],[MX(1,1),2*x[1]]])), (in1,v1,wwr,sparsify(DM([[2,0],[0,2]]))), (in1,v1,x[[1,0]],sparsify(DM([[0,1],[1,0]]))), (in1,v1,x[[1,0],0],sparsify(DM([[0,1],[1,0]]))), (in1,v1,w,sparsify(DM([[1,0],[0,2]]))), (in1,v1,w2,blockcat([[1,MX(1,1)],[x[1],x[0]]])), (in1,v1,ww,2*c.diag(x)), (in1,v1,wwf,vertcat(*[x[[1,0]].T,x[[1,0]].T])), (in1,v1,yy[:,0],DM.eye(2)), (in1,v1,yy2[:,0],2*c.diag(x)), (in1,v1,yyy[:,0],sparsify(DM([[0,1],[1,0]]))), (in1,v1,mtimes(y,x),y), (in1,v1,mtimes(x.T,y.T),y), (in1,v1,mac(y,x,DM.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]), (in1,v1,mac(x.T,y.T,DM.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]), (in1,v1,mtimes(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]), (in1,v1,mtimes(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]), (in1,v1,mtimes(y,x**2),y*2*vertcat(*[x.T,x.T])), (in1,v1,sin(x),c.diag(cos(x))), (in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)), (in1,v1,x*y[:,0],c.diag(y[:,0])), (in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])), (in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])), (in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])), (in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])), (in1,v1,c.dot(x,x),(2*x).T), (in1,v1,c.dot(x**2,x),(3*x**2).T), #(in1,v1,c.det(horzcat(*[x,DM([1,2])])),DM([-1,2])), not implemented (in1,v1,f1.call(in1)[1],y), (in1,v1,f1.call([x**2,y])[1],y*2*vertcat(*[x.T,x.T])), (in1,v1,f2.call(in1)[0],DM.zeros(0,2)), (in1,v1,f2(x**2,y),DM.zeros(0,2)), (in1,v1,f3.call(in1)[0],DM.zeros(0,2)), (in1,v1,f3.call([x**2,y])[0],DM.zeros(0,2)), (in1,v1,f4.call(in1)[0],DM.zeros(0,2)), (in1,v1,f4.call([x**2,y])[0],DM.zeros(0,2)), #(in1,v1,f1([x**2,[]])[1],DM.zeros(2,2)), #(in1,v1,f1([[],y])[1],DM.zeros(2,2)), (in1,v1,vertcat(*[x,DM(0,1)]),DM.eye(2)), (in1,v1,project(x**2, sparsify(DM([0,1])).sparsity()),blockcat([[MX(1,1),MX(1,1)],[MX(1,1),2*x[1]]])), (in1,v1,c.dot(x,y[:,0]),y[:,0].T), (in1,v1,x.nz[IM([[1,0]])].T*y.nz[IM([[0,2]])],blockcat([[MX(1,1),y.nz[0]],[y.nz[2],MX(1,1)]])), (in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX(1,1),y.nz[0]],[MX(1,1),MX(1,1)],[MX(1,1),MX(1,1)],[y.nz[2],MX(1,1)]])), ]: print(out) fun = Function("fun", inputs,[out,jac]) funsx = fun.expand("expand_fun") fun_ad = [Function("fun", inputs,[out,jac], {'ad_weight':w, 'ad_weight_sp':w}) for w in [0,1]] funsx_ad = [f.expand('expand_'+f.name()) for f in fun_ad] fun_out = fun.call(values) funsx_out = funsx.call(values) self.checkarray(fun_out[0],funsx_out[0]) self.checkarray(fun_out[1],funsx_out[1]) self.check_codegen(fun,inputs=values) J_ = fun_out[1] def vec(l): ret = [] for i in l: ret.extend(i) return ret storage2 = {} storage = {} vf_mx = None for f in [fun, fun.expand('expand_'+fun.name())]: d1 = f.forward(ndir) d2 = f.reverse(ndir) num_in = f.n_in() num_out = f.n_out() # evalThings for sym in [MX.sym, SX.sym]: if f.is_a('MXFunction') and sym==SX.sym: continue if f.is_a('SXFunction') and sym==MX.sym: continue # dense for spmod,spmod2 in itertools.product(spmods,repeat=2): fseeds = [[sym("f",spmod(f.sparsity_in(i))) for i in range(f.n_in())] for d in range(ndir)] aseeds = [[sym("a",spmod2(f.sparsity_out(i))) for i in range(f.n_out())] for d in range(ndir)] inputss = [sym("i",f.sparsity_in(i)) for i in range(f.n_in())] res = f.call(inputss,True) fwdsens = forward(res,inputss,fseeds,dict(always_inline=True)) adjsens = reverse(res,inputss,aseeds,dict(always_inline=True)) fseed = [DM(fseeds[d][0].sparsity(),random.random(fseeds[d][0].nnz())) for d in range(ndir) ] aseed = [DM(aseeds[d][0].sparsity(),random.random(aseeds[d][0].nnz())) for d in range(ndir) ] vf = Function("vf", inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)])) vf_in = list(values) offset = len(inputss) for d in range(ndir): vf_in.append(fseed[d]) for i in range(len(values)-1): vf_in.append(0) vf_in.append(aseed[d]) vf_in.append(0) vf_out = vf.call(vf_in) self.check_codegen(vf,inputs=vf_in) offset = len(res) for d in range(ndir): seed = array(fseed[d].T).ravel() sens = array(vf_out[offset+0].T).ravel() offset+=len(inputss) self.checkarray(sens,mtimes(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym))) seed = array(aseed[d].T).ravel() sens = array(vf_out[offset+0].T).ravel() offset+=len(inputss) self.checkarray(sens,mtimes(J_.T,seed),"eval Adj %d %s" % (d,str([vf_out[i] for i in range(vf.n_out())]))) assert(offset==vf.n_out()) # Complete random seeding random.seed(1) vf_in = [] for i in range(vf.n_in()): vf_in.append(DM(vf.sparsity_in(i),random.random(vf.nnz_in(i)))) vf_out = vf.call(vf_in) self.check_codegen(vf,inputs=vf_in) storagekey = (spmod,spmod2) if not(storagekey in storage): storage[storagekey] = [] storage[storagekey].append(vf_out) # Added to make sure that the same seeds are used for SX and MX if sym is MX.sym: vf_mx = vf # Second order sensitivities for sym2 in [MX.sym, SX.sym]: if vf.is_a('MXFunction') and sym2==SX.sym: continue if vf.is_a('MXFunction') and sym2==MX.sym: continue for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2): fseeds2 = [[sym2("f",vf_mx.sparsity_in(i)) for i in range(vf.n_in())] for d in range(ndir)] aseeds2 = [[sym2("a",vf_mx.sparsity_out(i)) for i in range(vf.n_out()) ] for d in range(ndir)] inputss2 = [sym2("i",vf_mx.sparsity_in(i)) for i in range(vf.n_in())] res2 = vf.call(inputss2,True) fwdsens2 = forward(res2,inputss2,fseeds2,dict(always_inline=True)) adjsens2 = reverse(res2,inputss2,aseeds2,dict(always_inline=True)) vf2 = Function("vf2", inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)])) random.seed(1) vf2_in = [] for i in range(vf2.n_in()): vf2_in.append(DM(vf2.sparsity_in(i),random.random(vf2.nnz_in(i)))) vf2_out = vf2.call(vf2_in) self.check_codegen(vf2,inputs=vf2_in) storagekey = (spmod,spmod2) if not(storagekey in storage2): storage2[storagekey] = [] storage2[storagekey].append(vf2_out) # Remainder of eval testing for store,order in [(storage,"first-order"),(storage2,"second-order")]: for stk,st in list(store.items()): for i in range(len(st)-1): for k,(a,b) in enumerate(zip(st[0],st[i+1])): if b.numel()==0 and sparsify(a).nnz()==0: continue if a.numel()==0 and sparsify(b).nnz()==0: continue self.checkarray(sparsify(a),sparsify(b),("%s, output(%d)" % (order,k))) for expand in [False, True]: # jacobian() for mode in ["forward","reverse"]: ind = 0 if mode=='forward' else 1 f = fun_ad[ind] if expand else funsx_ad[ind] Jf=f.jacobian_old(0,0) Jf_out = Jf.call(values) self.check_codegen(Jf,inputs=values) self.checkarray(Jf_out[0],J_) self.checkarray(DM.ones(Jf.sparsity_out(0)),DM.ones(J_.sparsity()),str(out)+str(mode)) self.checkarray(DM.ones(f.sparsity_jac(0, 0)),DM.ones(J_.sparsity())) # Scalarized if out.is_empty(): continue s_i = out.sparsity().row()[0] s_j = out.sparsity().get_col()[0] s_k = s_i*out.size2()+s_j H_ = None for expand in [False, True]: for mode in ["forward","reverse"]: w = 0 if mode=='forward' else 1 f = Function("fun", inputs,[out[s_i,s_j],jac[s_k,:].T], {'ad_weight':w, 'ad_weight_sp':w}) if expand: f=f.expand('expand_'+f.name()) f_out = f.call(values) J_ = f_out[1] Hf=f.hessian_old(0, 0) Hf_out = Hf.call(values) self.check_codegen(Hf,inputs=values) if H_ is None: H_ = Hf_out[0] self.checkarray(Hf_out[0],H_,failmessage=("mode: %s" % mode))
def construct_upd_xz(self, problem=None): # construct optifather & give reference to problem self.father_updx = OptiFather(self.group.values()) self.problem.father = self.father_updx # define z_ij variables init = self.q_ij_struct(0) for nghb, q_ij in self.q_ij.items(): for child, q_j in q_ij.items(): for name, ind in q_j.items(): var = np.array(child._values[name]) v = var.T.flatten()[ind] init[nghb.label, child.label, name, ind] = v z_ij = self.define_variable( 'z_ij', self.q_ij_struct.shape[0], value=np.array(init.cat)) # define parameters l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0]) l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0]) # put them in the struct format z_ij = self.q_ij_struct(z_ij) l_ij = self.q_ij_struct(l_ij) l_ji = self.q_ji_struct(l_ji) # get (part of) variables x_i = self._get_x_variables(symbolic=True) # construct local copies of parameters par = {} for name, s in self.par_global.items(): par[name] = self.define_parameter(name, s.shape[0], s.shape[1]) if problem is None: # get time info t = self.define_symbol('t') T = self.define_symbol('T') 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, tf, self.q_i) self._transform_spline([z_ij, l_ij], tf, self.q_ij) self._transform_spline(l_ji, tf, self.q_ji) # 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] for nghb in self.q_ji.keys(): l = l_ji[str(nghb), child.label, name] obj += mtimes(l.T, x) for nghb, q_j in self.q_ij.items(): for child in q_j.keys(): for name in q_j[child].keys(): z = z_ij[str(nghb), child.label, name] l = l_ij[str(nghb), child.label, name] obj -= mtimes(l.T, z) self.define_objective(obj) # construct constraints for con in self.global_constraints: c = con[0] for sym in symvar(c): for label, child in self.group.items(): if sym.name() in child.symbol_dict: name = child.symbol_dict[sym.name()][1] v = x_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.name() in child.symbol_dict: name = child.symbol_dict[sym.name()][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_global.items(): if s.name() == sym.name(): c = substitute(c, sym, par[name]) lb, ub = con[1], con[2] self.define_constraint(c, lb, ub) # construct problem prob, buildtime = self.father_updx.construct_problem( self.options, str(self._index), problem) self.problem_upd_xz = prob self.father_updx.init_transformations(self.problem.init_primal_transform, self.problem.init_dual_transform) self.init_var_dd() return buildtime
def test_linear_system(self): self.message("Linear ODE") if not(scipy_available): return A=array([2.3,4.3,7.6]) B=array([[1,2.3,4],[-2,1.3,4.7],[-2,6,9]]) te=0.7 Be=expm(B*te) t=SX.sym("t") q=SX.sym("q",3,1) p=SX.sym("p",9,1) f=SXFunction(daeIn(t=t,x=q,p=p),daeOut(ode=mul(c.reshape(p,3,3),q))) f.init() integrator = Integrator("cvodes",f) integrator.setOption("fsens_err_con", True) integrator.setOption("reltol",1e-15) integrator.setOption("abstol",1e-15) #integrator.setOption("verbose",True) integrator.setOption("steps_per_checkpoint",10000) integrator.setOption("t0",0) integrator.setOption("tf",te) integrator.init() q0 = MX.sym("q0",3,1) par = MX.sym("p",9,1) qend, = integratorOut(integrator.call(integratorIn(x0=q0,p=par)),"xf") qe=MXFunction([q0,par],[qend]) qe.init() qendJ=integrator.jacobian("x0","xf") qendJ.init() qendJ = qendJ.call(integratorIn(x0=q0,p=par))[0] qeJ=MXFunction([q0,par],[qendJ]) qeJ.init() qendJ2=integrator.jacobian("x0","xf") qendJ2.init() qendJ2 = qendJ2.call(integratorIn(x0=q0,p=par))[0] qeJ2=MXFunction([q0,par],[qendJ2]) qeJ2.init() qe.setInput(A,0) qe.setInput(vec(B),1) qe.evaluate() self.checkarray(dot(Be,A)/1e3,qe.getOutput()/1e3,"jacobian(INTEGRATOR_X0,INTEGRATOR_XF)") qeJ.setInput(A,0) qeJ.setInput(vec(B),1) qeJ.evaluate() self.checkarray(qeJ.getOutput()/1e3,Be/1e3,"jacobian(INTEGRATOR_X0,INTEGRATOR_XF)") qeJ2.setInput(A,0) qeJ2.setInput(vec(B),1) qeJ2.evaluate() return # this should return identical zero H=qeJ.jacobian(0,0) H.setOption("ad_mode","reverse") H.init() H.setInput(A,0) H.setInput(vec(B),1) H.evaluate() print array(H.getOutput())
# Lesser General Public License for more details. # # You should have received a copy of the GNU Lesser General Public # License along with CasADi; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA # # #! createParent #!====================== from casadi import * from numpy import * import casadi as c ##! We create a bunch of matrices that depend on V V = MX("V",13) A = c.reshape(V[:2],sp_dense(2,1)) # 2x1 matrix B = c.reshape(V[2:3],sp_dense(1,1)) # 1x1 matrix C = c.reshape(V[3:],sp_tril(4)) # Triangular matrix print "A = ", A print "A.mapping() = ", A.mapping() print "B = ", B print "B.mapping() = ", B.mapping() print "C = ", C print "C.mapping() = ", C.mapping() ##! We create a bunch of matrices that depend on V V_= DMatrix(13,1,True) # create a dense DMatrix ##! We can use mapping() to index into this DMatrix: V_[A.mapping()] = 1