def set_path(self, path, r2r=False):
"""Define an analytic expression of the geometric path.
Note: The path must be defined as a function of self.s[0].
Args:
path (list of SXMatrix): An expression of the geometric path as
a function of self.s[0]. Its dimension must equal self.sys.ny
r2r (boolean): Reparameterize path such that a rest to rest
transition is performed
Example:
>>> S = FlatSystem(2, 4)
>>> P = PathFollowing(S)
>>> P.set_path([P.s[0], P.s[0]])
"""
if isinstance(path, list):
path = cas.vertcat(path)
if r2r:
path = cas.substitute(path, self.s[0], self._r2r())
self.path[:, 0] = path
dot_s = cas.vertcat([self.s[1:], 0])
for i in range(1, self.sys.order + 1):
self.path[:, i] = cas.mul(
cas.jacobian(self.path[:, i - 1], self.s), dot_s)
def _simulate_with_casadi_no_inputs(self, initcon, tsim, integrator,
integrator_options):
# Old way (10 times faster, but can't incorporate time
# varying parameters/controls)
xalltemp = [self._templatemap[i] for i in self._diffvars]
xall = casadi.vertcat(*xalltemp)
odealltemp = [convert_pyomo2casadi(self._rhsdict[i])
for i in self._derivlist]
odeall = casadi.vertcat(*odealltemp)
dae = {'x': xall, 'ode': odeall}
if len(self._algvars) != 0:
zalltemp = [self._templatemap[i] for i in self._simalgvars]
zall = casadi.vertcat(*zalltemp)
algalltemp = [convert_pyomo2casadi(i) for i in self._alglist]
algall = casadi.vertcat(*algalltemp)
dae['z'] = zall
dae['alg'] = algall
integrator_options['grid'] = tsim
integrator_options['output_t0'] = True
F = casadi.integrator('F', integrator, dae, integrator_options)
sol = F(x0=initcon)
profile = sol['xf'].full().T
if len(self._algvars) != 0:
algprofile = sol['zf'].full().T
profile = np.concatenate((profile, algprofile), axis=1)
return [tsim, profile]
def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
def exitEquation(self, tree):
logger.debug('exitEquation')
if isinstance(tree.left, list):
src_left = ca.vertcat(*[self.get_mx(c) for c in tree.left])
else:
src_left = self.get_mx(tree.left)
if isinstance(tree.right, list):
src_right = ca.vertcat(*[self.get_mx(c) for c in tree.right])
else:
src_right = self.get_mx(tree.right)
src_left = ca.MX(src_left)
src_right = ca.MX(src_right)
# According to the Modelica spec,
# "It is possible to omit left hand side component references and/or truncate the left hand side list in order to discard outputs from a function call."
if isinstance(tree.right, ast.Expression) and tree.right.operator in self.root.classes:
if src_left.size1() < src_right.size1():
src_right = src_right[0:src_left.size1()]
if isinstance(tree.left, ast.Expression) and tree.left.operator in self.root.classes:
if src_left.size1() > src_right.size1():
src_left = src_left[0:src_right.size1()]
# If dimensions between the lhs and rhs do not match, but the dimensions of lhs
# and transposed rhs do match, transpose the rhs.
if src_left.shape != src_right.shape and src_left.shape == src_right.shape[::-1]:
src_right = ca.transpose(src_right)
self.src[tree] = src_left - src_right
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]),
C.horzcat([ 0, conf['j2'], 0]),
C.horzcat([conf['j31'], 0, conf['j3']])])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
C.horzcat([mm10,mm11,mm12]),
C.horzcat([mm20,mm21,mm22])])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def get_derivative(self, s):
# Case 1: s is a constant, e.g. MX(5)
if ca.MX(s).is_constant():
return 0
# Case 2: s is a symbol, e.g. MX(x)
elif s.is_symbolic():
if s.name() not in self.derivative:
if len(self.for_loops) > 0 and s in self.for_loops[-1].indexed_symbols:
# Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol.
for_loop_symbol = self.for_loops[-1].indexed_symbols[s]
s_without_index = self.get_mx(ast.ComponentRef(name=for_loop_symbol.tree.name))
der_s_without_index = self.get_derivative(s_without_index)
if ca.MX(der_s_without_index).is_symbolic():
return self.get_indexed_symbol(ast.ComponentRef(name=der_s_without_index.name(), indices=for_loop_symbol.tree.indices), der_s_without_index)
else:
return 0
else:
der_s = _new_mx("der({})".format(s.name()), s.size())
# If the derivative contains an expression (e.g. der(x + y)) this method is
# called with MX variables that are the result of a ca.symvar call. This
# ca.symvar call strips the _modelica_shape field from the MX variable,
# therefore we need to find the original MX to get the modelica shape.
der_s._modelica_shape = \
self.nodes[self.current_class][s.name()]._modelica_shape
self.derivative[s.name()] = der_s
self.nodes[self.current_class][der_s.name()] = der_s
return der_s
else:
return self.derivative[s.name()]
# Case 3: s is an already indexed symbol, e.g. MX(x[1])
elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic():
slice_info = s.info()['slice']
dep = s.dep()
if dep.name() not in self.derivative:
der_dep = _new_mx("der({})".format(dep.name()), dep.size())
der_dep._modelica_shape = \
self.nodes[self.current_class][dep.name()]._modelica_shape
self.derivative[dep.name()] = der_dep
self.nodes[self.current_class][der_dep.name()] = der_dep
return der_dep[slice_info['start']:slice_info['stop']:slice_info['step']]
else:
return self.derivative[dep.name()][slice_info['start']:slice_info['stop']:slice_info['step']]
# Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2)
# Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2)
else:
# Differentiate expression using CasADi
orig_deps = ca.symvar(s)
deps = ca.vertcat(*orig_deps)
J = ca.Function('J', [deps], [ca.jacobian(s, deps)])
J_sparsity = J.sparsity_out(0)
der_deps = [self.get_derivative(dep) if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size()) for j, dep in enumerate(orig_deps)]
return ca.mtimes(J(deps), ca.vertcat(*der_deps))
def _stateDictToVec(self, stateDict):
stateList = [stateDict[s] for s in self.states]
# concatanate and return appropriate type
if isinstance(stateList[0], C.MX): # MX
return C.vertcat(stateList)
elif isinstance(stateList[0], C.SX): # SX
return C.vertcat(stateList)
else: # numpy array
return np.concatenate(stateList, axis=0)
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho*x)
G = -(1/rho)*mtimes(A, A.T)
h = b + (1/rho)*mtimes(A, f)
mu = solve(G, h)
z = -(1/rho)*(mtimes(A.T, mu)+f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
self.problem_upd_z = prob
return buildtime
def split_dae_alg(eqs: SYM, dx: SYM) -> Dict[str, SYM]:
"""Split equations into differential algebraic and algebraic only"""
dae = []
alg = []
for eq in ca.vertsplit(eqs):
if ca.depends_on(eq, dx):
dae.append(eq)
else:
alg.append(eq)
return {
'dae': ca.vertcat(*dae),
'alg': ca.vertcat(*alg)
}
def solve(self):
"""
define extra variables
homotopy parameters >= 0!!
"""
if not self.prob['s']:
self.set_grid()
N = self.options['N']
Nc = self.options['Nc']
self.prob['vars'] = [cas.ssym("b", N + 1, self.sys.order),
cas.ssym("h", Nc, self.h.size1())]
# Vectorize variables
V = cas.vertcat([
cas.vec(self.prob['vars'][0]),
cas.vec(self.prob['vars'][1])
])
self._make_constraints()
self._make_objective()
self._h_init()
con = cas.SXFunction([V], [self.prob['con'][0]])
obj = cas.SXFunction([V], [self.prob['obj']])
if self.options.get('solver') == 'Ipopt':
solver = cas.IpoptSolver(obj, con)
else:
print """Other solver than Ipopt are currently not supported,
switching to Ipopt"""
solver = cas.IpoptSolver(obj, con)
for option, value in self.options.iteritems():
if solver.hasOption(option):
solver.setOption(option, value)
solver.init()
# Setting constraints
solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg")
solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg")
solver.setInput(
cas.vertcat([
[np.inf] * self.sys.order * (N + 1),
[1] * self.h.size1() * Nc
]), "ubx"
)
solver.setInput(
cas.vertcat([
[0] * (N + 1),
[-np.inf] * (self.sys.order - 1) * (N + 1),
[0] * self.h.size1() * Nc
]), "lbx"
)
solver.solve()
self.prob['solver'] = solver
self._get_solution()
def makeModel(conf, propertiesDir = '../properties'):
print "\n#### File '%s' is old. It does not use the NED convention you should use carouselModel in offline_mhe_stuff instead.\n" \
% inspect.getfile(inspect.currentframe())
#input("Press Enter if you wish to continue...")
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
rA = conf['rArm']
g = conf['g']
pIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat')))
RIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat')))
ddpIMU = C.mul(R.T,ddp) - ddelta**2*C.mul(R.T,C.vertcat([x+rA,y,0])) + 2*ddelta*C.mul(R.T,C.vertcat([-dy,dx,0])) + dddelta*C.mul(R.T,C.vertcat([-y,x+rA,0])) + C.mul(R.T,C.vertcat([0,0,g]))
aBridle = C.cross(ddt_w_bn_b,pIMU)
dae['IMU_acceleration'] = C.mul(RIMU,ddpIMU+aBridle)
dae['IMU_angular_velocity'] = C.mul(RIMU,dae['w_bn_b'])
camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC1.dat'))),
'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC2.dat'))),
'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC1.dat'))),
'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC2.dat'))),
'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body1.dat'))),
'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body2.dat'))),
'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body3.dat')))}
dae['marker_positions'] = camModel.fullCamModel(dae,camConf)
dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
#dae['ConstDelta'] = dae['cos_delta']*dae['cos_delta'] + dae['sin_delta']*dae['sin_delta'] - 1
dae['ConstDelta'] = ( dae['cos_delta']**2 + dae['sin_delta']**2 - 1 )
return dae
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A([par])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
def set_data(self, data, weights=None):
""" Attach experimental measurement data.
data : a pd.DataFrame object
Data should have columns corresponding to the state labels in
self.boundary_species, with an index corresponding to the measurement
times.
"""
# Should raise an error if no state name is present
df = data.loc[:, self.boundary_species]
# Rename columns with state indicies
df.columns = np.arange(self.nx)
# Remove empty (nonmeasured) states
self.data = df.loc[:, ~pd.isnull(df).all(0)]
if weights is None:
weights = self.data.max()
obj_list = []
for ((ti, state), xi) in self.data.stack().iteritems():
obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]]
obj_resid = cs.sum_square(cs.vertcat(obj_list))
self.objective_sx += obj_resid
def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def solve(self):
"""Solve the optimal control problem
solve() first check for steady state feasibility and defines the
optimal control problem. After solving, the instance variable sol can
be used to examine the solution.
TODO: Add support for other solvers
TODO: version bump
"""
if len(self.prob['s']) == 0:
self.set_grid()
# Check feasibility
# self.check_ss_feasibility()
# Construct optimization problem
self.prob['solver'] = None
N = self.options['N']
self.prob['vars'] = cas.ssym("b", N + 1, self.sys.order)
V = cas.vec(self.prob['vars'])
self._make_objective()
self._make_constraints()
con = cas.SXFunction([V], [self.prob['con'][0]])
obj = cas.SXFunction([V], [self.prob['obj']])
if self.options.get('solver') == 'Ipopt':
solver = cas.IpoptSolver(obj, con)
else:
print """Other solver than Ipopt are currently not supported,
switching to Ipopt"""
solver = cas.IpoptSolver(obj, con)
for option, value in self.options.iteritems():
if solver.hasOption(option):
solver.setOption(option, value)
solver.init()
# Setting constraints
solver.setInput(cas.vertcat(self.prob['con'][1]), "lbg")
solver.setInput(cas.vertcat(self.prob['con'][2]), "ubg")
solver.setInput([np.inf] * self.sys.order * (N + 1), "ubx")
solver.setInput(
cas.vertcat((
[0] * (N + 1),
(self.sys.order - 1) * (N + 1) * [-np.inf])),
"lbx")
solver.solve()
self.prob['solver'] = solver
self._get_solution()
def setUp(self):
# System
self.x = ca.MX.sym("x", 2)
self.p = ca.MX.sym("p", 2)
self.u = ca.MX.sym("u", 0)
# self.v = ca.MX.sym("v", 2)
self.eps_e = ca.MX.sym("eps_e", 2)
self.f = ca.vertcat( \
[-1.0 * self.x[0] + self.p[0] * self.x[0] * self.x[1],
1.0 * self.x[1] - self.p[1] * self.x[0] * self.x[1]]) + \
self.eps_e
self.phi = self.x
self.odesys = pecas.systems.ExplODE(x = self.x, u = self.u, \
p = self.p, eps_e = self.eps_e, f = self.f, phi = self.phi)
# Inputs
data = np.array(np.loadtxt("test/data_lotka_volterra.txt"))
self.tu = data[:, 0]
self.invalidpargs = [[0, 1, 2], [[2, 3], [2, 3]], \
np.asarray([1, 2, 3]), np.asarray([[2, 3], [2, 3]])]
self.validpargs = [None, [2, 3], np.asarray([3, 4]), \
np.asarray([1, 2]).T, np.asarray([[2], [3]])]
self.invalidxargs = [np.ones((self.x.size() - 1, self.tu.size)), \
np.ones((self.tu.size - 1, self.x.size()))]
self.validxargs = [None, np.ones((self.x.size(), self.tu.size)), \
np.ones((self.tu.size, self.x.size()))]
# Since the supported values are never used, there is no case of
# invalid u-arguments in this testcase
self.invaliduargs = []
self.validuargs = [None, [], [1, 2, 3]]
# -- TODO! --
# None of the checks will detect an invalidxpvbarg of
# [[2, 1], [3]], since shape and size both fit.
# --> How to check for this?
self.yN = data[:, 1::2]
self.wv = 1.0 / data[:, 2::2]**2
self.uN = None
self.weps_e = [1.0 / 1e-4, 1.0 / 1e-4]
self.weps_u = [None]
self.pinit = [0.5, 1.0]
self.xinit = self.yN
self.phat = np.atleast_2d([0.69348187, 0.34116928]).T
def getFourierDcm(vars):
return C.vertcat(
[
C.horzcat([vars["e11"], vars["e12"], vars["e13"]]),
C.horzcat([vars["e21"], vars["e22"], vars["e23"]]),
C.horzcat([vars["e31"], vars["e32"], vars["e33"]]),
]
)
def _set_objective_from_data(self, data, weights):
obj_list = []
for ((ti, state), xi) in data.stack().iteritems():
obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]]
obj_resid = cs.sum_square(cs.vertcat(obj_list))
self.objective_sx += obj_resid
def chen_model():
""" The following ODE model comes from Chen1998. """
nx, nu = (2, 1)
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1]))
return Function('chen', [x, u], [rhs]), nx, nu
def casadi_struct2vec(s):
flat = []
if isinstance(s,OrderedDict):
for f in s.keys():
flat.append(casadi_struct2vec(s[f]))
return C.vertcat(flat)
else:
return C.vec(s)
def setUp(self):
self.x = ca.MX.sym("x", 4)
self.p = ca.MX.sym("p", 6)
self.u = ca.MX.sym("u", 2)
self.f = ca.vertcat( \
[self.x[3] * np.cos(self.x[2] + self.p[0] * self.u[0]),
self.x[3] * np.sin(self.x[2] + self.p[0] * self.u[0]),
self.x[3] * self.u[0] * self.p[1],
self.p[2] * self.u[1] \
- self.p[3] * self.u[1] * self.x[3] \
- self.p[4] * self.x[3]**2 \
- self.p[5] \
- (self.x[3] * self.u[0])**2 * self.p[1] * self.p[0]])
self.phi = self.x
data = np.array(np.loadtxt("test/data_2d_vehicle_pe.dat", \
delimiter = ", ", skiprows = 1))
self.time_points = data[100:250, 1]
self.ydata = data[100:250, [2, 4, 6, 8]]
self.udata = data[100:249, [9, 10]]
self.pinit =[0.5, 17.06, 12.0, 2.17, 0.1, 0.6]
self.xinit = self.ydata
self.phat = np.atleast_2d( \
[0.200652, 11.6528, -26.2501, -74.1967, 16.8705, -1.80125]).T
self.covariance_matrix = np.loadtxt( \
"test/covariance_matrix_2d_vehicle_pe.txt", delimiter=",")
self.time_points_doe = data[200:205, 1]
self.ydata_doe = data[200:205, [2, 4, 6, 8]]
self.uinit_doe = data[200:205, [9, 10]][:-1, :]
self.pdata_doe = [0.273408, 11.5602, 2.45652, 7.90959, -0.44353, -0.249098]
self.umin_doe = [-0.436332, -0.3216]
self.umax_doe = [0.436332, 1.0]
self.xmin_doe = [-0.787, -1.531, -12.614, 0.0]
self.xmax_doe = [1.2390, 0.014, 0.013, 0.7102]
self.design_results = np.atleast_2d( \
np.loadtxt("test/optimized_controls_2d_vehicle_doe.txt")).T
def exitIfEquation(self, tree):
logger.debug('exitIfEquation')
# Check if every equation block contains the same number of equations
if len(set((len(x) for x in tree.blocks))) != 1:
raise Exception("Every branch in an if-equation needs the same number of equations.")
# NOTE: We currently assume that we always have an else-clause. This
# is not strictly necessary, see the Modelica Spec on if equations.
assert tree.conditions[-1] == True
src = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-1]])
for cond_index in range(1, len(tree.conditions)):
cond = self.get_mx(tree.conditions[-(cond_index + 1)])
expr1 = ca.vertcat(*[self.get_mx(e) for e in tree.blocks[-(cond_index + 1)]])
src = ca.if_else(cond, expr1, src, True)
self.src[tree] = src
def skew_mat(xyz):
'''
return the skew symmetrix matrix of a 3-vector
'''
x = xyz[0]
y = xyz[1]
z = xyz[2]
return C.vertcat([C.horzcat([ 0, -z, y]),
C.horzcat([ z, 0, -x]),
C.horzcat([-y, x, 0])])
def _get_solution(self):
"""Get the solution from the solver output
Fills the dictionary self.sol with the information:
* 's': The optimal s as a function of time
* 't': The time vector
* 'states': Numerical values of the states defined in self.sys
TODO: perform accurate integration to determine time
TODO: Do exact interpolation
"""
solver = self.prob['solver']
N = self.options['N']
x_opt = np.array(solver.getOutput("x")).ravel()
b_opt = np.reshape(x_opt, (N + 1, -1), order='F')
self.sol['b'] = b_opt
# Determine time on a sufficiently fine spatial grid
s0 = np.linspace(0, 1, 1001)
delta = s0[1] - s0[0]
pieces = [lambda s, b=b_opt, ss=ss, j=j:
sum([bb * (s - ss) ** i / fact[i]
for i, bb in enumerate(b[j])])
for j, ss in enumerate(self.prob['s'][:-1])]
conds = lambda s0: [np.logical_and(self.prob['s'][i] <= s0,
s0 <= self.prob['s'][i+1])
for i in range(N)]
b0_opt = np.piecewise(s0, conds(s0), pieces)
b0_opt[b0_opt < 0] = 0
time = np.cumsum(np.hstack([0, 2 * delta / (np.sqrt(b0_opt[:-1]) +
np.sqrt(b0_opt[1:]))]))
# Resample to constant time-grid
t = np.arange(time[0], time[-1], self.options['Ts'])
st = np.interp(t, time, s0)
# Evaluate solution on equidistant time grid
b_opt = np.c_[[np.piecewise(st, conds(st), pieces, b=b_opt[:, i:])
for i in range(self.sys.order)]].T
st = np.matrix(st)
# Determine s and derivatives from b_opt
b, Ds = self._make_path()[1:]
Ds_f = cas.SXFunction([b], [Ds]) # derivatives of s wrt b
Ds_f.init()
s_opt = np.hstack((st.T, np.array([evalf(Ds_f, bb).toArray().ravel()
for bb in b_opt])))
self.sol['s'] = np.asarray(s_opt)
self.sol['t'] = t
# Evaluate the states
f = cas.SXFunction([self.s], [cas.substitute(cas.vertcat(self.sys.x.values()),
self.sys.y, self.path)])
f_val = np.array([evalf(f, s.T).toArray().ravel() for s in s_opt])
self.sol['states'] = dict([(k, f_val[:, i]) for i, k in
enumerate(self.sys.x.keys())])
def init(self, horizon_times=None):
if self.options['spline_traj'] == False:
# pos, vel, acc
x = self.define_parameter('x', self.n_dim)
v = self.define_parameter('v', self.n_dim)
a = self.define_parameter('a', self.n_dim)
# pos, vel, acc at time zero of time horizon
self.t = self.define_symbol('t')
# motion time can be passed from environment
if horizon_times is None:
self.T = self.define_symbol('T')
elif not isinstance(horizon_times, list):
horizon_times = [horizon_times]
v0 = v - self.t*a
x0 = x - self.t*v0 - 0.5*(self.t**2)*a
a0 = a
if horizon_times: # not None
# build up pos_spline gradually, e.g. the pos_spline for second segment starts at
# end position for first segment (pos0(1))
pos0 = x0
self.pos_spline = [0]*self.n_dim
for horizon_time in horizon_times:
for k in range(self.n_dim):
self.pos_spline[k] = BSpline(self.basis, vertcat(pos0[k], 0.5*v0[k]*horizon_time + pos0[k], pos0[k] + v0[k]*horizon_time + 0.5*a0[k]*(horizon_time**2)))
# update start position for next segment
pos0 = [self.pos_spline[k](1) for k in range(self.n_dim)]
else:
# horizon_times was None
# pos spline over time horizon
self.pos_spline = [BSpline(self.basis, vertcat(x0[k], 0.5*v0[k]*self.T + x0[k], x0[k] + v0[k]*self.T + 0.5*a0[k]*(self.T**2)))
for k in range(self.n_dim)]
else:
# using a spline to define obstacle trajectory
self.basis = BSplineBasis(self.options['spline_params']['knots'], self.options['spline_params']['degree'])
traj_coeffs = self.define_parameter('traj_coeffs', len(self.basis), self.n_dim)
# pos spline over time horizon
self.pos_spline = [BSpline(self.basis, traj_coeffs[:, k]) for k in range(self.n_dim)]
# checkpoints + radii
checkpoints, _ = self.shape.get_checkpoints()
self.checkpoints = self.define_parameter('checkpoints', len(checkpoints)*self.n_dim)
self.rad = self.define_parameter('rad', len(checkpoints))
def _initialize_polynomial_coefs(self):
""" Setup radau polynomials and initialize the weight factor matricies
"""
self.col_vars['tau_root'] = cs.collocationPoints(self.d, "radau")
# Dimensionless time inside one control interval
tau = cs.SX.sym("tau")
# For all collocation points
L = [[]]*(self.d+1)
for j in range(self.d+1):
# Construct Lagrange polynomials to get the polynomial basis at the
# collocation point
L[j] = 1
for r in range(self.d+1):
if r != j:
L[j] *= (
(tau - self.col_vars['tau_root'][r]) /
(self.col_vars['tau_root'][j] -
self.col_vars['tau_root'][r]))
self.col_vars['lfcn'] = lfcn = cs.SXFunction(
'lfcn', [tau], [cs.vertcat(L)])
# Evaluate the polynomial at the final time to get the coefficients of
# the continuity equation
# Coefficients of the continuity equation
self.col_vars['D'] = lfcn([1.0])[0].toArray().squeeze()
# Evaluate the time derivative of the polynomial at all collocation
# points to get the coefficients of the continuity equation
tfcn = lfcn.tangent()
# Coefficients of the collocation equation
self.col_vars['C'] = np.zeros((self.d+1, self.d+1))
for r in range(self.d+1):
self.col_vars['C'][:,r] = tfcn([self.col_vars['tau_root'][r]]
)[0].toArray().squeeze()
# Find weights for gaussian quadrature: approximate int_0^1 f(x) by
# Sum(
xtau = cs.SX.sym("xtau")
Phi = [[]] * (self.d+1)
for j in range(self.d+1):
tau_f_integrator = cs.SXFunction('ode', cs.daeIn(t=tau, x=xtau),
cs.daeOut(ode=L[j]))
tau_integrator = cs.Integrator(
"integrator", "cvodes", tau_f_integrator, {'t0':0., 'tf':1})
Phi[j] = np.asarray(tau_integrator({'x0' : 0})['xf'])[0][0]
self.col_vars['Phi'] = np.array(Phi)
self.col_vars['alpha'] = cs.SX.sym('alpha')
def fullCamModel(dae,conf):
PdatC1 = conf['PdatC1']
PdatC2 = conf['PdatC2']
RPC1 = conf['RPC1']
RPC2 = conf['RPC2']
pos_marker_body1 = conf['pos_marker_body1']
pos_marker_body2 = conf['pos_marker_body2']
pos_marker_body3 = conf['pos_marker_body3']
RpC1 = C.DMatrix.eye(4)
RpC1[0:3,0:3] = RPC1[0:3,0:3].T
RpC1[0:3,3] = C.mul(-RPC1[0:3,0:3].T,RPC1[0:3,3])
RpC2 = C.DMatrix.eye(4);
RpC2[0:3,0:3] = RPC2[0:3,0:3].T
RpC2[0:3,3] = C.mul(-RPC2[0:3,0:3].T,RPC2[0:3,3])
PC1 = C.SXMatrix(3,3)
PC1[0,0] = PdatC1[0]
PC1[1,1] = PdatC1[1]
PC1[0,2] = PdatC1[2]
PC1[1,2] = PdatC1[3]
PC1[2,2] = 1.0
PC2 = C.SXMatrix(3,3)
PC2[0,0] = PdatC2[0]
PC2[1,1] = PdatC2[1]
PC2[0,2] = PdatC2[2]
PC2[1,2] = PdatC2[3]
PC2[2,2] = 1.0
p = C.vertcat([dae['x'],dae['y'],dae['z']])
R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
uv_all = C.vertcat([C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body1)) ,\
C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body2)) ,\
C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body3)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body1)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body2)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body3))])
return uv_all
def _initialize_tgrid(self):
# Choose collocation points
tau_root = cs.collocationPoints(self.opts["degree"], self.opts["polynomial"])
# Degree of interpolating polynomial
d = self.opts["degree"]
# Size of the finite elements
self.opts["h"] = self.opts["tf"] / self.opts["nk"]
# Coefficients of the collocation equation
self.opts["C"] = np.zeros((d + 1, d + 1))
# Dimensionless time inside one control interval
tau = cs.SX.sym("tau")
T = np.zeros((self.opts["nk"], d + 1))
for k in range(self.opts["nk"]):
for j in range(d + 1):
T[k, j] = self.opts["h"] * (k + tau_root[j])
self.opts["T"] = T
# Construct Lagrange polynomials to get the polynomial basis at the
# collocation point
L = [[]] * (d + 1)
# For all collocation points
for j in range(d + 1):
L[j] = 1
for r in range(d + 1):
if r != j:
L[j] *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r])
self._lfcn = cs.SXFunction("lfcn", [tau], [cs.vertcat(L)])
# Evaluate the polynomial at the final time to get the coefficients of
# the continuity equation
self.opts["D"] = np.asarray(self._lfcn([1.0])[0]).squeeze()
# Evaluate the time derivative of the polynomial at all collocation
# points to get the coefficients of the continuity equation
tfcn = self._lfcn.tangent()
for r in range(d + 1):
self.opts["C"][:, r] = tfcn([tau_root[r]])[0].toArray().squeeze()
self._tgrid = np.array(
[
point + self.opts["h"] * np.array(tau_root)
for point in np.linspace(0, self.opts["tf"], self.opts["nk"], endpoint=False)
]
).flatten()
def _make_objective(self):
"""Construct objective function from the problem definition
Make time optimal objective function and add a regularization to
ensure a unique solution. When additional objective terms are defined
these are added to the objective as well.
TODO: Improve accuracy of integration
"""
order = self.sys.order
N = self.options['N']
b = self.prob['vars']
# ds = 1.0/(N+1)
obj = 2 * sum(np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0])))
# reg = sum(cas.sqrt((b[1:, order - 1] - b[:N, order - 1]) ** 2))
reg = sum((b[2:, order - 1] - 2 * b[1:N, order - 1] +
b[:(N - 1), order - 1]) ** 2)
for f in self.objective['Lagrange']:
path, bs = self._make_path()[0:2]
# S = np.arange(0, 1, 1.0/(N+1))
S = self.prob['s']
b = self.prob['vars']
L = cas.substitute(f, self.sys.y, path)
L = 2 * sum(cas.vertcat([
cas.substitute(
L,
cas.vertcat([self.s[0], bs]),
cas.vertcat([S[j], b[j, :].T])
) for j in range(1, N + 1)
])
* np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0]))
)
# L = sum(cas.vertcat([cas.substitute(L, cas.vertcat([self.s[0],
# [bs[i] for i in range(0, bs.numel())]]),
# cas.vertcat([S[j], [b[j, i] for i in range(0, self.sys.order)]]))
# for j in range(0, N + 1)]))
obj = obj + L
self.prob['obj'] = obj + self.options['reg'] * reg
def T2WJ(T,p):
"""
w_101 = T2WJ(T_10,p).diff(p,t)
"""
R = T2R(T)
RT = R.T
temp = []
for i,k in [(2,1),(0,2),(1,0)]:
#temp.append(c.mul(c.jacobian(R[:,k],p).T,R[:,i]).T)
temp.append(c.mul(RT[i,:],c.jacobian(R[:,k],p)))
return c.vertcat(temp)
-5.53636820e-01, 1.86726808e-01, -1.32319806e-01, -2.06761360e+00,
3.12421835e-02, 8.89043596e-01, -7.03329152e-01
]
q0_2 = [
0.36148756, 0.19562711, 0.34339407, -2.06759027, -0.08427634,
0.89133467, 0.75131025
]
#Implementing with my L1 norm method
hqp = hqp()
#decision variables for robot 1
q1, q_dot1 = hqp.create_variable(7, 1e-6)
J1 = jac_fun_rob(q1)
J1 = cs.vertcat(J1[0], J1[1])
#progress variable for robot 1
s_1, s_dot1 = hqp.create_variable(1, 1e-6)
fk_vals1 = robot.fk(q1)[6] #forward kinematics first robot
#decision variables for robot 2
q2, q_dot2 = hqp.create_variable(7, 1e-6)
J2 = jac_fun_rob(q2)
J2 = cs.vertcat(J2[0], J2[1])
#progress variables for robot 2
s_2, s_dot2 = hqp.create_variable(1, 1e-6)
fk_vals2 = robot.fk(q2)[6] #forward kinematics second robot
fk_vals2[1, 3] += 0.3 #accounting for the base offset in the y-direction
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter),
*expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True,
tree.operands[0],
f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(
entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(
entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
arg = self.get_mx(tree.operands[2])
if n_operands == 4:
assert isinstance(tree.operands[3], ast.Primary)
mode = tree.operands[3].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp], yp)
src = func(arg)
elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(
entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(
entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
if isinstance(tree.operands[2], ast.ComponentRef):
zp = self.get_mx(
entered_class.symbols[tree.operands[2].name].value)
else:
zp = self.get_mx(tree.operands[2])
arg_1 = self.get_mx(tree.operands[3])
arg_2 = self.get_mx(tree.operands[4])
if n_operands == 6:
assert isinstance(tree.operands[5], ast.Primary)
mode = tree.operands[5].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp, yp],
np.array(zp).ravel(order='F'))
src = func(ca.vertcat(arg_1, arg_2))
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
func = self.get_function(op)
src = ca.vertcat(*func.call(
[self.get_mx(operand)
for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def ODEmodel():
#==================================================================
#State variable definitions
#==================================================================
M = cs.ssym("M")
Pc = cs.ssym("Pc")
Pn = cs.ssym("Pn")
#for Casadi
y = cs.vertcat([M, Pc, Pn])
# Time Variable
t = cs.ssym("t")
#===================================================================
#Parameter definitions
#===================================================================
vs0 = cs.ssym('vs0')
light = cs.ssym('light')
alocal = cs.ssym('alocal')
couplingStrength = cs.ssym('couplingStrength')
n = cs.ssym('n')
vm = cs.ssym('vm')
k1 = cs.ssym('k1')
km = cs.ssym('km')
ks = cs.ssym('ks')
vd = cs.ssym('vd')
kd = cs.ssym('kd')
k1_ = cs.ssym('k1_')
k2_ = cs.ssym('k2_')
paramset = cs.vertcat([vs0, light, alocal, couplingStrength,
n, vm, k1, km,
ks, vd, kd, k1_,
k2_])
#===================================================================
# Model Equations
#===================================================================
ode = [[]]*EqCount
def pm_prod(Pn, K, v0, n, light, a, M, Mi):
vs = v0+light+a*(M-Mi)
return (vs*K**n)/(K**n + Pn**n)
def pm_deg(M, Km, vm):
return vm*M/(Km+M)
def Pc_prod(ks, M):
return ks*M
def Pc_deg(Pc, K, v):
return v*Pc/(K+Pc)
def Pc_comp(k1,k2,Pc,Pn):
return -k1*Pc + k2*Pn
def Pn_comp(k1,k2,Pc,Pn):
return k1*Pc - k1*Pn
#Rxns
ode[0] = (pm_prod(Pn, k1, vs0, n, light, alocal, M, M) - pm_deg(M,km,vm))
ode[1] = Pc_prod(ks,M) - Pc_deg(Pc,kd,vd) + Pc_comp(k1_,k2_,Pc,Pn)
ode[2] = Pn_comp(k1_,k2_,Pc,Pn)
ode = cs.vertcat(ode)
fn = cs.SXFunction(cs.daeIn(t=t, x=y, p=paramset),
cs.daeOut(ode=ode))
fn.setOption("name","arya")
return fn
theta = ocp.state() # Defince controls delta = ocp.control() V = ocp.control(order=0) # Specify ODE ocp.set_der(x, V*cos(theta)) ocp.set_der(y, V*sin(theta)) ocp.set_der(theta, V/L*tan(delta)) # Define parameter X_0 = ocp.parameter(nx) # Initial constraints X = vertcat(x, y, theta) ocp.subject_to(ocp.at_t0(X) == X_0) # Initial guess ocp.set_initial(x, 0) ocp.set_initial(y, 0) ocp.set_initial(theta, 0) ocp.set_initial(V, 0.5) # Path constraints ocp.subject_to( 0 <= (V <= 1) ) #ocp.subject_to( -0.3 <= (ocp.der(V) <= 0.3) ) ocp.subject_to( -pi/6 <= (delta <= pi/6) ) # Minimal time
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case):
"""
QP solver for path-following algorithm
inputs: prob - problem description
obj - problem equations
p_init - initial parameter
x_init - initial primal variable
y_init - initial dual variable
lam_opt - Lagrange multipliers of equality and active constraints
mu_opt - Lagrange multipliers of inequality constraints
outputs: y - solution primal variable
qp_val - objective function value
qp_exit - return status of QP solver
deriv - derivatives of the problem
k_zero_tilde - active set index
k_plus_tilde - inactive set index
grad - gradient of objective function
"""
print 'Current point x:', x_init
#Importing problem to be solved
nx, np, neq, niq, name = prob()
x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj(
x_init, y_init, p_init, neq, niq, nx, np)
#Deteriming constraint types
eq_con_ind = array([]) #indices of equality constraints
iq_con_ind = array([]) #indices of inequality constraints
eq_con = array([]) #equality constraints
iq_con = array([]) #inequality constraints
for i in range(0, len(lbg[0])):
if lbg[0, i] == 0:
eq_con = vertcat(eq_con, con[i])
eq_con_ind = append(eq_con_ind, i)
elif lbg[0, i] < 0:
iq_con = vertcat(iq_con, con[i])
iq_con_ind = append(iq_con_ind, i)
# print 'Equality Constraint:', eq_con
# print 'Inequality Constraint:', iq_con
# if case == 'pure-predictor':
# return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
if case == 'predictor-corrector':
#Evaluating constraints at current iteration point
con_vals = conf(x_init, p_init)
#Determining which inequality constraints are active
k_plus_tilde = array([]) #active constraint
k_zero_tilde = array([]) #inactive constraint
tol = 10e-5 #tolerance
for i in range(0, len(iq_con_ind)):
if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0,
i] + tol:
k_plus_tilde = append(k_plus_tilde, i)
else:
k_zero_tilde = append(k_zero_tilde, i)
# print 'Active constraints:', k_plus_tilde
# print 'Inactive constraints:', k_zero_tilde
# print 'Constraint values:', con_vals
nk_pt = len(k_plus_tilde) #number of active constraints
nk_zt = len(k_zero_tilde) #number of inactive constraints
#Calculating Lagrangian
lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints
mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints
lag_f = f + mtimes(lam.T, eq_con) + mtimes(
mu.T, iq_con) #Lagrangian equation
#Calculating derivatives
g = gradient(f, x) #Derivative of objective function
g_fun = Function('g_fun', [x, p], [gradient(f, x)])
H = 2 * jacobian(gradient(lag_f, x),
x) #Second derivative of the Lagrangian
H_fun = Function('H_fun', [x, p, lam, mu],
[jacobian(jacobian(lag_f, x), x)])
if len(eq_con_ind) > 0:
deq = jacobian(eq_con, x) #Derivative of equality constraints
else:
deq = array([])
if len(iq_con_ind) > 0:
diq = jacobian(iq_con, x) #Derivative of inequality constraints
else:
diq = array([])
#Creating constraint matrices
nc = niq + neq #Total number of constraints
if (niq > 0) and (neq > 0): #Equality and inequality constraints
#this part needs to be tested
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
print deq
A[0, :] = deq #A matrix
lba = -1e16 * SX.zeros((nc, 1))
lba[0, :] = -eq_con #lower bound of A
uba = 1e16 * SX.zeros((nc, 1))
uba[0, :] = -eq_con #upper bound of A
for j in range(0, nk_pt): #adding active constraints
A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :]
lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])]
uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])]
for i in range(0, nk_zt): #adding inactive constraints
A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :]
uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else: #Active constraints only
A = vertcat(deq, diq)
lba = vertcat(-eq_con, -iq_con)
uba = vertcat(-eq_con, -iq_con)
elif (niq > 0) and (neq == 0): #Inquality constraints
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
lba = -1e16 * SX.ones((nc, 1))
uba = 1e16 * SX.ones((nc, 1))
for j in range(0, nk_pt): #adding active constraints
A[j, :] = diq[int(k_plus_tilde[j]), :]
lba[j] = -iq_con[int(k_plus_tilde[j])]
uba[j] = -iq_con[int(k_plus_tilde[j])]
for i in range(0, nk_zt): #adding inactive constraints
A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :]
uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else:
raw_input()
A = vertcat(deq, diq)
lba = -iq_con
uba = -iq_con
elif (niq == 0) and (neq > 0): #Equality constriants
A = deq
lba = -eq_con
uba = -eq_con
A_fun = Function('A_fun', [x, p], [A])
lba_fun = Function('lba_fun', [x, p], [lba])
uba_fun = Function('uba_fun', [x, p], [uba])
#Checking that matrices are correct sizes and types
if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'):
#H matrix should be a sparse (nxn) and symmetrical
print(
'WARNING: H matrix is not the correct dimensions or matrix type'
)
if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True':
#g matrix should be a dense (nx1)
print(
'WARNING: g matrix is not the correct dimensions or matrix type'
)
if (A.size1() !=
(neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'):
#A should be a sparse (nc x n)
print(
'WARNING: A matrix is not the correct dimensions or matrix type'
)
if lba.size1() != (neq + niq) or (lba.size2() !=
1) or lba.is_dense() == 'False':
print(
'WARNING: lba matrix is not the correct dimensions or matrix type'
)
if uba.size1() != (neq + niq) or (uba.size2() !=
1) or uba.is_dense() == 'False':
print(
'WARNING: uba matrix is not the correct dimensions or matrix type'
)
#Evaluating QP matrices at optimal points
H_opt = H_fun(x_init, p_init, lam_opt, mu_opt)
g_opt = g_fun(x_init, p_init)
A_opt = A_fun(x_init, p_init)
lba_opt = lba_fun(x_init, p_init)
uba_opt = uba_fun(x_init, p_init)
# print 'Lower bounds', lba_opt
# print 'Upper bounds', uba_opt
# print 'Bound matrix', A_opt
#Defining QP structure
qp = {}
qp['h'] = H_opt.sparsity()
qp['a'] = A_opt.sparsity()
optimize = conic('optimize', 'qpoases', qp)
optimal = optimize(h=H_opt,
g=g_opt,
a=A_opt,
lba=lba_opt,
uba=uba_opt,
x0=x_init)
x_qpopt = optimal['x']
if x_qpopt.shape == x_init.shape:
qp_exit = 'optimal'
else:
qp_exit = ''
lag_qpopt = optimal['lam_a']
#Determing Lagrangian multipliers (lambda and mu)
lam_qpopt = zeros(
(nk_pt, 1)) #Lagrange multiplier of active constraints
mu_qpopt = zeros(
(nk_zt, 1)) #Lagrange multiplier of inactive constraints
if nk_pt > 0:
for j in range(0, len(k_plus_tilde)):
lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])]
if nk_zt > 0:
for k in range(0, len(k_zero_tilde)):
print lag_qpopt[int(k_zero_tilde[k])]
return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
def create_integrator(self,
model,
inputs,
t_eval=None,
use_event_switch=False):
"""
Method to create a casadi integrator object.
If t_eval is provided, the integrator uses t_eval to make the grid.
Otherwise, the integrator has grid [0,1].
"""
pybamm.logger.debug("Creating CasADi integrator")
# Use grid if t_eval is given
use_grid = not (t_eval is None)
if use_grid is True:
t_eval_shifted = t_eval - t_eval[0]
t_eval_shifted_rounded = np.round(t_eval_shifted,
decimals=12).tobytes()
# Only set up problem once
if model in self.integrators:
# If we're not using the grid, we don't need to change the integrator
if use_grid is False:
return self.integrators[model]["no grid"]
# Otherwise, create new integrator with an updated grid
# We don't need to update the grid if reusing the same t_eval
# (up to a shift by a constant)
else:
if t_eval_shifted_rounded in self.integrators[model]:
return self.integrators[model][t_eval_shifted_rounded]
else:
method, problem, options = self.integrator_specs[model]
options["grid"] = t_eval_shifted
integrator = casadi.integrator("F", method, problem,
options)
self.integrators[model][
t_eval_shifted_rounded] = integrator
return integrator
else:
y0 = model.y0
rhs = model.casadi_rhs
algebraic = model.casadi_algebraic
# When not in DEBUG mode (level=10), suppress warnings from CasADi
if (pybamm.logger.getEffectiveLevel() == 10
or pybamm.settings.debug_mode is True):
show_eval_warnings = True
else:
show_eval_warnings = False
options = {
**self.extra_options_setup,
"reltol": self.rtol,
"abstol": self.atol,
"show_eval_warnings": show_eval_warnings,
}
# set up and solve
t = casadi.MX.sym("t")
p = casadi.MX.sym("p", inputs.shape[0])
y_diff = casadi.MX.sym("y_diff", rhs(0, y0, p).shape[0])
y_alg = casadi.MX.sym("y_alg", algebraic(0, y0, p).shape[0])
y_full = casadi.vertcat(y_diff, y_alg)
if use_grid is False:
# rescale time
t_min = casadi.MX.sym("t_min")
t_max = casadi.MX.sym("t_max")
t_max_minus_t_min = t_max - t_min
t_scaled = t_min + (t_max - t_min) * t
# add time limits as inputs
p_with_tlims = casadi.vertcat(p, t_min, t_max)
else:
options.update({"grid": t_eval_shifted, "output_t0": True})
# rescale time
t_min = casadi.MX.sym("t_min")
# Set dummy parameters for consistency with rescaled time
t_max_minus_t_min = 1
t_scaled = t_min + t
p_with_tlims = casadi.vertcat(p, t_min)
# define the event switch as the point when an event is crossed
# we don't do this for ODE models
# see #1082
event_switch = 1
if use_event_switch is True and not algebraic(0, y0, p).is_empty():
for event in model.casadi_terminate_events:
event_switch *= event(t_scaled, y_full, p)
problem = {
"t": t,
"x": y_diff,
# rescale rhs by (t_max - t_min)
"ode":
(t_max_minus_t_min) * rhs(t_scaled, y_full, p) * event_switch,
"p": p_with_tlims,
}
if algebraic(0, y0, p).is_empty():
method = "cvodes"
else:
method = "idas"
problem.update({
"z": y_alg,
"alg": algebraic(t_scaled, y_full, p),
})
integrator = casadi.integrator("F", method, problem, options)
self.integrator_specs[model] = method, problem, options
if use_grid is False:
self.integrators[model] = {"no grid": integrator}
else:
self.integrators[model] = {t_eval_shifted_rounded: integrator}
return integrator
def _integrate(self, model, t_eval, inputs_dict=None):
"""
Solve a DAE model defined by residuals with initial conditions y0.
Parameters
----------
model : :class:`pybamm.BaseModel`
The model whose solution to calculate.
t_eval : numeric type
The times at which to compute the solution
inputs_dict : dict, optional
Any external variables or input parameters to pass to the model when solving
"""
# Record whether there are any symbolic inputs
inputs_dict = inputs_dict or {}
has_symbolic_inputs = any(
isinstance(v, casadi.MX) for v in inputs_dict.values())
# convert inputs to casadi format
inputs = casadi.vertcat(*[x for x in inputs_dict.values()])
# Calculate initial event signs needed for some of the modes
if (has_symbolic_inputs is False and self.mode != "fast"
and model.terminate_events_eval):
init_event_signs = np.sign(
np.concatenate([
event(t_eval[0], model.y0, inputs)
for event in model.terminate_events_eval
]))
else:
init_event_signs = np.sign([])
if has_symbolic_inputs:
# Create integrator without grid to avoid having to create several times
self.create_integrator(model, inputs)
solution = self._run_integrator(model,
model.y0,
inputs_dict,
inputs,
t_eval,
use_grid=False)
solution.termination = "final time"
return solution
elif self.mode in ["fast", "fast with events"] or not model.events:
if not model.events:
pybamm.logger.info("No events found, running fast mode")
if self.mode == "fast with events":
# Create the integrator with an event switch that will set the rhs to
# zero when voltage limits are crossed
use_event_switch = True
else:
use_event_switch = False
# Create an integrator with the grid (we just need to do this once)
self.create_integrator(model,
inputs,
t_eval,
use_event_switch=use_event_switch)
solution = self._run_integrator(model, model.y0, inputs_dict,
inputs, t_eval)
# Check if the sign of an event changes, if so find an accurate
# termination point and exit
solution = self._solve_for_event(solution, init_event_signs)
return solution
elif self.mode in ["safe", "safe without grid"]:
y0 = model.y0
# Step-and-check
t = t_eval[0]
t_f = t_eval[-1]
pybamm.logger.debug("Start solving {} with {}".format(
model.name, self.name))
if self.mode == "safe without grid":
# in "safe without grid" mode,
# create integrator once, without grid,
# to avoid having to create several times
self.create_integrator(model, inputs)
# Initialize solution
solution = pybamm.Solution(np.array([t]), y0, model,
inputs_dict)
solution.solve_time = 0
solution.integration_time = 0
use_grid = False
else:
solution = None
use_grid = True
# Try to integrate in global steps of size dt_max. Note: dt_max must
# be at least as big as the the biggest step in t_eval (multiplied
# by some tolerance, here 1.01) to avoid an empty integration window below
if self.dt_max:
# Non-dimensionalise provided dt_max
dt_max = self.dt_max / model.timescale_eval
else:
dt_max = 0.01
dt_eval_max = np.max(np.diff(t_eval)) * 1.01
dt_max = np.max([dt_max, dt_eval_max])
while t < t_f:
# Step
solved = False
count = 0
dt = dt_max
while not solved:
# Get window of time to integrate over (so that we return
# all the points in t_eval, not just t and t+dt)
t_window = np.concatenate(
([t], t_eval[(t_eval > t) & (t_eval < t + dt)]))
# Sometimes near events the solver fails between two time
# points in t_eval (i.e. no points t < t_i < t+dt for t_i
# in t_eval), so we simply integrate from t to t+dt
if len(t_window) == 1:
t_window = np.array([t, t + dt])
if self.mode == "safe":
# update integrator with the grid
self.create_integrator(model, inputs, t_window)
# Try to solve with the current global step, if it fails then
# halve the step size and try again.
try:
current_step_sol = self._run_integrator(
model,
y0,
inputs_dict,
inputs,
t_window,
use_grid=use_grid)
solved = True
except pybamm.SolverError:
dt /= 2
# also reduce maximum step size for future global steps
dt_max = dt
count += 1
if count >= self.max_step_decrease_count:
raise pybamm.SolverError(
"Maximum number of decreased steps occurred at t={}. Try "
"solving the model up to this time only or reducing dt_max "
"(currently, dt_max={})."
"".format(t * model.timescale_eval,
dt_max * model.timescale_eval))
# Check if the sign of an event changes, if so find an accurate
# termination point and exit
current_step_sol = self._solve_for_event(
current_step_sol, init_event_signs)
# assign temporary solve time
current_step_sol.solve_time = np.nan
# append solution from the current step to solution
solution = solution + current_step_sol
if current_step_sol.termination == "event":
break
else:
# update time
t = t_window[-1]
# update y0
y0 = solution.all_ys[-1][:, -1]
return solution
def setup_initial_problem_solver(self):
"""Sets up the initial problem solver, for finding slack and virtual
variables before the solver should run."""
# Test if we don't need to do anything
shortcut = self.skill_spec._has_virtual is None
shortcut = shortcut and self.skill_spec.slack_var is None
if shortcut:
# If no slack, and no virtual, nothing to initializes
self._has_initial = False
return None
# Prepare variables
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
robot_vel_var = self.skill_spec.robot_vel_var
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
slack_var = self.skill_spec.slack_var
nvirt = self.skill_spec.n_virtual_var
nslack = self.skill_spec.n_slack_var
mu = self.weight_shifter
# Prepare cost expression
opt_var = []
opt_weights = []
if nvirt > 0:
opt_var += [virtual_vel_var]
opt_weights += [mu * self.virtual_var_weights]
if nslack > 0:
opt_var += [slack_var]
opt_weights += [(1 + mu) * self.slack_var_weights]
H_expr = cs.diag(cs.vertcat(*opt_weights))
# Prepare constraints expressions
cnstr_expr_list = []
lb_cnstr_expr_list = []
ub_cnstr_expr_list = []
slack_ind = 0
virt_ind = 0
for cnstr in self.skill_spec.constraints:
found_virt = False
found_slack = False
expr_size = cnstr.expression.size()
# Look for virtual variables
if nvirt > 0:
J_virt = cs.jacobian(cnstr.expression, virtual_var)
if J_virt.nnz() > 0: # if it has non-zero elements
cnstr_expr = J_virt
found_virt = True
virt_ind += 1
else:
cnstr_expr = cs.DM.zeros((expr_size[0], nvirt))
# Setup bounds/functions for numerics
rob_der = cnstr.jtimes(robot_var, robot_vel_var)
lb_cnstr_expr = -cnstr.jacobian(time_var) - rob_der
ub_cnstr_expr = -cnstr.jacobian(time_var) - rob_der
if isinstance(cnstr, EqualityConstraint):
lb_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
ub_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
elif isinstance(cnstr, SetConstraint):
lb_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_min - cnstr.expression)
ub_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_max - cnstr.expression)
elif isinstance(cnstr, VelocityEqualityConstraint):
lb_cnstr_expr += cnstr.target
ub_cnstr_expr += cnstr.target
elif isinstance(cnstr, VelocitySetConstraint):
lb_cnstr_expr += cnstr.set_min
ub_cnstr_expr += cnstr.set_max
# Look for slack variables
if nslack > 0:
slack_mat = cs.DM.zeros((expr_size[0], nslack))
if cnstr.constraint_type == "soft":
slack_mat[:, slack_ind:slack_ind +
expr_size[0]] = -cs.DM.eye(expr_size[0])
slack_ind += expr_size[0]
found_slack = True
if nvirt > 0:
cnstr_expr = cs.horzcat(cnstr_expr, slack_mat)
else:
cnstr_expr = slack_mat
# Only care about this expression if it's actually relevant
if (found_virt or found_slack):
cnstr_expr_list += [cnstr_expr]
lb_cnstr_expr_list += [lb_cnstr_expr]
ub_cnstr_expr_list += [ub_cnstr_expr]
if slack_ind == 0 and virt_ind == 0:
# Didn't find any of them.. return
self._has_initial = False
return None
A_expr = cs.vertcat(*cnstr_expr_list)
Blb_expr = cs.vertcat(*lb_cnstr_expr_list)
Bub_expr = cs.vertcat(*ub_cnstr_expr_list)
currval_vars = [time_var, robot_var, robot_vel_var]
currval_names = ["time_var", "robot_var", "robot_vel_var"]
if self.skill_spec._has_virtual:
currval_vars += [virtual_var]
currval_names += ["virtual_var"]
if self.skill_spec._has_input:
currval_vars += [input_var]
currval_names += ["input_var"]
func_opts = self.options["function_opts"]
self._initial_problem = {
"H":
cs.Function("H_initial", currval_vars, [H_expr], currval_names,
["H"], func_opts),
"A":
cs.Function("A_initial", currval_vars, [A_expr], currval_names,
["A"], func_opts),
"Blb":
cs.Function("Blb_initial", currval_vars, [Blb_expr], currval_names,
["Blb"], func_opts),
"Bub":
cs.Function("Bub_initial", currval_vars, [Bub_expr], currval_names,
["Bub"], func_opts)
}
self.initial_solver = cs.conic("solver", self.options["solver_name"], {
"h": H_expr.sparsity(),
"a": A_expr.sparsity()
}, self.options["initial_solver_opts"])
self._has_initial = True
def solve(self,
time_var,
robot_var,
virtual_var=None,
input_var=None,
warmstart_robot_vel_var=None,
warmstart_virtual_vel_var=None,
warmstart_slack_var=None):
"""Solve the skill specification.
"""
# Useful sizes
nrob = self.skill_spec.n_robot_var
nvirt = self.skill_spec.n_virtual_var
nslack = self.skill_spec.n_slack_var
has_virtual = self.skill_spec._has_virtual
has_input = self.skill_spec._has_input
# Pack current values
currvals = [time_var, robot_var]
if virtual_var is not None and has_virtual:
currvals += [virtual_var]
if input_var is not None and has_input:
currvals += [input_var]
# Get numerics
H = self.H_func(*currvals)
A = self.A_func(*currvals)
Blb = self.Blb_func(*currvals)
Bub = self.Bub_func(*currvals)
# Do we have warmstart?
ws_rob = warmstart_robot_vel_var is not None
ws_virt = warmstart_virtual_vel_var is not None and has_virtual
ws_slack = warmstart_slack_var is not None and nslack > 0
if not (ws_rob or ws_virt or ws_slack):
# If no warmstart, then just calculate results
self.res = self.solver(h=H, a=A, lba=Blb, uba=Bub)
else:
# Pack warmstart vector
warmstart = []
if ws_rob:
warmstart += [warmstart_robot_vel_var]
else:
warmstart += [cs.DM.zeros(nrob)]
if ws_virt:
warmstart += [warmstart_virtual_vel_var]
else:
if nvirt > 0:
warmstart += [cs.DM.zeros(nvirt)]
if ws_slack:
warmstart += [warmstart_slack_var]
else:
if nslack > 0:
warmstart += [cs.DM.zeros(nslack)]
# Calculate results
self.res = self.solver(x0=cs.vertcat(*warmstart),
h=H,
a=A,
lba=Blb,
uba=Bub)
res_robot_vel = self.res["x"][:nrob]
if nvirt > 0 and has_virtual:
res_virtual_vel = self.res["x"][nrob:nrob + nvirt]
else:
res_virtual_vel = None
if nslack > 0:
if not has_virtual:
# handles user error when user adds virtual_var
# but it's not actually in the expressions
nvirt = 0
res_slack = self.res["x"][nrob + nvirt:nrob + nvirt + nslack]
else:
res_slack = None
return res_robot_vel, res_virtual_vel, res_slack
def generate_c_code_external_cost(model, stage_type):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
if stage_type == 'terminal':
suffix_name = "_cost_ext_cost_e_fun"
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
elif stage_type == 'path':
suffix_name = "_cost_ext_cost_fun"
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
elif stage_type == 'initial':
suffix_name = "_cost_ext_cost_0_fun"
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost_0
# set up functions to be exported
fun_name = model.name + suffix_name
fun_name_hess = model.name + suffix_name_hess
fun_name_jac = model.name + suffix_name_jac
# generate expression for full gradient and Hessian
full_hess, grad = hessian(ext_cost, vertcat(u, x))
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(fun_name_hess, [x, u, p],
[ext_cost, grad, full_hess])
ext_cost_fun_jac = Function(fun_name_jac, [x, u, p], [ext_cost, grad])
# generate C code
if not os.path.exists("c_generated_code"):
os.mkdir("c_generated_code")
os.chdir("c_generated_code")
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir("../..")
return
# Adjust the relevant constraints.
for t in range(Nt):
varlb["u",t,:] = ulb
varub["u",t,:] = uub
# Now build up constraints and objective.
obj = casadi.SX(0)
con = []
for t in range(Nt):
con.append(ode_rk4_casadi(var["x",t],
var["u",t]) - var["x",t+1])
obj += l(var["x",t], var["u",t])
obj += Pf(var["x",Nt])
# Build solver object.
con = casadi.vertcat(*con)
conlb = np.zeros((Nx*Nt,))
conub = np.zeros((Nx*Nt,))
nlp = dict(x=var, f=obj, g=con)
nlpoptions = {
"ipopt" : {
"print_level" : 0,
"max_cpu_time" : 60,
"linear_solver" : "ma27", # Comment this line if you don't have MA27
"max_iter" : 100,
},
"print_time" : False,
}
solver = casadi.nlpsol("solver",
import casadi
import numpy as np
import matplotlib.pyplot as plt
CONSTANT_ACCELERATION = 0.1
TF = 10*np.pi
t = casadi.SX.sym('t')
q = casadi.SX.sym('q')
qd = casadi.SX.sym('qd')
state = casadi.vertcat(q, qd)
rhs = casadi.vertcat(qd, casadi.sin(t))
dae = {'x': state, 't': t, 'ode': rhs}
ts = np.linspace(0, TF, 10000)
integrator = casadi.integrator('integrator', 'cvodes', dae, {'grid':ts, 'output_t0':True})
sol = integrator(x0=[0, -1.0])
import pdb
# pdb.set_trace()
sol_q = np.array(sol['xf'][0, :]).flatten()
sol_qd = np.array(sol['xf'][1, :]).flatten()
plt.plot(ts, sol_q, 'r-', label='Position')
[2.0, 3.5, 0.0, 0.5, 0.3],
[3.5, 1.5, ca.pi, 0.7, 0.2],
[2.0, 2.0, -ca.pi, 0.6, 0.3]])
n_MO = len(MO_init[:, 0])
SO_init = np.array([[1.0, 3.0, 0.3],
[9.0, 1.5, 0.1],
[2.0, 2.0, 0.3],
[6.0, 2.5, 0.2]])
n_SO = len(SO_init[:, 0])
# System Model
x = ca.SX.sym('x')
y = ca.SX.sym('y')
theta = ca.SX.sym('theta')
states = ca.vertcat(x, y, theta)
n_states = 3 # len([states])
# Control system
v = ca.SX.sym('v')
omega = ca.SX.sym('omega')
controls = ca.vertcat(v, omega)
n_controls = 2 # len([controls])
rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)
# Obstacle states in each predictions
MOx = ca.SX.sym('MOx')
MOy = ca.SX.sym('MOy')
MOth = ca.SX.sym('MOth')
MOv = ca.SX.sym('MOv')
def gen_long_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_long_model()
Tf = T_IDXS[-1]
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
QR = np.zeros((COST_DIM, COST_DIM))
Q = np.zeros((COST_E_DIM, COST_E_DIM))
ocp.cost.W = QR
ocp.cost.W_e = Q
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
j_ego = ocp.model.u[0]
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
desired_dist_comfort = get_safe_obstacle_distance(v_ego)
# The main cost in normal operation is how close you are to the "desired" distance
# from an obstacle at every timestep. This obstacle can be a lead car
# or other object. In e2e mode we can use x_position targets as a cost
# instead.
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
x_ego, v_ego, a_ego, 20 * (a_ego - prev_a), j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an assymetrical cost.
constraints = vertcat((v_ego), (a_ego - a_min), (a_max - a_ego),
((x_obstacle - x_ego) - (3 / 4) *
(desired_dist_comfort)) / (v_ego + 10.))
ocp.model.con_h_expr = constraints
ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM))
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0])
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np.zeros(CONSTR_DIM)
ocp.cost.zl = cost_weights
ocp.cost.Zl = cost_weights
ocp.cost.Zu = cost_weights
ocp.cost.zu = cost_weights
ocp.constraints.lh = np.zeros(CONSTR_DIM)
ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
ocp.constraints.uh = 1e4 * np.ones(CONSTR_DIM)
ocp.constraints.uh_e = 1e4 * np.ones(CONSTR_DIM)
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where the compute time is strictly bounded
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
# does not cause issues since the problem is well bounded.
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver_cond_N = N // 4
# More iterations take too much time and less lead to inaccurate convergence in
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
ocp.solver_options.qp_solver_iter_max = 10
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = T_IDXS
ocp.code_export_directory = EXPORT_DIR
return ocp
def system_dynamics(x, u):
return casadi.vertcat(x[2], x[3], u[0], u[1])
def steer(self, from_node, to_node):
if self.mode == 'mpc':
# steer tree to desired node (using 1. MPC 2. LMPC 3. Feedback policy)
horizon = 5
temp_node = Node(self.node_arr[from_node].state)
dist = self.get_dist(temp_node, to_node)
n = self.env.observation_space.shape[0]
d = self.env.action_space.shape[0]
# while(dist >= 0.1):
# define variables
x = ca.SX.sym('x', (horizon + 1) * n)
u = ca.SX.sym('u', horizon * d)
# define cost
cost = 0
for i in range(horizon):
cost += ca.norm_2(x[(i + 1) * n:(i + 2) * n] - to_node.state)
# define constraints
current_state = temp_node.state
constraints = []
for j in range(horizon):
next_state = (self.env.A @ (current_state)) + (
self.env.B @ (u[j * d:(j + 1) * d]))
constraints = ca.vertcat(constraints,
x[(j + 1) * n + 0] == next_state[0])
constraints = ca.vertcat(constraints,
x[(j + 1) * n + 1] == next_state[1])
constraints = ca.vertcat(constraints,
x[(j + 1) * n + 2] == next_state[2])
constraints = ca.vertcat(constraints,
x[(j + 1) * n + 3] == next_state[3])
lbg = [0] * (horizon) * n
ubg = [0] * (horizon) * n
# solve
opts = {'verbose': False, 'ipopt.print_level': 0, 'print_time': 0}
nlp = {'x': ca.vertcat(x, u), 'f': cost, 'g': constraints}
solver = ca.nlpsol('solver', 'ipopt', nlp, opts)
lbx = current_state.tolist(
) + [-100] * n * horizon + [-1] * d * horizon
ubx = current_state.tolist() + [100
] * n * horizon + [1] * d * horizon
sol = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
sol_val = np.array(sol['x'])
u_star = sol_val[(horizon + 1) * n:(horizon + 1) * n + d]
new_node = Node()
new_node.state = self.env._next_state(current_state,
u_star.reshape(-1))
new_node.parent = self.node_arr[from_node]
if self.mode == 'lqr':
n = self.env.observation_space.shape[0]
d = self.env.action_space.shape[0]
# get lqr gains
Q = np.eye(n)
R = 50 * np.eye(d)
A = self.env.A
B = self.env.B
P = la.solve_discrete_are(A, B, Q, R)
Ks = -np.linalg.inv(R + B.T.dot(P).dot(B)).dot(B.T).dot(P).dot(A)
# obtain optimal control action
start_state = self.node_arr[from_node].state
goal_state = to_node.state
u_star = np.dot(Ks, (start_state - goal_state))
u_star = np.clip(u_star, -1, 1)
# obtain new node
new_node = Node()
if len(self.node_arr[from_node].state_arr) == 0:
self.node_arr[from_node].state_arr.append(start_state)
# for j in range(len(self.node_arr[from_node].state_arr)):
for i in range(self.n_states):
new_node.state_arr.append(
self.env._next_state(start_state, u_star))
new_node.state = sum(new_node.state_arr) / len(new_node.state_arr)
new_node.parent = self.node_arr[from_node]
new_node.cost = self.node_arr[from_node].cost + 1
return new_node
def _run_integrator(self,
model,
y0,
inputs_dict,
inputs,
t_eval,
use_grid=True):
pybamm.logger.debug("Running CasADi integrator")
if use_grid is True:
t_eval_shifted = t_eval - t_eval[0]
t_eval_shifted_rounded = np.round(t_eval_shifted,
decimals=12).tobytes()
integrator = self.integrators[model][t_eval_shifted_rounded]
else:
integrator = self.integrators[model]["no grid"]
len_rhs = model.concatenated_rhs.size
y0_diff = y0[:len_rhs]
y0_alg = y0[len_rhs:]
try:
# Try solving
if use_grid is True:
t_min = t_eval[0]
inputs_with_tmin = casadi.vertcat(inputs, t_min)
# Call the integrator once, with the grid
timer = pybamm.Timer()
casadi_sol = integrator(x0=y0_diff,
z0=y0_alg,
p=inputs_with_tmin,
**self.extra_options_call)
integration_time = timer.time()
y_sol = casadi.vertcat(casadi_sol["xf"], casadi_sol["zf"])
sol = pybamm.Solution(t_eval, y_sol, model, inputs_dict)
sol.integration_time = integration_time
return sol
else:
# Repeated calls to the integrator
x = y0_diff
z = y0_alg
y_diff = x
y_alg = z
for i in range(len(t_eval) - 1):
t_min = t_eval[i]
t_max = t_eval[i + 1]
inputs_with_tlims = casadi.vertcat(inputs, t_min, t_max)
timer = pybamm.Timer()
casadi_sol = integrator(x0=x,
z0=z,
p=inputs_with_tlims,
**self.extra_options_call)
integration_time = timer.time()
x = casadi_sol["xf"]
z = casadi_sol["zf"]
y_diff = casadi.horzcat(y_diff, x)
if not z.is_empty():
y_alg = casadi.horzcat(y_alg, z)
if z.is_empty():
sol = pybamm.Solution(t_eval, y_diff, model, inputs_dict)
else:
y_sol = casadi.vertcat(y_diff, y_alg)
sol = pybamm.Solution(t_eval, y_sol, model, inputs_dict)
sol.integration_time = integration_time
return sol
except RuntimeError as e:
# If it doesn't work raise error
raise pybamm.SolverError(e.args[0])
def model():
#===============================================================================
# Variable Assignments
#===============================================================================
MP = cs.ssym("MP")
MC = cs.ssym("MC")
MB = cs.ssym("MB")
PC = cs.ssym("PC")
CC = cs.ssym("CC")
PCP = cs.ssym("PCP")
CCP = cs.ssym("CCP")
PCC = cs.ssym("PCC")
PCN = cs.ssym("PCN")
PCCP = cs.ssym("PCCP")
PCNP = cs.ssym("PCNP")
BC = cs.ssym("BC")
BCP = cs.ssym("BCP")
BN = cs.ssym("BN")
BNP = cs.ssym("BNP")
IN = cs.ssym("IN")
y = [MP,MC,MB,PC,CC,PCP,CCP,PCC,PCN,PCCP,PCNP,BC,BCP,BN,BNP,IN]
#=======================================================================
# Parameter Assignments
#=======================================================================
k1 = cs.ssym("k1")
k2 = cs.ssym("k2")
k3 = cs.ssym("k3")
k4 = cs.ssym("k4")
k5 = cs.ssym("k5")
k6 = cs.ssym("k6")
k7 = cs.ssym("k7")
k8 = cs.ssym("k8")
KAP = cs.ssym("KAP")
KAC = cs.ssym("KAC")
KIB = cs.ssym("KIB")
kdmb = cs.ssym("kdmb")
kdmc = cs.ssym("kdmc")
kdmp = cs.ssym("kdmp")
kdn = cs.ssym("kdn")
kdnc = cs.ssym("kdnc")
Kd = cs.ssym("Kd")
Kdp = cs.ssym("Kdp")
Kp = cs.ssym("Kp")
KmB = cs.ssym("KmB")
KmC = cs.ssym("KmC")
KmP = cs.ssym("KmP")
ksB = cs.ssym("ksB")
ksC = cs.ssym("ksC")
ksP = cs.ssym("ksP")
m = cs.ssym("m")
n = cs.ssym("n")
V1B = cs.ssym("V1B")
V1C = cs.ssym("V1C")
V1P = cs.ssym("V1P")
V1PC = cs.ssym("V1PC")
V2B = cs.ssym("V2B")
V2C = cs.ssym("V2C")
V2P = cs.ssym("V2P")
V2PC = cs.ssym("V2PC")
V3B = cs.ssym("V3B")
V3PC = cs.ssym("V3PC")
V4B = cs.ssym("V4B")
V4PC = cs.ssym("V4PC")
vdBC = cs.ssym("vdBC")
vdBN = cs.ssym("vdBN")
vdCC = cs.ssym("vdCC")
vdIN = cs.ssym("vdIN")
vdPC = cs.ssym("vdPC")
vdPCC = cs.ssym("vdPCC")
vdPCN = cs.ssym("vdPCN")
vmB = cs.ssym("vmB")
vmC = cs.ssym("vmC")
vmP = cs.ssym("vmP")
vsB = cs.ssym("vsB")
vsC = cs.ssym("vsC")
vsP = cs.ssym("vsP")
p = [k1, k2, k3, k4, k5, k6, k7, k8, KAP, KAC, KIB, kdmb, kdmc, kdmp, kdn, kdnc, Kd, Kdp, Kp, KmB, KmC, KmP, ksB, ksC, ksP, m, n, V1B, V1C, V1P, V1PC, V2B, V2C, V2P, V2PC, V3B, V3PC, V4B, V4PC, vdBC, vdBN, vdCC, vdIN, vdPC, vdPCC, vdPCN, vmB, vmC, vmP, vsB, vsC, vsP]
# Time Variable
t = cs.ssym("t")
ode = [[]]*NEQ
# /* mRNA of per */
ode[0] = ((1 + 0.5*cs.sin(2*np.pi*t/24.))*vsP*pow(BN,n)/(pow(KAP,n) + pow(BN,n)) - vmP*MP/(KmP + MP) - kdmp*MP)
# /* mRNA of cry */
ode[1] = (vsC*pow(BN,n)/(pow(KAC,n) + pow(BN,n)) - vmC*MC/(KmC + MC) - kdmc*MC)
# /* mRNA of BMAL1 */
ode[2] = (vsB*pow(KIB,m)/(pow(KIB,m) + pow(BN,m)) - vmB*MB/(KmB + MB) - kdmb *MB)
# /* protein PER cytosol */
ode[3] = (ksP*MP - V1P*PC/(Kp + PC) + V2P*PCP/(Kdp + PCP) + k4*PCC - k3*PC*CC - kdn*PC)
# /* protein CRY cytosol */
ode[4] = (ksC*MC - V1C*CC/(Kp + CC) + V2C*CCP/(Kdp + CCP) + k4*PCC - k3*PC*CC - kdnc*CC)
# /* phosphorylated PER cytosol */
ode[5] = (V1P*PC/(Kp + PC) - V2P*PCP/(Kdp + PCP) - vdPC*PCP/(Kdp + PCP) - kdn*PCP)
# /* phosphorylated CRY cytosol */
ode[6] = (V1C*CC/(Kp + CC) - V2C*CCP/(Kdp + CCP) - vdCC*CCP/(Kd + CCP) - kdn*CCP)
# /* PER:CRY complex cytosol */
ode[7] = ( - V1PC*PCC/(Kp + PCC) + V2PC*PCCP/(Kdp + PCCP) - k4*PCC + k3*PC*CC + k2*PCN - k1*PCC - kdn*PCC)
# /* PER:CRY complex nucleus */
ode[8] = ( - V3PC*PCN/(Kp + PCN) + V4PC*PCNP/(Kdp + PCNP) - k2*PCN + k1*PCC - k7*BN*PCN + k8*IN - kdn*PCN)
# /* phopshorylated [PER:CRY)c cytosol */
ode[9] = (V1PC*PCC/(Kp + PCC) - V2PC*PCCP/(Kdp + PCCP) - vdPCC*PCCP/(Kd + PCCP) - kdn*PCCP)
# /* phosphorylated [PER:CRY)n */
ode[10] = (V3PC*PCN/(Kp + PCN) - V4PC*PCNP/(Kdp + PCNP) - vdPCN*PCNP/(Kd + PCNP) - kdn*PCNP)
# /* protein BMAL1 cytosol */
ode[11] = (ksB*MB - V1B*BC/(Kp + BC) + V2B*BCP/(Kdp + BCP) - k5*BC + k6*BN - kdn*BC)
# /* phosphorylated BMAL1 cytosol */
ode[12] = (V1B*BC/(Kp + BC) - V2B*BCP/(Kdp + BCP) - vdBC*BCP/(Kd + BCP) - kdn*BCP)
# /* protein BMAL1 nucleus */
ode[13] = ( - V3B*BN/(Kp + BN) + V4B*BNP/(Kdp + BNP) + k5*BC - k6*BN - k7*BN*PCN + k8*IN - kdn*BN)
# /* phosphorylatd BMAL1 nucleus */
ode[14] = (V3B*BN/(Kp + BN) - V4B*BNP/(Kdp + BNP) - vdBN*BNP/(Kd + BNP) - kdn*BNP)
# /* inactive complex between [PER:CRY)n abd [CLOCK:BMAL1)n */
ode[15] = ( - k8*IN + k7*BN*PCN - vdIN*IN/(Kd + IN) - kdn*IN)
fn = cs.SXFunction(cs.daeIn(t=t, x=cs.vertcat(y), p=cs.vertcat(p)),
cs.daeOut(ode=cs.vertcat(ode)))
fn.setOption("name","Leloup 16")
return fn
def _solve_for_event(self, coarse_solution, init_event_signs):
"""
Check if the sign of an event changes, if so find an accurate
termination point and exit
Locate the event time using a root finding algorithm and
event state using interpolation. The solution is then truncated
so that only the times up to the event are returned
"""
pybamm.logger.debug("Solving for events")
model = coarse_solution.all_models[-1]
inputs_dict = coarse_solution.all_inputs[-1]
inputs = casadi.vertcat(*[x for x in inputs_dict.values()])
def find_t_event(sol, typ):
# Check most recent y to see if any events have been crossed
if model.terminate_events_eval:
y_last = sol.all_ys[-1][:, -1]
crossed_events = np.sign(init_event_signs * np.concatenate([
event(sol.t[-1], y_last, inputs)
for event in model.terminate_events_eval
]) - 1e-5)
else:
crossed_events = np.sign([])
# Return None if no events have been triggered
if (crossed_events == 1).all():
return None, None
# get the index of the events that have been crossed
event_ind = np.where(crossed_events != 1)[0]
active_events = [model.terminate_events_eval[i] for i in event_ind]
# loop over events to compute the time at which they were triggered
t_events = [None] * len(active_events)
event_idcs_lower = [None] * len(active_events)
for i, event in enumerate(active_events):
# Implement our own bisection algorithm for speed
# This is used to find the time range in which the event is triggered
# Evaluations of the "event" function are (relatively) expensive
init_event_sign = init_event_signs[event_ind[i]][0]
f_eval = {}
def f(idx):
try:
return f_eval[idx]
except KeyError:
# We take away 1e-5 to deal with the case where the event sits
# exactly on zero, as can happen when the event switch is used
# (fast with events mode)
f_eval[idx] = (
init_event_sign *
event(sol.t[idx], sol.y[:, idx], inputs) - 1e-5)
return f_eval[idx]
def integer_bisect():
a_n = 0
b_n = len(sol.t) - 1
for _ in range(len(sol.t)):
if a_n + 1 == b_n:
return a_n
m_n = (a_n + b_n) // 2
f_m_n = f(m_n)
if np.isnan(f_m_n):
a_n = a_n
b_n = m_n
elif f_m_n < 0:
a_n = a_n
b_n = m_n
elif f_m_n > 0:
a_n = m_n
b_n = b_n
event_idx_lower = integer_bisect()
if typ == "window":
event_idcs_lower[i] = event_idx_lower
elif typ == "exact":
# Linear interpolation between the two indices to find the root time
# We could do cubic interpolation here instead but it would be
# slower
t_lower = sol.t[event_idx_lower]
t_upper = sol.t[event_idx_lower + 1]
event_lower = abs(f(event_idx_lower))
event_upper = abs(f(event_idx_lower + 1))
t_events[i] = (event_lower * t_upper + event_upper *
t_lower) / (event_lower + event_upper)
if typ == "window":
event_idx_lower = np.nanmin(event_idcs_lower)
return event_idx_lower, None
elif typ == "exact":
# t_event is the earliest event triggered
t_event = np.nanmin(t_events)
# create interpolant to evaluate y in the current integration
# window
y_sol = interp1d(sol.t, sol.y, kind="linear")
y_event = y_sol(t_event)
return t_event, y_event
# Find the interval in which the event was triggered
event_idx_lower, _ = find_t_event(coarse_solution, "window")
# Return the existing solution if no events have been triggered
if event_idx_lower is None:
# Flag "final time" for termination
self.check_interpolant_extrapolation(model, coarse_solution)
coarse_solution.termination = "final time"
return coarse_solution
# If events have been triggered, we solve for a dense window in the interval
# where the event was triggered, then find the precise location of the event
# Solve again with a more dense idx_window, starting from the start of the
# window where the event was triggered
t_window_event_dense = np.linspace(
coarse_solution.t[event_idx_lower],
coarse_solution.t[event_idx_lower + 1],
100,
)
if self.mode == "safe without grid":
use_grid = False
else:
self.create_integrator(model, inputs, t_window_event_dense)
use_grid = True
y0 = coarse_solution.y[:, event_idx_lower]
dense_step_sol = self._run_integrator(model,
y0,
inputs_dict,
inputs,
t_window_event_dense,
use_grid=use_grid)
# Find the exact time at which the event was triggered
t_event, y_event = find_t_event(dense_step_sol, "exact")
# If this returns None, no event was crossed in dense_step_sol. This can happen
# if the event crossing was right at the end of the interval in the coarse
# solution. In this case, return the t and y from the end of the interval
# (i.e. next point in the coarse solution)
if y_event is None: # pragma: no cover
# This is extremely rare, it's difficult to find a test that triggers this
# hence no coverage check
t_event = coarse_solution.t[event_idx_lower + 1]
y_event = coarse_solution.y[:,
event_idx_lower + 1].full().flatten()
# Return solution truncated at the first coarse event time
# Also assign t_event
t_sol = coarse_solution.t[:event_idx_lower + 1]
y_sol = coarse_solution.y[:, :event_idx_lower + 1]
solution = pybamm.Solution(
t_sol,
y_sol,
model,
inputs_dict,
np.array([t_event]),
y_event[:, np.newaxis],
"event",
)
solution.integration_time = (coarse_solution.integration_time +
dense_step_sol.integration_time)
self.check_interpolant_extrapolation(model, solution)
return solution
def setup(self, bending_BC_type="cantilevered"):
"""
Sets up the problem. Run this last.
:return: None (in-place)
"""
### Discretize and assign loads
# Discretize
point_load_locations = [load["location"] for load in self.point_loads]
point_load_locations.insert(0, 0)
point_load_locations.append(self.length)
self.x = cas.vertcat(*[
cas.linspace(point_load_locations[i], point_load_locations[i + 1],
self.points_per_point_load)
for i in range(len(point_load_locations) - 1)
])
# Post-process the discretization
self.n = self.x.shape[0]
dx = cas.diff(self.x)
# Add point forces
self.point_forces = cas.GenMX_zeros(self.n - 1)
for i in range(len(self.point_loads)):
load = self.point_loads[i]
self.point_forces[self.points_per_point_load * (i + 1) -
1] = load["force"]
# Add distributed loads
self.force_per_unit_length = cas.GenMX_zeros(self.n)
self.moment_per_unit_length = cas.GenMX_zeros(self.n)
for load in self.distributed_loads:
if load["type"] == "uniform":
self.force_per_unit_length += load["force"] / self.length
elif load["type"] == "point":
pass
else:
raise ValueError(
"Bad value of \"type\" for a load within beam.loads!")
# Initialize optimization variables
log_nominal_diameter = self.opti.variable(self.n)
self.opti.set_initial(log_nominal_diameter,
cas.log(self.diameter_guess))
self.nominal_diameter = cas.exp(log_nominal_diameter)
self.opti.subject_to([log_nominal_diameter > cas.log(self.thickness)])
def trapz(x):
out = (x[:-1] + x[1:]) / 2
# out[0] += x[0] / 2
# out[-1] += x[-1] / 2
return out
# Mass
self.volume = cas.sum1(
cas.pi / 4 * trapz((self.nominal_diameter + self.thickness)**2 -
(self.nominal_diameter - self.thickness)**2) *
dx)
self.mass = self.volume * self.density
# Mass proxy
self.volume_proxy = cas.sum1(cas.pi * trapz(self.nominal_diameter) *
dx * self.thickness)
self.mass_proxy = self.volume_proxy * self.density
# Find moments of inertia
self.I = cas.pi / 64 * ( # bending
(self.nominal_diameter + self.thickness)**4 -
(self.nominal_diameter - self.thickness)**4)
self.J = cas.pi / 32 * ( # torsion
(self.nominal_diameter + self.thickness)**4 -
(self.nominal_diameter - self.thickness)**4)
if self.bending:
# Set up derivatives
self.u = 1 * self.opti.variable(self.n)
self.du = 0.1 * self.opti.variable(self.n)
self.ddu = 0.01 * self.opti.variable(self.n)
self.dEIddu = 1 * self.opti.variable(self.n)
self.opti.set_initial(self.u, 0)
self.opti.set_initial(self.du, 0)
self.opti.set_initial(self.ddu, 0)
self.opti.set_initial(self.dEIddu, 0)
# Define derivatives
self.opti.subject_to([
cas.diff(self.u) == trapz(self.du) * dx,
cas.diff(self.du) == trapz(self.ddu) * dx,
cas.diff(self.E * self.I *
self.ddu) == trapz(self.dEIddu) * dx,
cas.diff(
self.dEIddu) == trapz(self.force_per_unit_length) * dx +
self.point_forces,
])
# Add BCs
if bending_BC_type == "cantilevered":
self.opti.subject_to([
self.u[0] == 0,
self.du[0] == 0,
self.ddu[-1] == 0, # No tip moment
self.dEIddu[-1] == 0, # No tip higher order stuff
])
else:
raise ValueError("Bad value of bending_BC_type!")
# Stress
self.stress_axial = (self.nominal_diameter +
self.thickness) / 2 * self.E * self.ddu
if self.torsion:
# Set up derivatives
phi = 0.1 * self.opti.variable(self.n)
dphi = 0.01 * self.opti.variable(self.n)
# Add forcing term
ddphi = -self.moment_per_unit_length / (self.G * self.J)
self.stress = self.stress_axial
self.opti.subject_to([
self.stress / self.max_allowable_stress < 1,
self.stress / self.max_allowable_stress > -1,
])
def _simulate_with_casadi_with_inputs(self, initcon, tsim, varying_inputs,
integrator, integrator_options):
xalltemp = [self._templatemap[i] for i in self._diffvars]
xall = casadi.vertcat(*xalltemp)
time = casadi.SX.sym('time')
odealltemp = [
time * convert_pyomo2casadi(self._rhsdict[i])
for i in self._derivlist
]
odeall = casadi.vertcat(*odealltemp)
# Time-varying inputs
ptemp = [self._templatemap[i] for i in self._siminputvars.values()]
pall = casadi.vertcat(time, *ptemp)
dae = {'x': xall, 'p': pall, 'ode': odeall}
if len(self._algvars) != 0:
zalltemp = [self._templatemap[i] for i in self._simalgvars]
zall = casadi.vertcat(*zalltemp)
# Need to do anything special with time scaling??
algalltemp = [convert_pyomo2casadi(i) for i in self._alglist]
algall = casadi.vertcat(*algalltemp)
dae['z'] = zall
dae['alg'] = algall
integrator_options['tf'] = 1.0
F = casadi.integrator('F', integrator, dae, integrator_options)
N = len(tsim)
# This approach removes the time scaling from tsim so must
# create an array with the time step between consecutive
# time points
tsimtemp = np.hstack([0, tsim[1:] - tsim[0:-1]])
tsimtemp.shape = (1, len(tsimtemp))
palltemp = [casadi.DM(tsimtemp)]
# Need a similar np array for each time-varying input
for p in self._siminputvars.keys():
profile = varying_inputs[p]
tswitch = list(profile.keys())
tswitch.sort()
tidx = [tsim.searchsorted(i) for i in tswitch] + \
[len(tsim) - 1]
ptemp = [profile[0]] + \
[casadi.repmat(profile[tswitch[i]], 1,
tidx[i + 1] - tidx[i])
for i in range(len(tswitch))]
temp = casadi.horzcat(*ptemp)
palltemp.append(temp)
I = F.mapaccum('simulator', N)
sol = I(x0=initcon, p=casadi.vertcat(*palltemp))
profile = sol['xf'].full().T
if len(self._algvars) != 0:
algprofile = sol['zf'].full().T
profile = np.concatenate((profile, algprofile), axis=1)
return [tsim, profile]
def generate_safety_constraints(self, p_all, q_all, u_0, k_fb_ctrl,
k_ff_all):
""" Generate all safety constraints
Parameters
----------
p_all:
q_all:
k_fb_0:
k_fb_ctrl:
k_ff:
ctrl_bounds:
Returns
-------
g: list[casadi.SX]
lbg: list[casadi.SX]
ubg: list[casadi.SX]
"""
g = []
lbg = []
ubg = []
g_name = []
H = np.shape(p_all)[0]
# control constraints
if self.has_ctrl_bounds:
g_u_0, lbg_u_0, ubg_u_0 = self._generate_control_constraint(u_0)
g = vertcat(g, g_u_0)
lbg += lbg_u_0
ubg += ubg_u_0
g_name += ["u_0_ctrl_constraint"]
for i in range(H - 1):
p_i = p_all[i, :].T
q_i = q_all[i, :].reshape((self.n_s, self.n_s))
k_ff_i = k_ff_all[i, :].reshape((self.n_u, 1))
k_fb_i = k_fb_ctrl
g_u_i, lbg_u_i, ubg_u_i = self._generate_control_constraint(k_ff_i, q_i,
k_fb_i)
g = vertcat(g, g_u_i)
lbg += lbg_u_i
ubg += ubg_u_i
g_name += ["ellipsoid_ctrl_constraint_{}".format(i)] * len(lbg_u_i)
# intermediate state constraints
if not self.h_mat_obs is None:
for i in range(H - 1):
p_i = p_all[i, :].T
q_i = q_all[i, :].reshape((self.n_s, self.n_s))
g_state = lin_ellipsoid_safety_distance(p_i, q_i, self.h_mat_obs,
self.h_obs)
g = vertcat(g, g_state)
lbg += [-cas.inf] * self.m_obs
ubg += [0] * self.m_obs
g_name += ["obstacle_avoidance_constraint{}".format(i)] * self.m_obs
# terminal state constraint
p_T = p_all[-1, :].T
q_T = q_all[-1, :].reshape((self.n_s, self.n_s))
g_terminal = lin_ellipsoid_safety_distance(p_T, q_T, self.h_mat_safe,
self.h_safe)
g = vertcat(g, g_terminal)
g_name += ["terminal constraint"] * self.m_safe
lbg += [-cas.inf] * self.m_safe
ubg += [0] * self.m_safe
return g, lbg, ubg, g_name
# In[103]:
# Build the constraints
g = equations
lbg = [0.0] * len(g)
ubg = lbg.copy()
# Build the objective
f = 0.0
for n in nodes.values():
f += n.objective**2
# In[104]:
# Construct the qp, and solver
qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)}
solver = ca.qpsol('qp', 'cplex', qp, {})
# In[105]:
results = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
# In[112]:
total_shortage = 0
max_shortage = 0
i = 0
for n in nodes.values():
if n.q_control is not None:
def init_solver(self, cost_func=None):
""" Generate the exloration NLP in casadi
Parameters
----------
T: int, optional
the safempc horizon
"""
u_0 = MX.sym("init_control", (self.n_u, 1))
k_ff_all = MX.sym("feed-forward control", (self.T - 1, self.n_u))
g = []
lbg = []
ubg = []
g_name = []
p_0 = MX.sym("initial state", (self.n_s, 1))
k_fb_safe_ctrl = MX.sym("Feedback term", (self.n_u, self.n_s))
p_all, q_all, gp_sigma_pred_safe_all = cas_multistep(p_0, u_0,
k_fb_safe_ctrl, k_ff_all,
self.gp.get_forward_model_casadi(True), self.l_mu,
self.l_sigma,
self.beta_safety, self.a,
self.b,
self.lin_trafo_gp_input)
# generate open_loop trajectory function [vertcat(x_0,u_0)],[f_x])
self.f_multistep_eval = cas.Function("safe_multistep",
[p_0, u_0, k_fb_safe_ctrl, k_ff_all],
[p_all, q_all])
g_safe, lbg_safe, ubg_safe, g_names_safe = self.generate_safety_constraints(
p_all, q_all, u_0, k_fb_safe_ctrl, k_ff_all)
g = vertcat(g, g_safe)
lbg += lbg_safe
ubg += ubg_safe
g_name += g_names_safe
if cost_func is None:
cost = -sum1(sum2(gp_sigma_pred_safe_all))
else:
cost = cost_func(p_all, q_all, gp_sigma_pred_safe_all, u_0, k_ff_all)
opt_vars = vertcat(p_0, u_0, k_ff_all.reshape((-1, 1)))
opt_params = vertcat(k_fb_safe_ctrl.reshape((-1, 1)))
prob = {'f': cost, 'x': opt_vars, 'p': opt_params, 'g': g}
opt = {'error_on_fail': False,
'ipopt': {'hessian_approximation': 'exact', "max_iter": 120,
"expect_infeasible_problem": "no", \
'acceptable_tol': 1e-4, "acceptable_constr_viol_tol": 1e-5,
"bound_frac": 0.5, "start_with_resto": "no",
"required_infeasibility_reduction": 0.85,
"acceptable_iter": 8}} # ipopt
# opt = {'qpsol':'qpoases','max_iter':120,'hessian_approximation':'exact'}#,"c1":5e-4} #sqpmethod #,'hessian_approximation':'limited-memory'
# opt = {'max_iter':120,'qpsol':'qpoases'}
self.solver = cas.nlpsol('solver', 'ipopt', prob, opt)
self.lbg = lbg
self.ubg = ubg
def get_derivative(self, s):
# Case 1: s is a constant, e.g. MX(5)
if ca.MX(s).is_constant():
return 0
# Case 2: s is a symbol, e.g. MX(x)
elif s.is_symbolic():
if s.name() not in self.derivative:
if len(self.for_loops
) > 0 and s in self.for_loops[-1].indexed_symbols:
# Create a new indexed symbol, referencing to the for loop index inside the vector derivative symbol.
for_loop_symbol = self.for_loops[-1].indexed_symbols[s]
s_without_index = self.get_mx(
ast.ComponentRef(name=for_loop_symbol.tree.name))
der_s_without_index = self.get_derivative(s_without_index)
if ca.MX(der_s_without_index).is_symbolic():
return self.get_indexed_symbol(
ast.ComponentRef(
name=der_s_without_index.name(),
indices=for_loop_symbol.tree.indices),
der_s_without_index)
else:
return 0
else:
der_s = _new_mx("der({})".format(s.name()), s.size())
# If the derivative contains an expression (e.g. der(x + y)) this method is
# called with MX variables that are the result of a ca.symvar call. This
# ca.symvar call strips the _modelica_shape field from the MX variable,
# therefore we need to find the original MX to get the modelica shape.
der_s._modelica_shape = \
self.nodes[self.current_class][s.name()]._modelica_shape
self.derivative[s.name()] = der_s
self.nodes[self.current_class][der_s.name()] = der_s
return der_s
else:
return self.derivative[s.name()]
# Case 3: s is an already indexed symbol, e.g. MX(x[1])
elif s.is_op(ca.OP_GETNONZEROS) and s.dep().is_symbolic():
slice_info = s.info()['slice']
dep = s.dep()
if dep.name() not in self.derivative:
der_dep = _new_mx("der({})".format(dep.name()), dep.size())
der_dep._modelica_shape = \
self.nodes[self.current_class][dep.name()]._modelica_shape
self.derivative[dep.name()] = der_dep
self.nodes[self.current_class][der_dep.name()] = der_dep
return der_dep[
slice_info['start']:slice_info['stop']:slice_info['step']]
else:
return self.derivative[dep.name(
)][slice_info['start']:slice_info['stop']:slice_info['step']]
# Case 4: s is an expression that requires differentiation, e.g. MX(x2 * x2)
# Need to do this sort of expansion: der(x1 * x2) = der(x1) * x2 + x1 * der(x2)
else:
# Differentiate expression using CasADi
orig_deps = ca.symvar(s)
deps = ca.vertcat(*orig_deps)
J = ca.Function('J', [deps], [ca.jacobian(s, deps)])
J_sparsity = J.sparsity_out(0)
der_deps = [
self.get_derivative(dep)
if J_sparsity.has_nz(0, j) else ca.DM.zeros(dep.size())
for j, dep in enumerate(orig_deps)
]
return ca.mtimes(J(deps), ca.vertcat(*der_deps))
sint = ca.sin( x[1] )
cost = ca.cos( x[1] )
fric = mic * ca.sign( x[2] )
num = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l )
denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) )
ddtheta = num / denom
ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp)
x_err = x-x_target
cost_mat = np.diag( [1,1,1,1,0] )
ode = ca.vertcat([ ddtheta, \
x[0], \
ddx, \
x[2], \
ca.mul( [ x_err.T, cost_mat, x_err ] )
] )
dae = ca.SXFunction( 'dae', ca.daeIn( x=x, p=u, t=t ), ca.daeOut( ode=ode ) )
# Create an integrator
opts = { 'tf': tf/nk } # final time
if coll:
opts[ 'number_of_finite_elements' ] = 5
opts['interpolation_order'] = 5
opts['collocation_scheme'] = 'legendre'
opts['implicit_solver'] = 'kinsol'
opts['implicit_solver_options'] = {'linear_solver' : 'csparse'}
opts['expand_f'] = True
integrator = ca.Integrator('integrator', 'oldcollocation', dae, opts)
def permute(x: SYM, perm: List[int]) -> SYM:
"""Perumute a vector"""
x_s = []
for i in perm:
x_s.append(x[i])
return ca.vertcat(*x_s)
def __set_costs(self, ocp):
# set weight for states and controls (default: 1.00)
# Q = 1.00 * np.eye(self.acados_ocp.dims.nx)
# R = 1.00 * np.eye(self.acados_ocp.dims.nu)
# self.acados_ocp.cost.W = linalg.block_diag(Q, R)
# self.acados_ocp.cost.W_e = Q
self.y_ref = []
self.y_ref_end = []
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[: self.acados_ocp.dims.nx, :] = np.eye(self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx :, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros((self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
# TODO: Please confirm this
raise NotImplementedError("ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
raise RuntimeError("Should we put y_ref = zeros?")
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
raise RuntimeError("TODO: Should we put y_ref_end = zeros?")
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1),))
self.acados_ocp.cost.yref_e = np.zeros((max(self.acados_ocp.dims.ny_e, 1),))
# TODO changed hard coded values below (you can use J["weight"])
Q_ocp = np.zeros((15, 15))
np.fill_diagonal(Q_ocp, 1000)
R_ocp = np.zeros((4, 4))
np.fill_diagonal(R_ocp, 1000)
self.acados_ocp.cost.W = linalg.block_diag(Q_ocp, R_ocp)
self.acados_ocp.cost.W_e = np.zeros((1, 1))
# TODO: Is the following useful?
# if len(self.y_ref):
# self.y_ref = np.vstack(self.y_ref)
# else:
# self.y_ref = [np.zeros((1, 1))] * self.ocp
# if len(self.y_ref_end):
# self.y_ref_end = np.vstack(self.y_ref_end)
# else:
# self.y_ref_end = np.zeros((1, 1))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError("TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
self.acados_ocp.model.cost_expr_ext_cost = sum1(self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
import casadi as ca
import matplotlib.pyplot as plt
eqs = rocket_casadi.rocket_equations()
x0, p0 = eqs['initialize'](75)
dt = 0.1
m_dot = 0.1
t_vect = np.arange(0, 4, dt)
n = len(t_vect)
elv_vect = ca.SX.sym('elv', n)
x = x0
for i in range(n):
t = t_vect[i]
elv = elv_vect[i]
u = ca.vertcat(m_dot, 0, elv, 0)
x = eqs['predict'](x, u, p0, t, dt)
x_final = x
dx_final = eqs['rhs'](x_final, u, p0)
s0 = 0 * np.ones(n)
x = ca.SX.sym('x')
nlp = {
'x': elv_vect,
'f': ca.dot(elv_vect, elv_vect),
'g': ca.vertcat(dx_final[12])
}
args = {
'print_time': 1,
'monitor': ['nlp_f', 'nlp_g'],
# constant for this problem
m = 1.0
L = 3.0
g = 9.81
# psi = pl.pi / 2.0
psi = pl.pi / (180.0 * 2)
# System
x = ca.MX.sym("x", 2)
p = ca.MX.sym("p", 1)
u = ca.MX.sym("u", 1)
# f = ca.vertcat([x[1], p[0]/(m*(L**2))*(u-x[0]) - g/L * pl.sin(x[0])])
f = ca.vertcat([x[1], p[0]/(m*(L**2))*(u-x[0]) - g/L * x[0]])
phi = x
odesys = pecas.systems.ExplODE(x = x, u = u, p = p, f = f, phi = phi)
odesys.show_system_information(showEquations = True)
data = pl.loadtxt('data_pendulum.txt')
tu = data[:500, 0]
numeas = data[:500, 1]
wmeas = data[:500, 2]
yN = pl.array([numeas,wmeas])
N = tu.size
uN = [psi] * (N-1)
wv = pl.ones(yN.shape)
