def from_dcm(cls, R):
assert R.shape == (3, 3)
b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2])
b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2])
b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2])
b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] - R[2, 2])
q1 = ca.SX(4, 1)
q1[0] = b1
q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1)
q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1)
q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1)
q2 = ca.SX(4, 1)
q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2)
q2[1] = b2
q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2)
q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2)
q3 = ca.SX(4, 1)
q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3)
q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3)
q3[2] = b3
q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3)
q4 = ca.SX(4, 1)
q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4)
q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4)
q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4)
q4[3] = b4
q = ca.if_else(R[0, 0] > 0, ca.if_else(R[1, 1] > 0, q1, q2),
ca.if_else(R[1, 1] > R[2, 2], q3, q4))
return q
def exp(cls, v):
assert v.shape == (3, 1) or v.shape == (3, )
angle = ca.norm_2(v)
res = ca.SX(4, 1)
res[:3] = ca.tan(angle / 4) * v / angle
res[3] = 0
return ca.if_else(angle > eps, res, ca.SX([0, 0, 0, 0]))
def test_concatenations(self):
y = np.linspace(0, 1, 10)[:, np.newaxis]
a = pybamm.Vector(y)
b = pybamm.Scalar(16)
c = pybamm.Scalar(3)
conc = pybamm.NumpyConcatenation(a, b, c)
self.assertTrue(
casadi.is_equal(conc.to_casadi(), casadi.SX(conc.evaluate())))
# Domain concatenation
mesh = get_mesh_for_testing()
a_dom = ["negative electrode"]
b_dom = ["positive electrode"]
a = 2 * pybamm.Vector(np.ones_like(mesh[a_dom[0]][0].nodes),
domain=a_dom)
b = pybamm.Vector(np.ones_like(mesh[b_dom[0]][0].nodes), domain=b_dom)
conc = pybamm.DomainConcatenation([b, a], mesh)
self.assertTrue(
casadi.is_equal(conc.to_casadi(), casadi.SX(conc.evaluate())))
# 2d
disc = get_1p1d_discretisation_for_testing()
a = pybamm.Variable("a", domain=a_dom)
b = pybamm.Variable("b", domain=b_dom)
conc = pybamm.Concatenation(a, b)
disc.set_variable_slices([conc])
expr = disc.process_symbol(conc)
y = casadi.SX.sym("y", expr.size)
x = expr.to_casadi(None, y)
f = casadi.Function("f", [x], [x])
y_eval = np.linspace(0, 1, expr.size)
self.assertTrue(
casadi.is_equal(f(y_eval), casadi.SX(expr.evaluate(y=y_eval))))
def create_LSpoly(d, x, y):
"creates polynomial by least squares fitting of order d. Builds Vandermonde matrix manually"
# x = np.append(x,0) # add (0,0) as data point
# y = np.append(y,0)
a = d + 1 # number of parameters including a0
M = ca.SX.sym('M', 2 * d + 1) # all the exponents from 1 to 2k
sizex = x.shape[0] # number of data points x
M[0] = sizex # first entry in matrix
for k in range(1, M.shape[0]): # collect all matrix entries
M[k] = sum([x[o]**k for o in range(0, sizex)])
sumM = ca.SX(a, a) # create actual matrix
for j in range(0, a):
for k in range(0, a):
sumM[j, k] = M[k + j]
B = ca.SX(a, 1) # create B vector (sum of y)
for k in range(0, a):
B[k] = sum([y[o] * x[o]**k for o in range(0, sizex)])
X = ca.solve(sumM, B) # parameters order: low to high power
xvar = ca.SX.sym('xvar')
poly = X[0]
for k in range(1, X.shape[0]):
poly += X[k] * xvar**(k) # create polynomial
pfun = ca.Function('poly', [xvar], [poly])
return pfun, X, poly
def test_special_functions(self):
a = pybamm.Array(np.array([1, 2, 3, 4, 5]))
self.assertEqual(pybamm.max(a).to_casadi(), casadi.SX(5))
self.assertEqual(pybamm.min(a).to_casadi(), casadi.SX(1))
b = pybamm.Array(np.array([-2]))
c = pybamm.Array(np.array([3]))
self.assertEqual(pybamm.Function(np.abs, b).to_casadi(), casadi.SX(2))
self.assertEqual(pybamm.Function(np.abs, c).to_casadi(), casadi.SX(3))
def log(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
v = ca.SX(3, 1)
norm_q = ca.norm_2(q)
theta = 2 * ca.acos(q[0])
c = ca.sin(theta / 2)
v[0] = theta * q[1] / c
v[1] = theta * q[2] / c
v[2] = theta * q[3] / c
return ca.if_else(ca.fabs(c) > eps, v, ca.SX([0, 0, 0]))
def __init__(self, variable_indexes, value, state_index):
"""
construct limitations on the state variables caused by internal
circuit switching
"""
super(State_variable_constraint, self).__init__()
self._indexes = cd.SX(variable_indexes)
self._value = cd.SX(value)
self._state_index = state_index
def exp(cls, v):
assert v.shape == (3, 1) or q.shape == (3, )
q = ca.SX(4, 1)
theta = ca.norm_2(v)
q[0] = ca.cos(theta / 2)
c = ca.sin(theta / 2)
n = ca.norm_2(v)
q[1] = c * v[0] / n
q[2] = c * v[1] / n
q[3] = c * v[2] / n
return ca.if_else(n > eps, q, ca.SX([1, 0, 0, 0]))
def parse_box_constraint(self, g):
assert (g.numel() == 1)
op = g.op()
if (not g.is_symbolic()) and (not op
in (casadi.OP_SUB, casadi.OP_NEG)):
raise NotImplementedError # All constraints are currently implemented as: g == lhs - rhs
# Parse: g == lhs - rhs
if op == casadi.OP_SUB:
lhs = g.dep(0)
rhs = g.dep(1)
elif op == casadi.OP_NEG:
lhs = casadi.SX(0.0)
rhs = g.dep(0)
elif g.is_symbolic():
lhs = g
rhs = casadi.SX(0.0)
if not (lhs.is_leaf() and rhs.is_leaf()):
return None # Not a (simple) box constraint
if lhs.is_constant():
is_lhs_variable = False
else:
assert lhs.is_symbolic()
is_lhs_variable = not lhs.name().startswith('P')
if rhs.is_constant():
is_rhs_variable = False
else:
assert rhs.is_symbolic()
is_rhs_variable = not rhs.name().startswith('P')
if is_lhs_variable and is_rhs_variable:
return None # Not a box constraint
if (not is_lhs_variable) and (not is_rhs_variable):
raise RuntimeError('A constraint without variables is non-sense')
# g <= 0
# lhs - rhs <= 0
# lhs <= rhs
# is_rhs_variable => lhs is lower bound
# is_lhs_variable => rhs is upper bound
BoxConstraint = collections.namedtuple(
'BoxConstraint', ['variable', 'bound', 'is_upper_bound'])
if is_lhs_variable:
return BoxConstraint(variable=lhs, bound=rhs, is_upper_bound=True)
else:
return BoxConstraint(variable=rhs, bound=lhs, is_upper_bound=False)
def _initialize_polynomial_constraints(self):
""" Add constraints to the model to account for system dynamics and
continuity constraints """
# All collocation time points
T = np.zeros((self.nk, self.d+1), dtype=object)
for k in range(self.nk):
for j in range(self.d+1):
T[k,j] = (self.var.h_sx[self._get_stage_index(k)] *
(k + self.col_vars['tau_root'][j]))
# For all finite elements
for k in range(self.nk):
# For all collocation points
for j in range(1, self.d+1):
# Get an expression for the state derivative at the collocation
# point
xp_jk = 0
for r in range(self.d+1):
xp_jk += self.col_vars['C'][r,j]*cs.SX(self.var.x_sx[k,r])
# Add collocation equations to the NLP.
# Boundary fluxes are calculated by multiplying the EFM
# coefficients in V by the efm matrix
[fk] = self.dxdt.call(
[T[k,j], cs.SX(self.var.x_sx[k,j]),
self._get_symbolic_flux(k, j)])
self.add_constraint(
self.var.h_sx[self._get_stage_index(k)] * fk - xp_jk,
msg='DXDT collocation - FE {0}, degree {1}'.format(k,j))
# Add continuity equation to NLP
if k+1 != self.nk:
# Get an expression for the state at the end of the finite
# element
xf_k = self.col_vars['D'].dot(cs.SX(self.var.x_sx[k]))
self.add_constraint(cs.SX(self.var.x_sx[k+1,0]) - xf_k,
msg='Continuity - FE {0}'.format(k))
# Get an expression for the endpoint for objective purposes
xf = self.col_vars['D'].dot(cs.SX(self.var.x_sx[-1]))
self.xf = {met : x_sx for met, x_sx in zip(self.boundary_species, xf)}
# Similarly, get an expression for the beginning point
x0 = self.var.x_sx[0,0,:]
self.x0 = {met : x_sx for met, x_sx in zip(self.boundary_species, x0)}
def test_direct_product():
G = DirectProduct([R3, R3])
v1 = ca.SX([1, 2, 3, 4, 5, 6])
v2 = G.product(v1, v1)
assert ca.norm_2(v2 - 2 * v1) < eps
G = DirectProduct([Mrp, R3])
a = ca.SX([0.1, 0.2, 0.3, 0, 5, 6, 7])
b = ca.SX([0, 0, 0, 0, 1, 2, 3])
c = ca.SX([0.1, 0.2, 0.3, 0, 6, 8, 10])
assert ca.norm_2(c - G.product(a, b)) < eps
v = ca.SX([0.1, 0.2, 0.3, 4, 5, 6])
assert ca.norm_2(v - G.log(G.exp(v))) < eps
def generate_cost_general_constraints(self, current_state, input, lambdas,
general_constraint_weights,
step_horizon):
"""
Evaluate function cost of all general constraints for 1 step in the horizon
L = lambda ci(x) + mu ci(x)^2
Parameters
---------
current_state: state of this step in the horizon
input: current input applied to the system
lambdas: lambda's for this step of the horizon
general_constraint_weights: mu's for this step of the horizon
step_horizon: the index of the step in the horizon (the first step is index 0)
number_of_general_constraints = length(obj.controller.general_constraints);
"""
offset_constraints = step_horizon * self._controller.number_of_general_constraints
cost = cd.SX(1, 1)
for i in range(0, self._controller.number_of_general_constraints):
constraint_cost = self._controller.general_constraints[
i].evaluate_cost(current_state, input)
cost = cost - constraint_cost * lambdas[offset_constraints + i]
cost = cost + (constraint_cost**2) * general_constraint_weights[
offset_constraints + i]
return cost
def _get_C(self, i_X_p, Si, Ic, q, q_dot, n_joints,
gravity=None, f_ext=None):
"""Internal function for calculating the joint space bias matrix."""
v = []
a = []
f = []
C = cs.SX.zeros(n_joints)
for i in range(0, n_joints):
vJ = cs.mtimes(Si[i], q_dot[i])
if i == 0:
v.append(vJ)
if gravity is not None:
ag = np.array([0., 0., 0., gravity[0], gravity[1], gravity[2]])
a.append(cs.mtimes(i_X_p[i], -ag))
else:
a.append(cs.SX([0., 0., 0., 0., 0., 0.]))
else:
v.append(cs.mtimes(i_X_p[i], v[i-1]) + vJ)
a.append(cs.mtimes(i_X_p[i], a[i-1]) + cs.mtimes(plucker.motion_cross_product(v[i]),vJ))
f.append(cs.mtimes(Ic[i], a[i]) + cs.mtimes(plucker.force_cross_product(v[i]), cs.mtimes(Ic[i], v[i])))
if f_ext is not None:
f = self._apply_external_forces(f_ext, f, i_X_0)
for i in range(n_joints-1, -1, -1):
C[i] = cs.mtimes(Si[i].T, f[i])
if i != 0:
f[i-1] = f[i-1] + cs.mtimes(i_X_p[i].T, f[i])
return C
def get_gravity_rnea(self, root, tip, gravity):
"""Returns the gravitational term as a casadi function."""
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
n_joints = self.get_n_joints(root, tip)
q = cs.SX.sym("q", n_joints)
i_X_p, Si, Ic = self._model_calculation(root, tip, q)
v = []
a = []
ag = cs.SX([0., 0., 0., gravity[0], gravity[1], gravity[2]])
f = []
tau = cs.SX.zeros(n_joints)
for i in range(0, n_joints):
if i == 0:
a.append(cs.mtimes(i_X_p[i], -ag))
else:
a.append(cs.mtimes(i_X_p[i], a[i - 1]))
f.append(cs.mtimes(Ic[i], a[i]))
for i in range(n_joints - 1, -1, -1):
tau[i] = cs.mtimes(Si[i].T, f[i])
if i != 0:
f[i - 1] = f[i - 1] + cs.mtimes(i_X_p[i].T, f[i])
tau = cs.Function("C", [q], [tau], self.func_opts)
return tau
def test_sum2():
# Check it returns the same results with casadi and numpy
a = np.array([[1, 2, 3], [1, 2, 3]])
b = cas.SX(a)
assert np.all(np.sum(a) == cas.DM(np.sum(b)))
assert np.all(np.sum(a, axis=1) == cas.DM(np.sum(b, axis=1)))
def shadow(cls, r):
assert r.shape == (4, 1) or r.shape == (4, )
n_sq = ca.dot(r[:3], r[:3])
res = ca.SX(4, 1)
res[:3] = -r[:3] / n_sq
res[3] = ca.logic_not(r[3])
return res
def from_quat(cls, q):
assert q.shape == (4, 1)
R = ca.SX(3, 3)
a = q[0]
b = q[1]
c = q[2]
d = q[3]
aa = a * a
ab = a * b
ac = a * c
ad = a * d
bb = b * b
bc = b * c
bd = b * d
cc = c * c
cd = c * d
dd = d * d
R[0, 0] = aa + bb - cc - dd
R[0, 1] = 2 * (bc - ad)
R[0, 2] = 2 * (bd + ac)
R[1, 0] = 2 * (bc + ad)
R[1, 1] = aa + cc - bb - dd
R[1, 2] = 2 * (cd - ab)
R[2, 0] = 2 * (bd - ac)
R[2, 1] = 2 * (cd + ab)
R[2, 2] = aa + dd - bb - cc
return R
def test_integrate(self):
# Constant
solver = pybamm.CasadiSolver(rtol=1e-8, atol=1e-8, method="idas")
y = casadi.SX.sym("y")
constant_growth = casadi.SX(0.5)
problem = {"x": y, "ode": constant_growth}
y0 = np.array([0])
t_eval = np.linspace(0, 1, 100)
solution = solver.integrate_casadi(problem, y0, t_eval)
np.testing.assert_array_equal(solution.t, t_eval)
np.testing.assert_allclose(0.5 * solution.t, solution.y[0])
# Exponential decay
solver = pybamm.CasadiSolver(rtol=1e-8, atol=1e-8, method="cvodes")
exponential_decay = -0.1 * y
problem = {"x": y, "ode": exponential_decay}
y0 = np.array([1])
t_eval = np.linspace(0, 1, 100)
solution = solver.integrate_casadi(problem, y0, t_eval)
np.testing.assert_allclose(solution.y[0], np.exp(-0.1 * solution.t))
self.assertEqual(solution.termination, "final time")
def _initialize_continuity_constraints(self):
# Constraint function for the NLP
d = self.opts['degree']
X = self._X
P = self._P
T = self.opts['T']
g = []
lbg = []
ubg = []
# For all finite elements
for k in range(self.opts['nk']):
# For all collocation points
for j in range(1, d + 1):
# Get an expression for the state derivative at the collocation
# point
xp_jk = 0
for r in range(d + 1):
xp_jk += self.opts['C'][r, j] * cs.SX(X[k, r])
# Add collocation equations to the NLP
[fk] = self.model.call([T[k, j], cs.SX(X[k, j]), P])
g.append(self.opts['h'] * fk - xp_jk)
lbg.append(np.zeros(self.NEQ)) # equality constraints
ubg.append(np.zeros(self.NEQ)) # equality constraints
# Add continuity equation to NLP
if k + 1 != self.opts['nk']:
# Get an expression for the state at the end of the finite
# element
xf_k = 0
for r in range(d + 1):
xf_k += self.opts['D'][r] * cs.SX(X[k, r])
g.append(cs.SX(X[k + 1, 0]) - xf_k)
lbg.append(np.zeros(self.NEQ))
ubg.append(np.zeros(self.NEQ))
# Concatenate constraints
self._g = cs.vertcat(g)
self.opts['lbg'] = np.concatenate(lbg)
self.opts['ubg'] = np.concatenate(ubg)
def _get_symbolic_flux(self, finite_element, degree):
""" Get a symbolic expression for the boundary fluxes at the given
finite_element and polynomial degree """
return cs.SX(self.efms_object.T.dot((
np.asarray(self.var.a_sx[finite_element, degree-1], dtype=object) *
np.asarray(self.var.v_sx[self._get_stage_index(finite_element)],
dtype=object)).flatten()).values)
def wedge(v):
X = ca.SX(3, 3)
X[0, 1] = -v[2]
X[0, 2] = v[1]
X[1, 0] = v[2]
X[1, 2] = -v[0]
X[2, 0] = -v[1]
X[2, 1] = v[0]
return X
def inv(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
qi = ca.SX(4, 1)
n = ca.norm_2(q)
qi[0] = q[0] / n
qi[1] = -q[1] / n
qi[2] = -q[2] / n
qi[3] = -q[3] / n
return qi
def vee(X):
v = ca.SX(6, 1)
v[0, 0] = X[2, 1]
v[1, 0] = X[0, 2]
v[2, 0] = X[1, 0]
v[3, 0] = X[3, 0]
v[4, 0] = X[3, 1]
v[5, 0] = X[3, 2]
return v
def test_convert_differentiated_function(self):
a = pybamm.Scalar(0)
b = pybamm.Scalar(1)
# function
def sin(x):
return anp.sin(x)
f = pybamm.Function(sin, b).diff(b)
self.assertEqual(f.to_casadi(), casadi.SX(np.cos(1)))
def myfunction(x, y):
return x + y**3
f = pybamm.Function(myfunction, a, b).diff(a)
self.assertEqual(f.to_casadi(), casadi.SX(1))
f = pybamm.Function(myfunction, a, b).diff(b)
self.assertEqual(f.to_casadi(), casadi.SX(3))
def from_mrp(cls, r):
assert r.shape == (4, 1) or r.shape == (4, )
a = r[:3]
q = ca.SX(4, 1)
n_sq = ca.dot(a, a)
den = 1 + n_sq
q[0] = (1 - n_sq) / den
for i in range(3):
q[i + 1] = 2 * a[i] / den
return ca.if_else(r[3], -q, q)
def from_quat(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
x = ca.SX(4, 1)
den = 1 + q[0]
x[0] = q[1] / den
x[1] = q[2] / den
x[2] = q[3] / den
x[3] = 0
r = cls.shadow_if_necessary(x)
r[3] = 0
return r
def product(cls, a, b):
assert a.shape == (4, 1) or a.shape == (4, )
assert b.shape == (4, 1) or b.shape == (4, )
r1 = a[0]
v1 = a[1:]
r2 = b[0]
v2 = b[1:]
res = ca.SX(4, 1)
res[0] = r1 * r2 - ca.dot(v1, v2)
res[1:] = r1 * v2 + r2 * v1 + ca.cross(v1, v2)
return res
def from_quat(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
e = ca.SX(3, 1)
a = q[0]
b = q[1]
c = q[2]
d = q[3]
e[0] = ca.atan2(2 * (a * b + c * d), 1 - 2 * (b**2 + c**2))
e[1] = ca.asin(2 * (a * c - d * b))
e[2] = ca.atan2(2 * (a * d + b * c), 1 - 2 * (c**2 + d**2))
return e
def vee(cls, X):
'''
This takes in an element of the SE3 Lie Group and returns the se3 Lie Algebra elements
'''
v = ca.SX(6, 1)
v[0, 0] = X[2, 1]
v[1, 0] = X[0, 2]
v[2, 0] = X[1, 0]
v[3, 0] = X[0, 3]
v[4, 0] = X[1, 3]
v[5, 0] = X[2, 3]
return v
def set_derivative(self, x, dxdt_fn):
if isinstance(dxdt_fn, float): dxdt_fn = casadi.SX(dxdt_fn)
assert isinstance(x, casadi.SX)
assert isinstance(dxdt_fn, casadi.SX)
assert x.is_leaf()
assert x.numel() == 1
assert dxdt_fn.numel() == 1
var_name = parse_variable_name(x.name())
assert var_name.flag == 'T'
assert self.phases[var_name.phase].trajectories[
var_name.name].derivative is None
self.phases[var_name.phase].trajectories[
var_name.name].derivative = dxdt_fn