def _convert(self, symbol, t, y, y_dot, inputs):
""" See :meth:`CasadiConverter.convert()`. """
if isinstance(
symbol,
(
pybamm.Scalar,
pybamm.Array,
pybamm.Time,
pybamm.InputParameter,
pybamm.ExternalVariable,
),
):
return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))
elif isinstance(symbol, pybamm.StateVector):
if y is None:
raise ValueError("Must provide a 'y' for converting state vectors")
return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.StateVectorDot):
if y_dot is None:
raise ValueError("Must provide a 'y_dot' for converting state vectors")
return casadi.vertcat(*[y_dot[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.BinaryOperator):
left, right = symbol.children
# process children
converted_left = self.convert(left, t, y, y_dot, inputs)
converted_right = self.convert(right, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.Minimum):
return casadi.fmin(converted_left, converted_right)
if isinstance(symbol, pybamm.Maximum):
return casadi.fmax(converted_left, converted_right)
# _binary_evaluate defined in derived classes for specific rules
return symbol._binary_evaluate(converted_left, converted_right)
elif isinstance(symbol, pybamm.UnaryOperator):
converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.AbsoluteValue):
return casadi.fabs(converted_child)
return symbol._unary_evaluate(converted_child)
elif isinstance(symbol, pybamm.Function):
converted_children = [
self.convert(child, t, y, y_dot, inputs) for child in symbol.children
]
# Special functions
if symbol.function == np.min:
return casadi.mmin(*converted_children)
elif symbol.function == np.max:
return casadi.mmax(*converted_children)
elif symbol.function == np.abs:
return casadi.fabs(*converted_children)
elif symbol.function == np.sqrt:
return casadi.sqrt(*converted_children)
elif symbol.function == np.sin:
return casadi.sin(*converted_children)
elif symbol.function == np.arcsinh:
return casadi.arcsinh(*converted_children)
elif symbol.function == np.arccosh:
return casadi.arccosh(*converted_children)
elif symbol.function == np.tanh:
return casadi.tanh(*converted_children)
elif symbol.function == np.cosh:
return casadi.cosh(*converted_children)
elif symbol.function == np.sinh:
return casadi.sinh(*converted_children)
elif symbol.function == np.cos:
return casadi.cos(*converted_children)
elif symbol.function == np.exp:
return casadi.exp(*converted_children)
elif symbol.function == np.log:
return casadi.log(*converted_children)
elif symbol.function == np.sign:
return casadi.sign(*converted_children)
elif isinstance(symbol.function, (PchipInterpolator, CubicSpline)):
return casadi.interpolant("LUT", "bspline", [symbol.x], symbol.y)(
*converted_children
)
elif symbol.function.__name__.startswith("elementwise_grad_of_"):
differentiating_child_idx = int(symbol.function.__name__[-1])
# Create dummy symbolic variables in order to differentiate using CasADi
dummy_vars = [
casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children))
]
func_diff = casadi.gradient(
symbol.differentiated_function(*dummy_vars),
dummy_vars[differentiating_child_idx],
)
# Create function and evaluate it using the children
casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff])
return casadi_func_diff(*converted_children)
# Other functions
else:
return symbol._function_evaluate(converted_children)
elif isinstance(symbol, pybamm.Concatenation):
converted_children = [
self.convert(child, t, y, y_dot, inputs) for child in symbol.children
]
if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)):
return casadi.vertcat(*converted_children)
# DomainConcatenation specifies a particular ordering for the concatenation,
# which we must follow
elif isinstance(symbol, pybamm.DomainConcatenation):
slice_starts = []
all_child_vectors = []
for i in range(symbol.secondary_dimensions_npts):
child_vectors = []
for child_var, slices in zip(
converted_children, symbol._children_slices
):
for child_dom, child_slice in slices.items():
slice_starts.append(symbol._slices[child_dom][i].start)
child_vectors.append(
child_var[child_slice[i].start : child_slice[i].stop]
)
all_child_vectors.extend(
[v for _, v in sorted(zip(slice_starts, child_vectors))]
)
return casadi.vertcat(*all_child_vectors)
else:
raise TypeError(
"""
Cannot convert symbol of type '{}' to CasADi. Symbols must all be
'linear algebra' at this stage.
""".format(
type(symbol)
)
)
def build(self):
"""
Build the internal nonlinear program (NLP).
"""
self.nlp = NonlinearProgram(solver='ipopt', options=self.nlp_options)
foothold, next_foothold = self.foothold, self.next_foothold
omega2, omega = self.omega2, self.omega
dcm_target = list(self.dcm_target)
start_com = list(self.start_com)
start_comd = list(self.start_comd)
T = self.nlp.new_variable(
'T', 1, init=[self.desired_duration],
lb=[self.dT_min * self.nb_steps], ub=[5. * self.desired_duration])
p_0 = self.nlp.new_constant('p_0', 3, start_com)
pd_0 = self.nlp.new_constant('pd_0', 3, start_comd)
dT = T / self.nb_steps
p_k, pd_k = p_0, pd_0
W_comdd = list(self.weights['minimize_comdd'])
assert len(W_comdd) in [1, 3]
for k in range(self.nb_steps):
z_min = list(foothold.p - [0.42, 0.42, 0.42])
z_max = list(foothold.p + [0.42, 0.42, 0.42])
pdd_k = self.nlp.new_variable(
'pdd_%d' % k, 3, init=[0, 0, 0], lb=self.pdd_min,
ub=self.pdd_max)
z_k = self.nlp.new_variable(
'z_%d' % k, 3, init=list(foothold.p), lb=z_min, ub=z_max)
CZ_k = z_k - foothold.p
self.nlp.add_equality_constraint(
pdd_k, self.omega2 * (p_k - z_k) + gravity)
self.nlp.extend_cost(
self.weights['center_zmp'] * casadi.dot(CZ_k, CZ_k) * dT)
self.nlp.extend_cost(
casadi.dot(pdd_k, W_comdd * pdd_k) * dT)
# Exact integration by solving the first-order ODE
p_next = p_k + pd_k / omega * casadi.sinh(omega * dT) \
+ pdd_k / omega2 * (casadi.cosh(omega * dT) - 1.)
pd_next = pd_k * casadi.cosh(omega * dT) \
+ pdd_k / omega * casadi.sinh(omega * dT)
self.add_com_height_constraint(p_k, ref_height=0.8, max_dev=0.2)
self.add_friction_constraint(p_k, z_k, foothold)
self.add_linear_cop_constraints(p_k, z_k, foothold)
p_k = self.nlp.new_variable(
'p_%d' % (k + 1), 3, init=start_com, lb=self.p_min,
ub=self.p_max)
pd_k = self.nlp.new_variable(
'pd_%d' % (k + 1), 3, init=start_comd, lb=self.pd_min,
ub=self.pd_max)
self.nlp.add_equality_constraint(p_next, p_k)
self.nlp.add_equality_constraint(pd_next, pd_k)
p_last, pd_last = p_k, pd_k
dcm_last = p_last + pd_last / omega
cp_last = dcm_last + gravity / omega2
self.add_friction_constraint(p_last, cp_last, next_foothold)
self.add_linear_cop_constraints(p_last, cp_last, next_foothold)
dcm_error = dcm_last - dcm_target
duration_error = T - self.desired_duration
self.nlp.extend_cost(
self.weights['match_dcm'] * casadi.dot(dcm_error, dcm_error))
self.nlp.extend_cost(
self.weights['match_duration'] * duration_error ** 2)
self.nlp.create_solver()
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def __init__(self,
nb_steps,
com_state,
com_target,
contact_sequence,
omega2=None,
swing_duration=None):
super(COPPredictiveController, self).__init__()
end_com = list(com_target.p)
end_comd = list(com_target.pd)
nb_contacts = len(contact_sequence)
start_com = list(com_state.p)
start_comd = list(com_state.pd)
p_0 = self.nlp.new_constant('p_0', 3, start_com)
v_0 = self.nlp.new_constant('v_0', 3, start_comd)
p_k = p_0
v_k = v_0
T_swing = 0
T_total = 0
for k in xrange(nb_steps):
contact = contact_sequence[nb_contacts * k / nb_steps]
u_k = self.nlp.new_variable('u_%d' % k,
3,
init=[0, 0, 0],
lb=self.u_min,
ub=self.u_max)
dT_k = self.nlp.new_variable('dT_%d' % k,
1,
init=[self.dT_init],
lb=[self.dT_min],
ub=[self.dT_max])
alpha_k = self.nlp.new_variable('alpha_%d' % k,
1,
init=[0.],
lb=[-self.alpha_lim],
ub=[+self.alpha_lim])
beta_k = self.nlp.new_variable('beta_%d' % k,
1,
init=[0.],
lb=[-self.beta_lim],
ub=[+self.beta_lim])
lambda_k = self.nlp.new_variable('lambda_%d' % k,
1,
init=[self.lambda_init],
lb=[self.lambda_min],
ub=[self.lambda_max])
z_k = contact.p + alpha_k * contact.X * contact.t + \
beta_k * contact.Y * contact.b
self.nlp.add_equality_constraint(u_k,
lambda_k * (p_k - z_k) + gravity)
self.nlp.extend_cost(self.weights['alpha'] * alpha_k * alpha_k *
dT_k)
self.nlp.extend_cost(self.weights['beta'] * beta_k * beta_k * dT_k)
# self.nlp.extend_cost(
# self.weights['lambda'] * lambda_k ** 2 * dT_k)
self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) *
dT_k)
# Full precision (no Taylor expansion)
omega_k = casadi.sqrt(lambda_k)
p_next = p_k \
+ v_k / omega_k * casadi.sinh(omega_k * dT_k) \
+ u_k / lambda_k * (cosh(omega_k * dT_k) - 1.)
v_next = v_k * cosh(omega_k * dT_k) \
+ u_k / omega_k * sinh(omega_k * dT_k)
T_total = T_total + dT_k
if 2 * k < nb_steps:
T_swing = T_swing + dT_k
self.add_friction_constraint(contact, p_k, z_k)
self.add_friction_constraint(contact, p_next, z_k)
# slower:
# add_linear_friction_constraints(contact, p_k, z_k)
# add_linear_friction_constraints(contact, p_next, z_k)
p_k = self.nlp.new_variable('p_%d' % (k + 1),
3,
init=start_com,
lb=self.p_min,
ub=self.p_max)
v_k = self.nlp.new_variable('v_%d' % (k + 1),
3,
init=start_comd,
lb=self.v_min,
ub=self.v_max)
self.nlp.add_equality_constraint(p_next, p_k)
self.nlp.add_equality_constraint(v_next, v_k)
self.nlp.add_constraint(T_swing,
lb=[swing_duration],
ub=[100],
name='T_swing')
p_last, v_last = p_k, v_k
p_diff = p_last - end_com
v_diff = v_last - end_comd
self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff))
self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff))
self.nlp.extend_cost(self.weights['t'] * T_total)
self.nlp.create_solver()
#
self.com_target = com_target
self.end_com = array(end_com)
self.end_comd = array(end_comd)
self.nb_steps = nb_steps
self.preview = ZMPPreviewBuffer(contact_sequence)
def __init__(self,
nb_steps,
state_estimator,
com_target,
contact_sequence,
omega2,
swing_duration=None):
super(FIPPredictiveController, self).__init__()
t_build_start = time()
omega = sqrt(omega2)
end_com = list(com_target.p)
end_comd = list(com_target.pd)
nb_contacts = len(contact_sequence)
start_com = list(state_estimator.com)
start_comd = list(state_estimator.comd)
p_0 = self.nlp.new_constant('p_0', 3, start_com)
v_0 = self.nlp.new_constant('v_0', 3, start_comd)
p_k = p_0
v_k = v_0
T_swing = 0
T_total = 0
for k in xrange(nb_steps):
contact = contact_sequence[nb_contacts * k / nb_steps]
z_min = list(contact.p - [1, 1, 1]) # TODO: smarter
z_max = list(contact.p + [1, 1, 1])
u_k = self.nlp.new_variable('u_%d' % k,
3,
init=[0, 0, 0],
lb=self.u_min,
ub=self.u_max)
z_k = self.nlp.new_variable('z_%d' % k,
3,
init=list(contact.p),
lb=z_min,
ub=z_max)
dT_k = self.nlp.new_variable('dT_%d' % k,
1,
init=[self.dT_init],
lb=[self.dT_min],
ub=[self.dT_max])
CZ_k = z_k - contact.p
self.nlp.add_equality_constraint(u_k,
omega2 * (p_k - z_k) + gravity)
self.nlp.extend_cost(self.weights['zmp'] * casadi.dot(CZ_k, CZ_k) *
dT_k)
self.nlp.extend_cost(self.weights['acc'] * casadi.dot(u_k, u_k) *
dT_k)
# Full precision (no Taylor expansion)
p_next = p_k \
+ v_k / omega * casadi.sinh(omega * dT_k) \
+ u_k / omega2 * (cosh(omega * dT_k) - 1.)
v_next = v_k * cosh(omega * dT_k) \
+ u_k / omega * sinh(omega * dT_k)
T_total = T_total + dT_k
if 2 * k < nb_steps:
T_swing = T_swing + dT_k
self.add_com_boundary_constraint(contact, p_k)
self.add_friction_constraint(contact, p_k, z_k)
# self.add_friction_constraint(contact, p_next, z_k)
# self.add_linear_friction_constraints(contact, p_k, z_k)
# self.add_linear_friction_constraints(contact, p_next, z_k)
# self.add_cop_constraint(contact, p_k, z_k)
# self.add_cop_constraint(contact, p_next, z_k)
self.add_linear_cop_constraints(contact, p_k, z_k)
# self.add_linear_cop_constraints(contact, p_next, z_k)
p_k = self.nlp.new_variable('p_%d' % (k + 1),
3,
init=start_com,
lb=self.p_min,
ub=self.p_max)
v_k = self.nlp.new_variable('v_%d' % (k + 1),
3,
init=start_comd,
lb=self.v_min,
ub=self.v_max)
self.nlp.add_equality_constraint(p_next, p_k)
self.nlp.add_equality_constraint(v_next, v_k)
if swing_duration is not None:
self.nlp.add_constraint(T_swing,
lb=[swing_duration],
ub=[self.T_max],
name='T_swing')
p_last, v_last, z_last = p_k, v_k, z_k
cp_last = p_last + v_last / omega
cp_last = end_com + end_comd / omega + gravity / omega2
z_last = z_k
# last_contact = contact_sequence[-1]
# self.add_friction_constraint(last_contact, p_last, cp_last)
# self.add_linear_cop_constraints(last_contact, p_last, cp_last)
self.nlp.add_equality_constraint(z_last, cp_last)
p_diff = p_last - end_com
v_diff = v_last - end_comd
cp_diff = p_diff + v_diff / omega
# self.nlp.extend_cost(self.weights['pos'] * casadi.dot(p_diff, p_diff))
# self.nlp.extend_cost(self.weights['vel'] * casadi.dot(v_diff, v_diff))
self.nlp.extend_cost(self.weights['cp'] * casadi.dot(cp_diff, cp_diff))
self.nlp.extend_cost(self.weights['time'] * T_total)
self.nlp.create_solver()
#
self.build_time = time() - t_build_start
self.contact_sequence = contact_sequence
self.cp_error = 1e-6 # => self.is_ready_to_switch is initially True
self.end_com = array(end_com)
self.end_comd = array(end_comd)
self.nb_contacts = nb_contacts
self.nb_steps = nb_steps
self.omega = omega
self.omega2 = omega2
self.p_last = None
self.preview = ZMPPreviewBuffer(contact_sequence)
self.state_estimator = state_estimator
self.swing_dT_min = self.dT_min
self.swing_duration = swing_duration
self.v_last = None
