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
