def state_contraints(self, robot, U):
constraintVar = c.MX(
2 * (robot.nx - 1) * self.N,
self.T) #skipping orientation. 2* to include min and max
U = c.reshape(U, robot.nu * self.N, self.T)
X = c.MX(robot.nx * self.N, self.T + 1)
X[:, 0] = np.reshape(self.X0, (robot.nx * self.N, ))
for i in range(self.T):
for n in range(self.N):
X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics(
X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i],
U[robot.nu * n + 1, i], U[robot.nu * n + 2, i])
constraintVar[2 * (robot.nx - 1) * n:2 * (robot.nx - 1) *
(n + 1), i] = c.blockcat(
[[
self.Xmax - X[robot.nx * n:robot.nx *
(n + 1) - 1, i + 1]
],
[
X[robot.nx * n:robot.nx *
(n + 1) - 1, i + 1] - self.Xmin
]])
constraintVar = c.reshape(constraintVar, 1,
2 * (robot.nx - 1) * self.N * self.T)
return constraintVar
def formulateNLP(self, walker):
Xk = ca.MX([-0.3, 0.7, 0.0, -0.5, -0.6, 0., 0., 0., 0., 0.])
Xk = ca.reshape(Xk, 10, 1)
self.g += [Xk]
self.lbg += [-np.pi / 2] * 10
self.ubg += [np.pi / 2] * 10
for k in range(walker.N):
if k == walker.N - 1:
Xk = ca.MX([-0.6, -0.5, 0.0, 0.7, -0.3, 0., 0., 0., 0., 0.])
Xk = ca.reshape(Xk, 10, 1)
self.g += [Xk]
self.lbg += [-np.pi / 2] * 10
self.ubg += [np.pi / 2] * 10
# New NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), 4, 1)
# Uk = ca.MX([0.])
self.w += [Uk]
self.lbw += [-walker.tau_max] * 4
self.ubw += [walker.tau_max] * 4
self.w0 += [0.] * 4
# Integrate till the end of the interval
Fk = walker.F(x0=Xk, u=Uk)
Xk = Fk['xf']
self.J = self.J + Fk['j']
def 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 __set_cost_function(self, costFunc, mean_ref_s, P_s):
""" Define stage cost and terminal cost
"""
mean_s = ca.MX.sym('mean', self.__Ny)
covar_x_s = ca.MX.sym('covar_x', self.__Ny, self.__Ny)
covar_u_s = ca.MX.sym('covar_u', self.__Nu, self.__Nu)
u_s = ca.MX.sym('u', self.__Nu)
delta_u_s = ca.MX.sym('delta_u', self.__Nu)
Q = ca.MX(self.__Q)
R = ca.MX(self.__R)
S = ca.MX(self.__S)
if costFunc is 'quad':
self.__l_func = ca.Function(
'l', [mean_s, covar_x_s, u_s, covar_u_s, delta_u_s], [
self.__cost_l(mean_s, mean_ref_s, covar_x_s, u_s,
covar_u_s, delta_u_s, Q, R, S)
])
self.__lf_func = ca.Function(
'lf', [mean_s, covar_x_s, P_s],
[self.__cost_lf(mean_s, mean_ref_s, covar_x_s, P_s)])
elif costFunc is 'sat':
self.__l_func = ca.Function(
'l', [mean_s, covar_x_s, u_s, covar_u_s, delta_u_s], [
self.__cost_saturation_l(mean_s, mean_ref_s, covar_x_s,
u_s, covar_u_s, delta_u_s, Q, R,
S)
])
self.__lf_func = ca.Function('lf', [mean_s, covar_x_s, P_s], [
self.__cost_saturation_lf(mean_s, mean_ref_s, covar_x_s, P_s)
])
else:
raise NameError('No cost function called: ' + costFunc)
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.assert_casadi_equal(conc.to_casadi(),
casadi.MX(conc.evaluate()),
evalf=True)
# Domain concatenation
mesh = get_mesh_for_testing()
a_dom = ["negative electrode"]
b_dom = ["separator"]
a = 2 * pybamm.Vector(np.ones_like(mesh[a_dom[0]].nodes), domain=a_dom)
b = pybamm.Vector(np.ones_like(mesh[b_dom[0]].nodes), domain=b_dom)
conc = pybamm.DomainConcatenation([b, a], mesh)
self.assert_casadi_equal(conc.to_casadi(),
casadi.MX(conc.evaluate()),
evalf=True)
# 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.assert_casadi_equal(f(y_eval), casadi.SX(expr.evaluate(y=y_eval)))
def move(self, u, t, delta_t, is_pred=False, prev_x=None, prev_v=None):
norm = np.linalg.norm
if prev_x is None:
prev_x = self.x_log[-1]
if prev_v is None:
prev_v = self.v_log[-1]
if is_pred:
norm = casadi.norm_2
prev_x = casadi.MX(prev_x)
if self.mm_order == 2:
prev_v = casadi.MX(prev_v)
if self.mm_order == 1:
x_new = prev_x + u * delta_t
v_new = u
elif self.mm_order == 2:
x_new = prev_x + prev_v * delta_t + u * (delta_t**2) / 2
v_new = prev_v + u * delta_t
# if norm(v_new) > self.vmax:
# v_new = v_new/norm(v_new)*self.vmax
# assert(norm(v_new) != 0)
# NEW
v_new = v_new / norm(v_new) * self.vmax
else:
return NotImplemented
# if is_pred:
# return x_new
# else:
return x_new, v_new
def test_special_functions(self):
a = pybamm.Array(np.array([1, 2, 3, 4, 5]))
self.assert_casadi_equal(pybamm.max(a).to_casadi(),
casadi.MX(5),
evalf=True)
self.assert_casadi_equal(pybamm.min(a).to_casadi(),
casadi.MX(1),
evalf=True)
b = pybamm.Array(np.array([-2]))
c = pybamm.Array(np.array([3]))
self.assert_casadi_equal(pybamm.Function(np.abs, b).to_casadi(),
casadi.MX(2),
evalf=True)
self.assert_casadi_equal(pybamm.Function(np.abs, c).to_casadi(),
casadi.MX(3),
evalf=True)
for np_fun in [
np.sqrt,
np.tanh,
np.cosh,
np.sinh,
np.exp,
np.log,
np.sign,
np.sin,
np.cos,
np.arccosh,
np.arcsinh,
]:
self.assert_casadi_equal(pybamm.Function(np_fun, c).to_casadi(),
casadi.MX(np_fun(3)),
evalf=True)
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())
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())
self.derivative[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 buildDynamicalSystem(self):
# q1 is first link q2 is second link
q1 = ca.MX.sym('q1')
dq1 = ca.MX.sym('dq1')
q2 = ca.MX.sym('q2')
dq2 = ca.MX.sym('dq2')
u = ca.MX.sym('u')
theta = ca.MX.sym("theta", 7, 1)
if self.trainMotor:
Rm = ca.MX.sym("Rm")
km = ca.MX.sym("km")
params = ca.vertcat(ca.MX.sym("params", 7, 1), Rm, km)
else:
Rm = self.Rm
km = self.km
params = theta
states = ca.vertcat(q1, q2, dq1, dq2)
controls = u # km * (u - km * dq1) / Rm;
if self.hackOn:
# convert actions which are given as torques to voltage to be consistent with the controller inputs of Lukas
u = u * Rm / km
controls_torque = km * (u - km * dq1) / Rm
# build matrix for mass matrix
phi_1_1 = ca.MX(2, 7)
phi_1_1[0, 0] = ca.MX.ones(1)
phi_1_1[0, 1] = ca.sin(q2) * ca.sin(q2)
phi_1_1[1, 2] = ca.cos(q2)
phi_1_2 = ca.MX(2, 7)
phi_1_2[0, 2] = -ca.cos(q2)
phi_1_2[1, 3] = ca.MX.ones(1)
mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params),
ca.mtimes(phi_1_2, params))
phi_2 = ca.MX(2, 7)
phi_2[0, 1] = ca.sin(2 * q2) * dq1 * dq2
phi_2[0, 2] = ca.sin(q2) * dq2 * dq2
phi_2[0, 5] = dq1
phi_2[1, 1] = -1 / 2 * ca.sin(2 * q2) * dq1 * dq1
phi_2[1, 4] = self.g * ca.sin(q2)
phi_2[1, 6] = dq2
forces = ca.mtimes(phi_2, params)
actions = ca.vertcat(controls_torque, ca.MX.zeros(1))
b = actions - forces
states_dd = ca.solve(mass_matrix, b)
states_d = ca.vertcat(dq1, dq2, states_dd[0], states_dd[1])
return states, states_d, controls, params
def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(
1) # Recycle these common nodes as much as possible.
for variable_list in [
self.states, self.alg_states, self.inputs, self.parameters,
self.constants
]:
attribute_lists = [[] for i in range(len(ast.Symbol.ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(
ast.Symbol.ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(
value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[
ca.veccat(*attribute_list)
for attribute_list in attribute_lists
])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
contains_if_else = ca.OP_IF_ELSE_ZERO in [
f.instruction_id(k) for k in range(f.n_instructions())
]
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var),
in_var).is_zero()
if contains_if_else or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return ca.Function('variable_metadata', [in_var], out)
def w_dynamics(self):
f_thrust = self.u * self.quad.max_thrust
y_f = cs.MX(self.quad.y_f)
x_f = cs.MX(self.quad.x_f)
c_f = cs.MX(self.quad.z_l_tau)
return cs.vertcat(
(cs.mtimes(f_thrust.T, y_f) + (self.quad.J[1] - self.quad.J[2]) * self.r[1] * self.r[2]) / self.quad.J[0],
(-cs.mtimes(f_thrust.T, x_f) + (self.quad.J[2] - self.quad.J[0]) * self.r[2] * self.r[0]) / self.quad.J[1],
(cs.mtimes(f_thrust.T, c_f) + (self.quad.J[0] - self.quad.J[1]) * self.r[0] * self.r[1]) / self.quad.J[2])
def __setCosts__(self):
Q = ca.diag(ca.MX([1, 1, 1]))
R = ca.diag(ca.MX([0.1, 0.1]))
cost = 0
for k in range(self.N):
q, dq = self.states[k], self.dstates[k]
u = self.actions[k]
cost += dq.T @ Q @ dq + u.T @ R @ u
self.opti.minimize(cost)
def _substitute_delay_arguments(self, delay_arguments, symbols, values):
exprs = ca.substitute(
[ca.MX(argument.expr) for argument in delay_arguments], symbols,
values)
durations = ca.substitute(
[ca.MX(argument.duration) for argument in delay_arguments],
symbols, values)
return [
DelayArgument(expr, duration)
for expr, duration in zip(exprs, durations)
]
def test_convert_differentiated_function(self):
a = pybamm.Scalar(0)
b = pybamm.Scalar(1)
def myfunction(x, y):
return x + y**3
f = pybamm.Function(myfunction, a, b).diff(a)
self.assert_casadi_equal(f.to_casadi(), casadi.MX(1), evalf=True)
f = pybamm.Function(myfunction, a, b).diff(b)
self.assert_casadi_equal(f.to_casadi(), casadi.MX(3), evalf=True)
def __init__(self, start_angles, start_angular_vel, start_pos):
# set our parameters of optimization
self.opti = ca.Opti()
self.terrain_factor = 0.1
self.N = 40
self.T = 0.3
self.step_max = 0.5
self.tauMax = 20
self.pi = np.pi
self.length = ca.MX([0.5, 0.5, 0.5, 0.5, 0.5])
self.mass = ca.MX([0.25, 0.25, 0.25, 0.25, 0.25])
self.inertia = self.mass * (self.length**2) / 12
self.gravity = 9.81
self.h = self.T / self.N
self.goal = [start_angles, start_angular_vel]
self.ini_goal = self.goal[0].to_DM()
self.fin_goal = ca.DM([
self.ini_goal[4], self.ini_goal[3], self.ini_goal[2],
self.ini_goal[1], self.ini_goal[0]
])
self.p0 = ca.MX(start_pos)
self.comh = self.length[0] * 0.5
#set our optimization variables
self.x = []
self.xdot = []
self.u = []
self.left = []
self.right = []
for i in range(self.N):
self.x.append(self.opti.variable(5))
self.xdot.append(self.opti.variable(5))
self.u.append(self.opti.variable(4))
self.left.append(self.opti.variable(2))
self.right.append(self.opti.variable(2))
self.pos = []
self.com = []
self.ddq = []
self.dpos = []
for n in range(self.N):
p, dp, ddp, c, dc, ddc = self.getKinematics(
self.x[n], self.xdot[n])
self.pos.append(p)
self.dpos.append(dp)
self.com.append(c)
ddq = self.getDynamics(self.x[n], self.xdot[n], self.u[n], p, ddp,
c, ddc)
self.ddq.append(ddq)
if n == self.N - 1:
self.x_impact, self.xdot_impact = self.impactMap(
self.x[n], self.xdot[n], p, dp, c, dc)
def bounds(self):
# Call parent class first for default values.
bounds = super().bounds()
# Parameter values
parameters = self.parameters(0)
parameter_values = [
parameters.get(param.name(), param)
for param in self.__mx['parameters']
]
# Load additional bounds from model
for v in itertools.chain(self.__pymoca_model.states,
self.__pymoca_model.alg_states,
self.__pymoca_model.inputs):
sym_name = v.symbol.name()
try:
(m, M) = bounds[sym_name]
except KeyError:
if self.__python_types.get(sym_name, float) == bool:
(m, M) = (0, 1)
else:
(m, M) = (-np.inf, np.inf)
m_ = ca.MX(v.min)
if not m_.is_constant():
[m_] = substitute_in_external([m_], self.__mx['parameters'],
parameter_values)
if not m_.is_constant():
raise Exception(
'Could not resolve lower bound for variable {}'.format(
sym_name))
m_ = float(m_)
M_ = ca.MX(v.max)
if not M_.is_constant():
[M_] = substitute_in_external([M_], self.__mx['parameters'],
parameter_values)
if not M_.is_constant():
raise Exception(
'Could not resolve upper bound for variable {}'.format(
sym_name))
M_ = float(M_)
# We take the intersection of all provided bounds
m = max(m, m_)
M = min(M, M_)
bounds[sym_name] = (m, M)
return bounds
def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(1) # Recycle these common nodes as much as possible.
for variable_list in [self.states, self.alg_states, self.inputs, self.parameters, self.constants]:
attribute_lists = [[] for i in range(len(CASADI_ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(CASADI_ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[ca.veccat(*attribute_list) for attribute_list in attribute_lists])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
# NOTE: This is not a complete list of operations that can be
# handled in an affine expression. That said, it should
# capture the most common ways variable attributes are
# expressed as a function of parameters.
allowed_ops = {ca.OP_INPUT, ca.OP_OUTPUT, ca.OP_CONST,
ca.OP_SUB, ca.OP_ADD, ca.OP_SUB, ca.OP_MUL, ca.OP_DIV, ca.OP_NEG}
f_ops = {f.instruction_id(k) for k in range(f.n_instructions())}
contains_unallowed_ops = not f_ops.issubset(allowed_ops)
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var), in_var).is_zero()
if contains_unallowed_ops or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return self._expand_mx_func(ca.Function('variable_metadata', [in_var], out))
def test_simplify_all(self):
# Create model, cache it, and load the cache
compiler_options = \
{'expand_vectors': True,
'replace_constant_values': True,
'replace_constant_expressions': True,
'replace_parameter_values': True,
'replace_parameter_expressions': True,
'eliminate_constant_assignments': True,
'detect_aliases': True,
'eliminable_variable_expression': r'_\w+',
'reduce_affine_expression': True}
casadi_model = transfer_model(TEST_DIR, 'Simplify', compiler_options)
ref_model = Model()
p3 = ca.MX.sym('p3')
x = ca.MX.sym('x')
der_x = ca.MX.sym('der(x)')
y = ca.MX.sym('y')
ref_model.states = list(map(Variable, [x]))
ref_model.states[0].min = 1
ref_model.states[0].max = 2
ref_model.states[0].nominal = 10
ref_model.der_states = list(map(Variable, [der_x]))
ref_model.alg_states = list(map(Variable, [y]))
ref_model.inputs = list(map(Variable, []))
ref_model.outputs = list(map(Variable, []))
ref_model.constants = list(map(Variable, []))
constant_values = []
for _cst, v in zip(ref_model.constants, constant_values):
_cst.value = v
ref_model.parameters = list(map(Variable, [p3]))
parameter_values = [np.nan]
for par, v in zip(ref_model.parameters, parameter_values):
par.value = v
A = ca.MX(2, 3)
A[0, 0] = -1.0
A[0, 1] = 1.0
A[1, 0] = -1.1
A[1, 2] = 1.0
b = ca.MX(2, 1)
b[0] = -6 - 3 * p3
b[1] = -7
x = ca.vertcat(x, der_x, y)
ref_model.equations = [ca.mtimes(A, x) + b]
# Compare
self.assert_model_equivalent_numeric(casadi_model, ref_model)
def validate(X_test, Y_test, X, Y, invK, hyper, meanFunc, alpha=None):
""" Validate GP model with new test data
"""
N, Ny = Y_test.shape
Nx = np.size(X, 1)
z_s = ca.MX.sym('z', Nx)
gp_func = ca.Function(
'gp', [z_s],
gp(invK,
ca.MX(X),
ca.MX(Y),
ca.MX(hyper),
z_s,
meanFunc=meanFunc,
alpha=alpha))
loss = 0
NLP = 0
for i in range(N):
mean, var = gp_func(X_test[i, :])
loss += (Y_test[i, :] - mean)**2
NLP += 0.5 * np.log(2 * np.pi *
(var)) + ((Y_test[i, :] - mean)**2) / (2 * var)
print(NLP)
print(var)
loss = loss / N
SMSE = loss / np.std(Y_test, 0)
MNLP = NLP / N
print('\n________________________________________')
print('# Validation of GP model ')
print('----------------------------------------')
print('* Num training samples: ' + str(np.size(Y, 0)))
print('* Num test samples: ' + str(N))
print('----------------------------------------')
print('* Mean squared error: ')
for i in range(Ny):
print('\t- State %d: %f' % (i + 1, loss[i]))
print('----------------------------------------')
print('* Standardized mean squared error:')
for i in range(Ny):
print('\t* State %d: %f' % (i + 1, SMSE[i]))
print('----------------------------------------\n')
print('* Mean Negative log Probability:')
for i in range(Ny):
print('\t* State %d: %f' % (i + 1, MNLP[i]))
print('----------------------------------------\n')
return SMSE, MNLP
def interpolate(ts, xs, t, equidistant, mode=0):
if interp1d is not None:
if mode == 0:
mode_str = 'linear'
elif mode == 1:
mode_str = 'floor'
else:
mode_str = 'ceil'
return interp1d(ts, xs, t, mode_str, equidistant)
else:
if mode == 1:
xs = xs[:-1] # block-forward
else:
xs = xs[1:] # block-backward
t = ca.MX(t)
if t.size1() > 1:
t_ = ca.MX.sym('t')
xs_ = ca.MX.sym('xs', xs.size1())
f = ca.Function('interpolant', [t_, xs_], [
ca.mtimes(ca.transpose((t_ >= ts[:-1]) * (t_ < ts[1:])), xs_)
])
f = f.map(t.size1(), 'serial')
return ca.transpose(f(ca.transpose(t), ca.repmat(xs, 1,
t.size1())))
else:
return ca.mtimes(ca.transpose((t >= ts[:-1]) * (t < ts[1:])), xs)
def getBounds(self, walker):
c = []
f = 30
for i in range(walker.N):
q = walker.x[i]
dq = walker.xdot[i]
u = walker.u[i]
c.extend([
walker.opti.bounded(ca.MX([-walker.pi / 2] * 5), q,
ca.MX([walker.pi / 2] * 5)),
# walker.opti.bounded(ca.MX([-f*walker.pi]*5),dq,ca.MX([f*walker.pi]*5)),
walker.opti.bounded(ca.MX([-walker.tauMax] * 4), u,
ca.MX([walker.tauMax] * 4)),
])
return c
def test_special_functions(self):
a = pybamm.Array(np.array([1, 2, 3, 4, 5]))
self.assert_casadi_equal(pybamm.max(a).to_casadi(),
casadi.MX(5),
evalf=True)
self.assert_casadi_equal(pybamm.min(a).to_casadi(),
casadi.MX(1),
evalf=True)
b = pybamm.Array(np.array([-2]))
c = pybamm.Array(np.array([3]))
self.assert_casadi_equal(pybamm.Function(np.abs, b).to_casadi(),
casadi.MX(2),
evalf=True)
self.assert_casadi_equal(pybamm.Function(np.abs, c).to_casadi(),
casadi.MX(3),
evalf=True)
def exitForEquation(self, tree):
logger.debug('exitForEquation')
f = self.for_loops.pop()
if len(f.values) > 0:
indexed_symbols = list(f.indexed_symbols.keys())
args = [f.index_variable] + indexed_symbols
expr = ca.vcat([ca.vec(self.get_mx(e)) for e in tree.equations])
free_vars = ca.symvar(expr)
arg_names = [arg.name() for arg in args]
free_vars = [e for e in free_vars if e.name() not in arg_names]
all_args = args + free_vars
F = ca.Function('loop_body', all_args, [expr])
indexed_symbols_full = []
for k in indexed_symbols:
s = f.indexed_symbols[k]
orig_symbol = self.nodes[self.current_class][s.tree.name]
indexed_symbol = orig_symbol[s.indices]
if s.transpose:
indexed_symbol = ca.transpose(indexed_symbol)
indexed_symbols_full.append(indexed_symbol)
Fmap = F.map("map", self.map_mode, len(f.values),
list(range(len(args), len(all_args))), [])
res = Fmap.call([f.values] + indexed_symbols_full + free_vars)
self.src[tree] = res[0].T
else:
self.src[tree] = ca.MX()
def linearized_quad_dynamics(self):
"""
Jacobian J matrix of the linearized dynamics specified in the function quad_dynamics. J[i, j] corresponds to
the partial derivative of f_i(x) wrt x(j).
:return: a CasADi symbolic function that calculates the 13 x 13 Jacobian matrix of the linearized simplified
quadrotor dynamics
"""
jac = cs.MX(self.state_dim, self.state_dim)
# Position derivatives
jac[0:3, 7:10] = cs.diag(cs.MX.ones(3))
# Angle derivatives
jac[3:7, 3:7] = skew_symmetric(self.r) / 2
jac[3, 10:] = 1 / 2 * cs.horzcat(-self.q[1], -self.q[2], -self.q[3])
jac[4, 10:] = 1 / 2 * cs.horzcat(self.q[0], -self.q[3], self.q[2])
jac[5, 10:] = 1 / 2 * cs.horzcat(self.q[3], self.q[0], -self.q[1])
jac[6, 10:] = 1 / 2 * cs.horzcat(-self.q[2], self.q[1], self.q[0])
# Velocity derivatives
a_u = (self.u[0] + self.u[1] + self.u[2] + self.u[3]) * self.quad.max_thrust / self.quad.mass
jac[7, 3:7] = 2 * cs.horzcat(a_u * self.q[2], a_u * self.q[3], a_u * self.q[0], a_u * self.q[1])
jac[8, 3:7] = 2 * cs.horzcat(-a_u * self.q[1], -a_u * self.q[0], a_u * self.q[3], a_u * self.q[2])
jac[9, 3:7] = 2 * cs.horzcat(0, -2 * a_u * self.q[1], -2 * a_u * self.q[1], 0)
# Rate derivatives
jac[10, 10:] = (self.quad.J[1] - self.quad.J[2]) / self.quad.J[0] * cs.horzcat(0, self.r[2], self.r[1])
jac[11, 10:] = (self.quad.J[2] - self.quad.J[0]) / self.quad.J[1] * cs.horzcat(self.r[2], 0, self.r[0])
jac[12, 10:] = (self.quad.J[0] - self.quad.J[1]) / self.quad.J[2] * cs.horzcat(self.r[1], self.r[0], 0)
return cs.Function('J', [self.x, self.u], [jac])
def cost_func_SSP(self, robot, U, X0):
cost = 0
U = c.reshape(U, robot.nu, self.T)
X = c.MX(robot.nx, self.T + 1)
X[:, 0] = X0
for i in range(self.T):
cost = (cost + c.mtimes(c.mtimes(U[:, i].T, self.R), U[:, i]) +
c.mtimes(c.mtimes((self.Xg - X[:, i]).T, self.Q),
(self.Xg - X[:, i])))
X_temp = X[0:2, i]
if params.OBSTACLES:
obstacle_cost = params.M*(c.exp(-(c.mtimes(c.mtimes((params.c_obs_1 - X_temp).T, params.E_obs_1),(params.c_obs_1 - X_temp))))+ \
c.exp(-(c.mtimes(c.mtimes((params.c_obs_2 - X_temp).T, params.E_obs_2),(params.c_obs_2 - X_temp)))) + \
c.exp(-(c.mtimes(c.mtimes((params.c_obs_3 - X_temp).T, params.E_obs_3),(params.c_obs_3 - X_temp)))) + \
c.exp(-(c.mtimes(c.mtimes((params.c_obs_4 - X_temp).T, params.E_obs_4),(params.c_obs_4 - X_temp)))))
cost = cost + obstacle_cost
X[:, i + 1] = robot.kinematics(X[:, i], U[:, i])
cost = cost + c.mtimes(c.mtimes((self.Xg - X[:, self.T]).T, self.Qf),
(self.Xg - X[:, self.T]))
return cost
def proc_model(self):
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
g = c.MX(self.nx,self.nu)
g[0,0] = c.cos(self.x[2]); g[0,1] = 0;
g[1,0] = c.sin(self.x[2]); g[1,1] = 0;
g[2,0] = c.tan(self.x[3])/self.length; g[2,1] = 0
g[3,0] = 0; g[3,1] = 1;
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
f = c.Function('f',[self.x,self.u],[self.x + c.mtimes(g,self.u)*self.dt])
# A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.x),
# c.jacobian(f(self.x,self.u)[1],self.x),
# c.jacobian(f(self.x,self.u)[2],self.x),
# c.jacobian(f(self.x,self.u)[3],self.x)]) #linearization
# B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.u),
# c.jacobian(f(self.x,self.u)[1],self.u),
# c.jacobian(f(self.x,self.u)[2],self.u),
# c.jacobian(f(self.x,self.u)[3],self.u)])
A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])
B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])
return f,A, B
def move(self, u, t, delta_t, is_pred=False, prev_x=None, prev_v=None):
# x'(t) = Ax(t) + Bu(t) assumes u(t) = u(t+delta_t)
# x(t + delta_t) = x(t) + delta_t*(Ax(t) + Bu(t))
prev_x = self.x_log[-1]
A = self.A
B = self.B
if is_pred:
prev_x = casadi.MX(prev_x)
A = casadi.MX(A)
B = casadi.MX(B)
else:
u = np.array(u).reshape(self.m)
x_new = prev_x + delta_t * (A @ prev_x + B @ u)
return x_new
def test_convert_array_symbols(self):
# Arrays
a = np.array([1, 2, 3, 4, 5])
pybamm_a = pybamm.Array(a)
self.assert_casadi_equal(pybamm_a.to_casadi(), casadi.MX(a))
casadi_t = casadi.MX.sym("t")
casadi_y = casadi.MX.sym("y", 10)
casadi_y_dot = casadi.MX.sym("y_dot", 10)
pybamm_t = pybamm.Time()
pybamm_y = pybamm.StateVector(slice(0, 10))
pybamm_y_dot = pybamm.StateVectorDot(slice(0, 10))
# Time
self.assertEqual(pybamm_t.to_casadi(casadi_t, casadi_y), casadi_t)
# State Vector
self.assert_casadi_equal(pybamm_y.to_casadi(casadi_t, casadi_y),
casadi_y)
# State Vector Dot
self.assert_casadi_equal(
pybamm_y_dot.to_casadi(casadi_t, casadi_y, casadi_y_dot),
casadi_y_dot)
def state_contraints(self, robot, U):
constraintVar = c.MX(
2 * 2, self.T
) #skipping orientation & steer angle. 2* to include min and max
U = c.reshape(U, robot.nu, self.T)
X = c.MX(robot.nx, self.T + 1)
X[:, 0] = self.X0
for i in range(self.T):
X[:, i + 1] = robot.kinematics(X[:, i], U[:, i])
constraintVar[:, i] = c.blockcat([[self.Xmax - X[0:2, i + 1]],
[X[0:2, i + 1] - self.Xmin]])
constraintVar = c.reshape(constraintVar, 1, 2 * 2 * self.T)
return constraintVar
def reduce_matvec(e, v):
"""
Reduces the MX graph e of linear operations on p into a matrix-vector product.
This reduces the number of nodes required to represent the linear operations.
"""
Af = ca.Function('Af', [ca.MX()], [ca.jacobian(e, v)])
A = Af(ca.DM())
return ca.reshape(ca.mtimes(A, v), e.shape)
