def eval_opt_ineq_N(self, z, p): x = z[:self.n_x] hyp_w = p[self.n_x+self.N_ineq*self.d_ineq+self.N_ineq : self.n_x+self.N_ineq*self.d_ineq+self.N_ineq+self.n_x] hyp_b = p[self.n_x+self.N_ineq*self.d_ineq+self.N_ineq+self.n_x] t_opt = z[0:2] R_opt = np.array([ [ca.cos(z[2]), -ca.sin(z[2])], [ca.sin(z[2]), ca.cos(z[2])] ]) obca = [] obca_d = [] obca_norm = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ self.n_x + j : self.n_x + j + self.n_ineq[i] ] mu = z[ self.n_x + self.N_ineq + i*self.m_ineq : self.n_x + self.N_ineq + (i+1)*self.m_ineq ] obca.append( ca.mtimes( ca.transpose(self.G), mu) + ca.mtimes( ca.transpose(ca.mtimes(A, R_opt)), Lambda ) ) obca_d.append( -ca.dot(self.g, mu) + ca.dot(ca.mtimes(A, t_opt)-b, Lambda) ) obca_norm.append( ca.dot(ca.mtimes( ca.transpose(A), Lambda), ca.mtimes( ca.transpose(A), Lambda)) ) j += self.n_ineq[i] opt_ineq = ca.vcat( [ca.vcat(obca), ca.vcat(obca_d), ca.vcat(obca_norm)] ) opt_ineq = ca.vcat( [opt_ineq, ca.dot(hyp_w, x) - hyp_b] ) return opt_ineq
def error_contour_lag(self, z, p):
theta = z[self.index["theta_position"]]
px = z[self.index['x_position']]
py = z[self.index['y_position']]
pz = z[self.index['z_position']]
rx = casadi.polyval(p[self.index["px"]], theta)
ry = casadi.polyval(p[self.index["py"]], theta)
rz = casadi.polyval(p[self.index["pz"]], theta)
drx = casadi.polyval(p[self.index["pdx"]], theta)
dry = casadi.polyval(p[self.index["pdy"]], theta)
drz = casadi.polyval(p[self.index["pdz"]], theta)
tangent_normalized = self.calculate_tangent(drx, dry, drz)
# set point
s = casadi.vcat([rx, ry, rz])
pos = casadi.vcat([px, py, pz])
r = s - pos
# lag
lag = casadi.dot(r, tangent_normalized)
e_lag = lag**2
# contour
contour = r - (lag * tangent_normalized)
e_contour = casadi.dot(contour, contour)
return e_contour, e_lag
def add_friction_constraint(self, contact, p, z):
mu = contact.friction
ZG = p - z
ZG2 = casadi.dot(ZG, ZG)
ZGn2 = casadi.dot(ZG, contact.n)**2
slackness = ZG2 - (1 + mu**2) * ZGn2
self.nlp.add_constraint(slackness, lb=[-self.nlp.infty], ub=[0])
def eval_ws_eq(self, z, p): t_ws = p[0:2] R_ws = np.array( [ [ca.cos(p[2]), -ca.sin(p[2])], [ca.sin(p[2]), ca.cos(p[2])] ] ) obca_d = [] obca = [] j = 0 for i in range(self.n_obs): A = p[ self.n_x + j*self.d_ineq : self.n_x + (j+self.n_ineq[i])*self.d_ineq ].reshape( (self.n_ineq[i], self.d_ineq) ) b = p[ self.n_x + self.N_ineq*self.d_ineq + j : self.n_x + self.N_ineq*self.d_ineq + j + self.n_ineq[i] ] Lambda = z[ j : j + self.n_ineq[i] ] mu = z[ self.N_ineq + i*self.m_ineq : self.N_ineq + (i+1)*self.m_ineq ] d = z[ self.N_ineq + self.M_ineq + i ] obca_d.append( -ca.dot(self.g, mu) + ca.dot( ca.mtimes(A, t_ws)-b, Lambda ) - d) obca.append( ca.mtimes(self.G.T, mu) + ca.mtimes( ca.mtimes(A, R_ws).T, Lambda ) ) j += self.n_ineq[i] ws_eq = ca.vcat( [ca.vcat(obca_d), ca.vcat(obca)] ) return ws_eq
def create_collision_constraints(lam, mu, Ar, Ao, br, bo, eps=1e-6):
cons = []
cons.append(-dot(br, lam) - dot(bo, mu) >= eps)
cons.append(Ar.T @ lam + Ao.T @ mu == 0.0)
cons.append(dot(Ar.T @ lam, Ar.T @ lam) <= 1.0)
cons.append(lam >= 0.0)
cons.append(mu >= 0.0)
return cons
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 add_cop_constraint(self, contact, p, z, scaling=0.95):
X = scaling * contact.X
Y = scaling * contact.Y
CZ, ZG = z - contact.p, p - z
CZxZG = casadi.cross(CZ, ZG)
Dx = casadi.dot(contact.b, CZxZG) / X
Dy = casadi.dot(contact.t, CZxZG) / Y
ZGn = casadi.dot(contact.n, ZG)
slackness = Dx**2 + Dy**2 - ZGn**2
self.nlp.add_constraint(slackness, lb=[-self.nlp.infty], ub=[-0.005])
def setContactConstraints(self):
total_left = 0
total_right = 0
##--Left Leg--###
j = 0
for is_contact, del_T in self.time_phases['Left Leg'].items():
if is_contact == 'No Contact @ ' + str(j):
for n in range(self.knot_points_per_phase):
lforce = self.lforce[str(j) + '_' + str(n)]
self.opti.subject_to(lforce == 0)
else: # Contact
for knot_point in self.dlpos:
lpos = self.lpos[knot_point]
dlpos = self.dlpos[knot_point]
lforce = self.lforce[knot_point]
self.opti.subject_to(
(self.terrain.mu * lforce[0])**2 - lforce[1]**2 >= 0)
self.opti.subject_to(ca.dot(lforce, lpos) >= 0)
self.opti.subject_to(
lpos[1] == self.terrain.heightMap(lpos[0]))
self.opti.subject_to(dlpos == 0)
total_left += del_T
j += 1
self.opti.subject_to(total_left == self.total_duration)
##--Right Leg--###
j = 0
for is_contact, del_T in self.time_phases['Right Leg'].items():
if is_contact == 'No Contact @ ' + str(j):
for n in range(self.knot_points_per_phase):
rforce = self.rforce[str(j) + '_' + str(n)]
self.opti.subject_to(rforce == 0)
else:
for knot_point in self.drpos:
rpos = self.rpos[knot_point]
drpos = self.drpos[knot_point]
rforce = self.rforce[knot_point]
self.opti.subject_to(
(self.terrain.mu * rforce[0])**2 - rforce[1]**2 >= 0)
self.opti.subject_to(ca.dot(rforce, rpos) >= 0)
self.opti.subject_to(
rpos[1] == self.terrain.heightMap(rpos[0]))
self.opti.subject_to(drpos == 0)
total_right += del_T
j += 1
self.opti.subject_to(total_right == self.total_duration)
def ExplicitFixedStepIntegrator(f,times=None,a=None,b=None,c=None):
""" a,b,c are the tableau coefficients
If s is the number of stages, then we have:
a: tril(s-1 x s-1)
b,c: (sx1)
times may be DVector or SXVector
"""
if not(isinstance(times,DMatrix)):
times = DMatrix(times)
def toSX(a):
return casadi.reshape(SXMatrix(a),a.shape[0],a.shape[1])
times = toSX(times)
a = toSX(a)
b = toSX(b)
c = toSX(c)
x_init = f.inputSX(ODE_Y)
N = x_init.numel()
p = f.inputSX(ODE_P)
s=b.numel()
assert(a.size1()==s-1)
assert(a.size2()==s-1)
assert(c.numel()==s)
if s>1:
for lhs,rhs in zip(c[1:,0],casadi.sum(a,1)):
pass
#assert(lhs==rhs)
ks = SXMatrix(N,s)
y = x_init
for k in range(len(times)-1):
t = times[k]
h = times[k+1]-times[k]
for i in range(s):
if i>0:
x = y + casadi.dot(ks[:,:i],a[i-1,:i].T)*h
else:
x = y
ks[:,i] = f.eval({ODE_T: t+c[i,0]*h, ODE_Y: x, ODE_P: p})[0]
y+= casadi.dot(ks,b)*h
return SXFunction([x_init,p],[y])
def product(cls, r1, r2):
assert r1.shape == (4, 1) or r1.shape == (4, )
assert r2.shape == (4, 1) or r2.shape == (4, )
a = r1[:3]
b = r2[:3]
na_sq = ca.dot(a, a)
nb_sq = ca.dot(b, b)
res = ca.SX(4, 1)
den = (1 + na_sq * nb_sq - 2 * ca.dot(b, a))
res[:3] = ((1 - na_sq) * b +
(1 - nb_sq) * a - 2 * ca.cross(b, a)) / den
res[3] = 0 # shadow state
return res
def add_linear_friction_constraints(self, contact, p, z):
mu_inner = contact.friction / casadi.sqrt(2)
ZG = p - z
ZGt = casadi.dot(ZG, contact.t)
ZGb = casadi.dot(ZG, contact.b)
ZGn = casadi.dot(ZG, contact.n)
c0 = ZGt - mu_inner * ZGn
c1 = -ZGt - mu_inner * ZGn
c2 = ZGb - mu_inner * ZGn
c3 = -ZGb - mu_inner * ZGn
self.nlp.add_constraint(c0, lb=[-self.nlp.infty], ub=[0])
self.nlp.add_constraint(c1, lb=[-self.nlp.infty], ub=[0])
self.nlp.add_constraint(c2, lb=[-self.nlp.infty], ub=[0])
self.nlp.add_constraint(c3, lb=[-self.nlp.infty], ub=[0])
def get_state_trans_func(x, u, dt):
pos, θ, vel, ω, m, J, r, μ = x[0:2], x[2], x[3:5], x[5], x[6], x[7], x[
8:10], x[10]
fx, fy, tr = u[0], u[1], u[2]
pos_new = pos + vel * dt
# position of the center of mass.
θ_new = θ + ω * dt
# angular velocity of the center of mass
#θ_new = cs.mod(θ_new, 2*pi)
vel_new = vel - μ / m * vel * dt + 1 / m * cs.vcat([fx, fy]) * dt
ω_new = ω - 1/J*cs.dot(r,cs.vcat([cs.sin(θ),cs.cos(θ)]))*fx*dt + \
1/J*cs.dot(r,cs.vcat([cs.cos(θ),-cs.sin(θ)]))*fy*dt + 1/J*tr*dt
x_new = cs.vcat([pos_new, θ_new, vel_new, ω_new, m, J, r, μ])
f = cs.Function('f', [x, u], [x_new], ['x', 'u'], ['x_new'])
return f
def setupfun(self, N):
A = SX.sym("A", Sparsity.diag(N))
A[-1, 0] = SX("off") # Have one of-diagonal element
B = SX.sym("B", N, 1)
f = SXFunction([A, B], [c.dot(A, B)])
f.init()
return {'f': f}
def setupfun(self,N):
A = SX.sym("A",Sparsity.diag(N))
A[-1,0]=SX("off") # Have one of-diagonal element
B = SX.sym("B",N,1)
f = SXFunction([A,B],[c.dot(A,B)])
f.init()
return {'f':f}
def shadow(cls, r):
assert r.shape == (4, 1) or r.shape == (4, )
n_sq = ca.dot(r[:3], r[:3])
res = ca.SX(4, 1)
res[:3] = -r[:3] / n_sq
res[3] = ca.logic_not(r[3])
return res
def get_cost(self, x, u, slack, slack_obst, lam, q, safe_set, n_iter):
n = self.n_state
d = self.n_action
N = self.horizon
goal = self.goal
cost_mode = 'quad'
#terminal constraint cost
cost = 10e8 * ca.dot(slack, slack)
#stage cost
if cost_mode == 'quad':
for i in range(N):
cost = cost + (x[n * i:n * (i + 1)] -
goal).T @ self.Q @ (x[n * i:n * (i + 1)] - goal)
cost = cost + u[d * i:d * (i + 1)].T @ self.R @ u[d * i:d *
(i + 1)]
if cost_mode == 'unit':
for i in range(N):
cost = cost + 1.0
#terminal cost
val_list = [item for sublist in safe_set.value for item in sublist]
val_list = np.array(val_list).reshape([-1, 1])
cost = cost + lam.T @ val_list
return cost
def fit(self, input, output):
nsamples = input.shape[0]
fobj = 0
for n in range(nsamples):
yhat = self.sym['fcn'].call({
'u': input[n, :],
'w': self.sym['w']
})['yhat']
y = indicator_array(yhat.shape, output[n])
fobj -= cas.sum1(xlogy(y, yhat) + xlogy(
(1 - y), 1 - yhat)) # cross entropy loss
fobj = fobj / nsamples # scaling
if self.alpha:
fobj += self.alpha * cas.dot(self.sym['w'],
self.sym['w']) # L2 regularization
nlp = {'x': self.sym['w'], 'f': fobj, 'g': []}
opts = {
'print_time': False,
'ipopt': {
'tol': 1e-3,
'max_iter': 1000,
'hessian_approximation': 'limited-memory',
'print_level': 0
}
}
solver = cas.nlpsol('nlpsol', 'ipopt', nlp, opts)
res = solver(x0=self.weights)
self.weights = np.asarray(res['x'])
def _get_interp(self, t, states=None, x_rep='sx'):
""" Return a polynomial representation of the state vector
evaluated at time t.
states: list
indicies of which states to return
x_rep: 'sx' or 'op', most likely.
whether or not to interpolate symbolic or optimal values of the
state variable
"""
assert t < self.tf, "Requested time is outside of the simulation range"
h = self.tf / self.nk
if states is None: states = range(1, self.nx)
finite_element = int(t / h)
tau = (t % h) / h
basis = np.asarray(self.col_vars['lfcn'](tau)).flatten()
if x_rep != 'sx':
x = getattr(self.var, 'x_' + x_rep)
x_roots = x[finite_element, :, states]
return np.inner(basis, x_roots)
else:
x_roots = self.var.x_sx[finite_element, :, states]
return cs.dot(basis, x_roots)
def __init__(self, state_probabilities, asset_values):
self.state_probabilities = state_probabilities
self.asset_values = asset_values
self.validate()
n_states = self.n_states
n_assets = self.n_assets
self.opti = opti = casadi.Opti()
self.expressions = {}
p_asset_values = opti.parameter(n_states, n_assets)
opti.set_value(p_asset_values, asset_values)
p_state_probabilities = opti.parameter(n_states)
opti.set_value(p_state_probabilities, state_probabilities)
v_coded = opti.variable(n_assets)
coded_total = casadi.sum1(v_coded)
asset_weights = v_coded / (coded_total + 1) # the rest is cash
cash = 1.0 / (coded_total + 1) # 1-sum(weights)
state_values = casadi.mtimes(p_asset_values, asset_weights) + cash
kelly_criterion = casadi.dot(casadi.log(state_values),
p_state_probabilities)
opti.minimize(-kelly_criterion)
opti.subject_to(v_coded >= 0)
opti.set_initial(v_coded, np.ones(n_assets))
opti.solver('ipopt')
self.expressions = DS({
name: value
for name, value in locals().items()
if isinstance(value, casadi.MX)
})
def pseudorange_rate(x, params=None):
""" x = [x, y, z, b, xd, yd, zd, alpha, ...] where alpha is the receiver
clock bias rate term, assumes sat vel and xd are in same
coordinate representation.
y = [pseudorange rate] """
r = params["sat_pos"] - x[:3]
LoS = r / norm_2(r)
return dot(params["sat_vel"] - x[4:7], LoS) + x[7]
def cyclic(ocp, weight=1):
if ocp.nlp[0].nx != ocp.nlp[-1].nx:
raise RuntimeError(
"Cyclic constraint without same nx is not supported yet")
ocp.J += casadi.dot(ocp.nlp[-1].X[-1] - ocp.nlp[0].X[0],
ocp.nlp[-1].X[-1] - ocp.nlp[0].X[0]) * weight
def out(self, inn):
inn = cas.vec(inn)
assert self.number_in == inn.size()[0], 'Wrong number of inputs given.'
if self.activation == 'softmax':
y = self.activation_function(self.weights * inn)
else:
y = self.activation_function(cas.dot(self.weights, inn))
return y
def cyclic(ocp, weight=1):
if ocp.nlp[0]["nx"] != ocp.nlp[-1]["nx"]:
raise RuntimeError("Cyclic constraint without same nx is not supported yet")
ocp.J += (
casadi.dot(ocp.nlp[-1]["X"][-1] - ocp.nlp[0]["X"][0], ocp.nlp[-1]["X"][-1] - ocp.nlp[0]["X"][0]) * weight
)
def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False):
if self._thermo_constraint_expression is None:
self.build_thermo_constraint(thermoTem=298.15)
Pnlp = self._Pnlp
ini_p = np.hstack([dE_start])
dev = Pnlp - ini_p
if py == 2:
object_fxn = cas.mul(dev.T, dev)
elif py == 3:
object_fxn = cas.dot(dev, dev)
nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression)
nlpopts = dict()
if py == 2:
nlpopts['max_iter'] = 500
nlpopts['tol'] = 1e-8
nlpopts['expect_infeasible_problem'] = 'yes'
nlpopts['hessian_approximation'] = 'exact'
nlpopts['jac_d_constant'] = 'yes'
elif py == 3:
nlpopts['ipopt.max_iter'] = 500
nlpopts['ipopt.tol'] = 1e-8
nlpopts['ipopt.expect_infeasible_problem'] = 'yes'
nlpopts['ipopt.hessian_approximation'] = 'exact'
nlpopts['ipopt.jac_d_constant'] = 'yes'
if py == 2:
solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
elif py == 3:
solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts)
# Bounds and initial guess
lbP = -np.inf * np.ones(self._Np)
ubP = np.inf * np.ones(self._Np)
for i in range(len(fix_Ea)):
if check_index(fix_Ea[i], self.dEa_index):
lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
for i in range(len(fix_BE)):
if check_index(fix_BE[i], self.dBE_index):
lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
lbG = 0 * np.ones(2 * self.nrxn)
ubG = np.inf * np.ones(2 * self.nrxn)
if not print_screen:
old_stdout = sys.stdout
sys.stdout = tempfile.TemporaryFile()
solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP)
dE_corr = solution['x'].full().T[0].tolist()
if not print_screen:
sys.stdout = old_stdout
return(dE_corr)
def evaluate_coordinate_state_cost(self, coordinates_state):
""" evaluate the function h(x) """
value = 1.
for i in range(0, self._number_of_constraints):
value *= Obstacle.trim_and_square(\
cd.dot(self._a[:,i], coordinates_state) + self._b[i]\
)
return value
def from_mrp(cls, r):
assert r.shape == (4, 1)
a = r[:3]
X = wedge(a)
n_sq = ca.dot(a, a)
X_sq = ca.mtimes(X, X)
R = ca.SX.eye(3) + (8 * X_sq - 4 * (1 - n_sq) * X) / (1 + n_sq)**2
# return transpose, due to convention difference in book
return R.T
def kinematics(cls, r, w):
assert r.shape == (4, 1) or r.shape == (4, )
assert w.shape == (3, 1) or w.shape == (3, )
a = r[:3]
n_sq = ca.dot(a, a)
X = wedge(a)
B = 0.25 * ((1 - n_sq) * ca.SX.eye(3) + 2 * X +
2 * ca.mtimes(a, ca.transpose(a)))
return ca.vertcat(ca.mtimes(B, w), 0)
def add_friction_constraint(self, p, z, foothold):
"""
Add a circular friction cone constraint for a COM located at `p` and a
(floating-base) ZMP located at `z`.
Parameters
----------
p : casadi.MX
Symbol of COM position variable.
z : casadi.MX
Symbol of ZMP position variable.
"""
mu = foothold.friction
ZG = p - z
ZG2 = casadi.dot(ZG, ZG)
ZGn2 = casadi.dot(ZG, foothold.n) ** 2
slackness = ZG2 - (1 + mu ** 2) * ZGn2
self.nlp.add_constraint(slackness, lb=[-self.nlp.infty], ub=[-0.005])
def _add_to_penalty(ocp,
nlp,
val,
weight=1,
quadratic=False,
**parameters):
if quadratic:
ocp.J += casadi.dot(val, val) * weight
else:
ocp.J += casadi.sum1(val) * weight
def _add_to_penalty(ocp,
nlp,
val,
weight=1,
quadratic=False,
**extra_param):
if quadratic:
ocp.J += casadi.dot(val, val) * weight * nlp["dt"] * nlp["dt"]
else:
ocp.J += casadi.sum1(val) * weight * nlp["dt"]
def create_adjoint_states(self):
lamb = SX.sym(self.name + '_lamb', self.model.n_x)
nu = SX.sym(self.name + '_nu', self.model.n_y)
self.eta = SX.sym(self.name + '_eta', self.n_h_final)
self.H = self.L + dot(lamb, self.model.ode) + dot(nu, self.model.alg)
l_dot = -gradient(self.H, self.model.x)
alg_eq = gradient(self.H, self.model.y)
self.include_state(lamb, l_dot, suppress=True)
self.model.has_adjoint_variables = True
self.include_algebraic(nu, alg_eq)
self.h_final = vertcat(
self.h_final,
self.model.lamb_sym - gradient(self.V, self.model.x_sys_sym) -
mtimes(jacobian(self.h_final, self.model.x_sys_sym).T, self.eta))
def fun(self, N, setup):
c.dot(setup["A"], setup["B"])
def setupfun(self, N):
A = SX.sym("A", N, 1)
B = SX.sym("B", N, 1)
f = SXFunction("f", [A, B], [c.dot(A.T, B)])
return {"f": f}
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 norm_2(mx): return c.dot(mx,mx)
def fun(self,N,setup): c.dot(setup['A'],setup['B'])
def setupfun(self,N):
A = SX.sym("A",N,1)
B = SX.sym("B",N,1)
f = Function('f', [A,B],[c.dot(A.T,B)])
return {'f':f}
def __init__(self,objective,*args):
"""
optisolve(objective)
optisolve(objective,constraints)
"""
if len(args)>=1:
constraints = args[0]
else:
constraints = []
options = dict()
if len(args)>=2:
options = args[1]
if not isinstance(constraints,list):
raise Exception("Constraints must be given as list: [x>=0,y<=0]")
[ gl_pure, gl_equality] = sort_constraints( constraints )
symbols = OptimizationObject.get_primitives([objective]+gl_pure)
# helper functions for 'x'
X = C.veccat(*symbols["x"])
helper = C.Function('helper',[X],symbols["x"])
helper_inv = C.Function('helper_inv',symbols["x"],[X])
# helper functions for 'p' if applicable
if 'p' in symbols:
P = C.veccat(*symbols["p"])
self.Phelper_inv = C.Function('Phelper_inv',symbols["p"],[P])
else:
P = C.MX.sym('p',0,1)
if len(gl_pure)>0:
g_helpers = [];
for p in gl_pure:
g_helpers.append(C.MX.sym('g',p.sparsity()))
G_helpers = C.veccat(*g_helpers)
self.Ghelper = C.Function('Ghelper',[G_helpers],g_helpers)
self.Ghelper_inv = C.Function('Ghelper_inv',g_helpers,[G_helpers])
codegen = False;
if 'codegen' in options:
codegen = options["codegen"]
del options["codegen"]
opt = {}
if codegen:
options["jit"] = True
opt["jit"] = True
gl_pure_v = C.MX()
if len(gl_pure)>0:
gl_pure_v = C.veccat(*gl_pure)
if objective.is_vector() and objective.numel()>1:
F = C.vec(objective)
objective = 0.5*C.dot(F,F)
FF = C.Function('nlp',[X,P], [F])
JF = FF.jacobian()
J_out = JF.call([X,P])
J = J_out[0].T;
H = C.mtimes(J,J.T)
sigma = C.MX.sym('sigma')
Hf = C.Function('H',dict(x=X,p=P,lam_f=sigma,hess_gamma_x_x=sigma*C.triu(H)),['x', 'p', 'lam_f', 'lam_g'],['hess_gamma_x_x'],opt)
if "expand" in options and options["expand"]:
Hf = Hf.expand()
options["hess_lag"] = Hf
nlp = {"x":X,"p":P,"f":objective,"g":gl_pure_v}
self.solver = C.nlpsol('solver','ipopt', nlp, options)
# Save to class properties
self.symbols = symbols
self.helper = helper
self.helper_inv = helper_inv
self.gl_equality = gl_equality
self.resolve()
def setupfun(self,N):
A = SX.sym("A",N,1)
B = SX.sym("B",N,1)
f = SXFunction([A,B],[c.dot(A.T,B)])
f.init()
return {'f':f}
