def apply_finish_line_constraints(
self, opti, endx, endy, endtheta, finish_line, direction, backwards=False
):
# Transform robot geometry to pose
# Only need to consider front or back edge, depending on direction
p1, p2 = self.GEOMETRY[1] if backwards else self.GEOMETRY[0]
x1, y1 = rotate_around_origin(p1, endtheta)
x2, y2 = rotate_around_origin(p2, endtheta)
x1 += endx
y1 += endy
x2 += endx
y2 += endy
# Enforce that at least one corner crosses the line
(x1_fin, y1_fin), (x2_fin, y2_fin) = finish_line
if direction in ["right", "left"]:
assert x1_fin == x2_fin
if direction == "right":
opti.subject_to(ca.fmax(x1, x2) > x1_fin)
else:
opti.subject_to(ca.fmin(x1, x2) < x1_fin)
elif direction in ["up", "down"]:
assert y1_fin == y2_fin
if direction == "up":
opti.subject_to(ca.fmax(y1, y2) > y1_fin)
else:
opti.subject_to(ca.fmin(y1, y2) < y1_fin)
else:
raise Exception(f"Unknown direction '{direction}")
def Min(x, y):
"""
!gets very imprecise if inputs outside of [-1e7,1e7]!
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:return: min(x, y)
:rtype: Union[float, Symbol]
"""
return ca.fmin(x, y)
def scattering_factor(elevation_angle):
"""
Calculates a scattering factor (a factor that gives losses due to atmospheric scattering at low elevation angles).
Source: AeroSandbox/studies/SolarPanelScattering
:param elevation_angle: Angle between the horizon and the sun [degrees]
:return: Fraction of the light that is not lost to scattering.
"""
elevation_angle = cas.fmin(cas.fmax(elevation_angle, 0), 90)
theta = 90 - elevation_angle # Angle between panel normal and the sun, in degrees
theta_rad = theta * cas.pi / 180
# # Model 1
# c = (
# 0.27891510500505767300438719757949,
# -0.015994330894744987481281839336589,
# -19.707332432605799255043166340329,
# -0.66260979582573353852126274432521
# )
# scattering_factor = c[0] + c[3] * theta_rad + cas.exp(
# c[1] * (
# cas.tan(theta_rad) + c[2] * theta_rad
# )
# )
# Model 2
c = (
-0.04636,
-0.3171
)
scattering_factor = cas.exp(
c[0] * (
cas.tan(theta_rad * 0.999) + c[1] * theta_rad
)
)
# # Model 3
# p1 = -21.74
# p2 = 282.6
# p3 = -1538
# p4 = 1786
# q1 = -923.2
# q2 = 1456
# x = theta_rad
# scattering_factor = ((p1*x**3 + p2*x**2 + p3*x + p4) /
# (x**2 + q1*x + q2))
# Keep this:
# scattering_factor = cas.fmin(cas.fmax(scattering_factor, 0), 1)
return scattering_factor
def smoothmax(value1, value2, hardness):
"""
A smooth maximum between two functions.
Useful because it's differentiable and convex!
Great writeup by John D Cook here:
https://www.johndcook.com/soft_maximum.pdf
:param value1: Value of function 1.
:param value2: Value of function 2.
:param hardness: Hardness parameter. Higher values make this closer to max(x1, x2).
:return: Soft maximum of the two supplied values.
"""
value1 = value1 * hardness
value2 = value2 * hardness
max = cas.fmax(value1, value2)
min = cas.fmin(value1, value2)
out = max + cas.log(1 + cas.exp(min - max))
out /= hardness
return out
def OPTController(self,e, tgt, C,maxCore):
print(e,tgt,C)
self.model = casadi.Opti()
nApp=len(tgt)
T=self.model.variable(1,nApp);
S=self.model.variable(1,nApp);
E_l1=self.model.variable(1,nApp);
self.model.subject_to(T>=0)
self.model.subject_to(self.model.bounded(0,S,maxCore))
self.model.subject_to(E_l1>=0)
sSum=0
obj=0;
for i in range(nApp):
sSum+=S[0,i]
#obj+=E_l1[0,i]
obj+=(T[0,i]/C[i]-1/tgt[i])**2
self.model.subject_to(sSum<=maxCore)
for i in range(nApp):
# optCTRL.addCons(T[i] <= S[i] / e[i])
# optCTRL.addCons(T[i] <= C[i] / e[i])
# optCTRL.addCons(T[i] >= S[i] / e[i] - C[i] / e[i] * D[i])
# optCTRL.addCons(T[i] >= C[i] / e[i] - C[i] / e[i] * (1 - D[i]))
# optCTRL.addCons(E_l1[i] >= ((C[i]/T[i])-tgt[i]))
# optCTRL.addCons(E_l1[i] >= -((C[i]/T[i])-tgt[i]))
self.model.subject_to(T[0,i]==casadi.fmin(S[0,i] / e[i],C[i] / e[i]))
# self.model.subject_to((E_l1[0,i]+tgt[i])>=((C[i]/T[0,i])))
# self.model.subject_to((E_l1[0,i]-tgt[i])>=-((C[i]/T[0,i])))
self.model.minimize(obj)
optionsIPOPT={'print_time':False,'ipopt':{'print_level':0}}
#self.model.solver('osqp',{'print_time':False,'error_on_fail':False})
self.model.solver('ipopt',optionsIPOPT)
sol=self.model.solve()
return sol.value(S).tolist()
def test_builtin(self):
with open(os.path.join(TEST_DIR, 'BuiltinFunctions.mo'), 'r') as f:
txt = f.read()
ast_tree = parser.parse(txt)
casadi_model = gen_casadi.generate(ast_tree, 'BuiltinFunctions')
print("BuiltinFunctions", casadi_model)
ref_model = Model()
x = ca.MX.sym("x")
y = ca.MX.sym("y")
z = ca.MX.sym("z")
w = ca.MX.sym("w")
u = ca.MX.sym("u")
ref_model.inputs = list(map(Variable, [x]))
ref_model.outputs = list(map(Variable, [y, z, w, u]))
ref_model.alg_states = list(map(Variable, [y, z, w, u]))
ref_model.equations = [y - ca.sin(ref_model.time), z - ca.cos(x), w - ca.fmin(y, z), u - ca.fabs(w)]
self.assert_model_equivalent_numeric(ref_model, casadi_model)
def calculate_optimal_control(self):
dd_h_dudu, d_h_du = hessian(self.problem.H, self.model.u)
if is_equal(dd_h_dudu, DM.zeros(self.model.n_u, self.model.n_u)):
# TODO: Implement the case where the controls are linear on the Hamiltonian ("Bang-Bang" control)
raise Exception(
'The Hamiltonian "H" is not strictly convex with respect to the control "u". '
+ 'The obtained hessian d^2 H/du^2 = 0')
# if not ddH_dudu.is_constant():
# raise NotImplementedError('The Hessian of the Hamiltonian with respect to "u" is not constant,
# this case has not been implemented')
u_opt = -mtimes(inv(dd_h_dudu), substitute(d_h_du, self.model.u, 0))
for i in range(self.model.n_u):
if not self.problem.u_min[i] == -inf:
u_opt[i] = fmax(u_opt[i], self.problem.u_min[i])
if not self.problem.u_max[i] == inf:
u_opt[i] = fmin(u_opt[i], self.problem.u_max[i])
return u_opt
def apply_obstacle_constraints(self, opti, xpos, ypos, theta, obstacles):
for obx, oby, obr in obstacles:
for p1, p2 in self.GEOMETRY:
# Transform robot geometry to pose
x1, y1 = rotate_around_origin(p1, theta)
x2, y2 = rotate_around_origin(p2, theta)
x1 += xpos
y1 += ypos
x2 += xpos
y2 += ypos
# Compute the closest distance between a point and a line segment
px = x2 - x1
py = y2 - y1
norm = px * px + py * py
u = ((obx - x1) * px + (oby - y1) * py) / norm
u = ca.fmax(ca.fmin(u, 1), 0)
x = x1 + u * px
y = y1 + u * py
dx = x - obx
dy = y - oby
dist = np.sqrt((dx * dx + dy * dy))
opti.subject_to(dist > obr + self.OBSTACLE_BUFFER)
def simplify(self, options):
if options.get('replace_parameter_expressions', False):
logger.info("Replacing parameter expressions")
simple_parameters, symbols, values = [], [], []
for p in self.parameters:
if isinstance(p.value, list):
p.value = np.array(p.value)
if not np.issubdtype(p.value.dtype, np.number):
raise NotImplementedError(
"Only parameters arrays with numeric values can be simplified")
simple_parameters.append(p)
else:
value = ca.MX(p.value)
if value.is_constant():
simple_parameters.append(p)
else:
symbols.append(p.symbol)
values.append(value)
self.parameters = simple_parameters
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values), ca.veccat(*new_values), CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, symbols, values)
# Replace parameter expressions in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_expressions', False):
logger.info("Replacing constant expressions")
simple_constants, symbols, values = [], [], []
for c in self.constants:
value = ca.MX(c.value)
if value.is_constant():
simple_constants.append(c)
else:
symbols.append(c.symbol)
values.append(value)
self.constants = simple_constants
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values), ca.veccat(*new_values), CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, symbols, values)
# Replace constant expressions in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminate_constant_assignments', False):
logger.info("Elimating constant variable assignments")
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states:
constant = alg_states.pop(eq.name())
constant.value = 0.0
self.constants.append(constant)
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name() in alg_states and eq.dep(1).is_constant():
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name() in alg_states and eq.dep(0).is_constant():
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
constant = alg_states.pop(variable.name())
if eq.is_op(ca.OP_SUB):
constant.value = value
else:
constant.value = -value
self.constants.append(constant)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if options.get('replace_parameter_values', False):
logger.info("Replacing parameter values")
# N.B. Any parameter expression elimination must be done first.
unspecified_parameters, symbols, values = [], [], []
for p in self.parameters:
if ca.MX(p.value).is_constant() and ca.MX(p.value).is_regular():
symbols.append(p.symbol)
values.append(p.value)
else:
unspecified_parameters.append(p)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
self.parameters = unspecified_parameters
# Replace parameter values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_values', False):
logger.info("Replacing constant values")
# N.B. Any parameter expression elimination must be done first.
symbols = self._symbols(self.constants)
values = [v.value for v in self.constants]
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, symbols, values)
self.constants = []
# Replace constant values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminable_variable_expression', None) is not None:
logger.info("Elimating variables that match the regular expression {}".format(options['eliminable_variable_expression']))
p = re.compile(options['eliminable_variable_expression'])
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
variables = []
values = []
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states and p.match(eq.name()):
variables.append(eq)
values.append(0.0)
del alg_states[eq.name()]
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name() in alg_states and p.match(eq.dep(0).name()):
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name() in alg_states and p.match(eq.dep(1).name()):
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
del alg_states[variable.name()]
variables.append(variable)
if eq.is_op(ca.OP_SUB):
values.append(value)
else:
values.append(-value)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, variables, values)
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, variables, values)
if options.get('expand_vectors', False):
logger.info("Expanding vectors")
symbols = []
values = []
for l in ['states', 'der_states', 'alg_states', 'inputs', 'parameters', 'constants']:
old_vars = getattr(self, l)
new_vars = []
for old_var in old_vars:
# For delayed states we do not have any reliable shape
# information available due to it being an arbitrary
# expression, so we just always expand.
if (old_var.symbol._modelica_shape != (1,) or old_var.symbol.name() in self.delay_states):
expanded_symbols = []
for ind in np.ndindex(old_var.symbol._modelica_shape):
component_symbol = ca.MX.sym('{}[{}]'.format(old_var.symbol.name(), ",".join(str(i+1) for i in ind)))
component_var = Variable(component_symbol, old_var.python_type)
for attribute in CASADI_ATTRIBUTES:
# Can't convert 3D arrays to MX, so we convert to nparray instead
value = getattr(old_var, attribute)
if not isinstance(value, ca.MX) and not np.isscalar(value):
value = np.array(value)
else:
value = ca.MX(getattr(old_var, attribute))
if np.prod(value.shape) == 1:
setattr(component_var, attribute, value)
else:
setattr(component_var, attribute, value[ind])
expanded_symbols.append(component_var)
s = old_var.symbol._mx if isinstance(old_var.symbol, _MTensor) else old_var.symbol
symbols.append(s)
values.append(ca.reshape(ca.vertcat(*[x.symbol for x in expanded_symbols]), *tuple(reversed(s.shape))).T)
new_vars.extend(expanded_symbols)
# Replace variable in delay expressions and durations if needed
try:
assert len(self.delay_states) == len(self.delay_arguments)
i = self.delay_states.index(old_var.symbol.name())
except ValueError:
pass
else:
delay_state = self.delay_states.pop(i)
delay_argument = self.delay_arguments.pop(i)
for ind in np.ndindex(old_var.symbol._modelica_shape):
new_name = '{}[{}]'.format(delay_state, ",".join(str(i+1) for i in ind))
self.delay_states.append(new_name)
self.delay_arguments.append(
DelayArgument(delay_argument.expr[ind], delay_argument.duration))
# Replace variable in list of outputs if needed
try:
i = self.outputs.index(old_var.symbol.name())
except ValueError:
pass
else:
self.outputs.pop(i)
for new_s in reversed(expanded_symbols):
self.outputs.insert(i, new_s.symbol.name())
else:
new_vars.append(old_var)
setattr(self, l, new_vars)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
self.equations = list(itertools.chain.from_iterable(ca.vertsplit(ca.vec(eq)) for eq in self.equations))
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
self.initial_equations = list(itertools.chain.from_iterable(ca.vertsplit(ca.vec(eq)) for eq in self.initial_equations))
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, symbols, values)
# Make sure that the naming in the main loop and the delay argument loop match
input_names = [v.symbol.name() for v in self.inputs]
assert set(self.delay_states).issubset(input_names)
# Replace values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('factor_and_simplify_equations', False):
# Operations that preserve the equivalence of an equation
# TODO: There may be more, but this is the most frequent set
unary_ops = ca.OP_NEG, ca.OP_FABS, ca.OP_SQRT
binary_ops = ca.OP_MUL, ca.OP_DIV
# Recursive factor and simplify function
def factor_and_simplify(eq):
# These are ops that can simply be dropped
if eq.n_dep() == 1 and eq.op() in unary_ops:
return factor_and_simplify(eq.dep())
# These are binary ops and can get a little tricky
# For now, we just drop constant divisors or multipliers
elif eq.n_dep() == 2 and eq.op() in binary_ops:
if eq.dep(1).is_constant():
return factor_and_simplify(eq.dep(0))
elif eq.dep(0).is_constant() and eq.op() == ca.OP_MUL:
return factor_and_simplify(eq.dep(1))
# If no hits, return unmodified
return eq
# Do the simplifications
simplified_equations = [factor_and_simplify(eq) for eq in self.equations]
# Debugging output
if logger.getEffectiveLevel() == logging.DEBUG:
changed_equations = [(o,s) for o, s in zip(self.equations, simplified_equations) if o is not s]
for orig, simp in changed_equations:
logger.debug('Equation {} simplified to {}'.format(orig, simp))
# Store changes
self.equations = simplified_equations
if options.get('detect_aliases', False):
logger.info("Detecting aliases")
states = OrderedDict([(s.symbol.name(), s) for s in self.states])
der_states = OrderedDict([(s.symbol.name(), s) for s in self.der_states])
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
inputs = OrderedDict([(s.symbol.name(), s) for s in self.inputs])
parameters = OrderedDict([(s.symbol.name(), s) for s in self.parameters])
all_states = OrderedDict()
all_states.update(states)
all_states.update(der_states)
all_states.update(alg_states)
all_states.update(inputs)
all_states.update(parameters)
alias_rel = AliasRelation()
# For now, we only eliminate algebraic states.
do_not_eliminate = set(list(der_states) + list(states) + list(inputs) + list(parameters))
reduced_equations = []
for eq in self.equations:
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(1).is_symbolic():
if eq.dep(0).name() in alg_states:
alg_state = eq.dep(0)
other_state = eq.dep(1)
elif eq.dep(1).name() in alg_states:
alg_state = eq.dep(1)
other_state = eq.dep(0)
else:
alg_state = None
other_state = None
# If both states are algebraic, we need to decide which to eliminate
if eq.dep(0).name() in alg_states and eq.dep(1).name() in alg_states:
# Most of the time it does not matter which one we eliminate.
# The exception is if alg_state has already been aliased to a
# variable in do_not_eliminate. If this is the case, setting the
# states in the default order will cause the new canonical variable
# to be other_state, unseating (and eliminating) the current
# canonical variable (which is in do_not_eliminate).
if alias_rel.canonical_signed(alg_state.name())[0] in do_not_eliminate:
# swap the states
other_state, alg_state = alg_state, other_state
if alg_state is not None:
# Check to see if we are linking two entries in do_not_eliminate
if alias_rel.canonical_signed(alg_state.name())[0] in do_not_eliminate and \
alias_rel.canonical_signed(other_state.name())[0] in do_not_eliminate:
# Don't do anything for now, we only eliminate alg_states
pass
else:
# Eliminate alg_state by aliasing it to other_state
if eq.is_op(ca.OP_SUB):
alias_rel.add(other_state.name(), alg_state.name())
else:
alias_rel.add(other_state.name(), '-' + alg_state.name())
# To keep equations balanced, drop this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
variables, values = [], []
for canonical, aliases in alias_rel:
canonical_state = all_states[canonical]
python_type = canonical_state.python_type
start = canonical_state.start
m, M = canonical_state.min, canonical_state.max
nominal = canonical_state.nominal
fixed = canonical_state.fixed
for alias in aliases:
if alias[0] == '-':
sign = -1
alias = alias[1:]
else:
sign = 1
alias_state = all_states[alias]
variables.append(alias_state.symbol)
values.append(sign * canonical_state.symbol)
# If any of the aliases has a nonstandard type, apply it to
# the canonical state as well
if alias_state.python_type != float:
python_type = alias_state.python_type
# If any of the aliases has a nondefault start value, apply it to
# the canonical state as well
alias_state_start = ca.MX(alias_state.start)
if alias_state_start.is_regular() and not alias_state_start.is_zero():
start = sign * alias_state.start
# The intersection of all bound ranges applies
m = ca.fmax(m, alias_state.min if sign == 1 else -alias_state.max)
M = ca.fmin(M, alias_state.max if sign == 1 else -alias_state.min)
# Take the largest nominal of all aliases
nominal = ca.fmax(nominal, alias_state.nominal)
# If any of the aliases is fixed, the canonical state is as well
fixed = ca.fmax(fixed, alias_state.fixed)
del all_states[alias]
canonical_state.aliases = aliases
canonical_state.python_type = python_type
canonical_state.start = start
canonical_state.min = m
canonical_state.max = M
canonical_state.nominal = nominal
canonical_state.fixed = fixed
self.states = [v for k, v in all_states.items() if k in states]
self.der_states = [v for k, v in all_states.items() if k in der_states]
self.alg_states = [v for k, v in all_states.items() if k in alg_states]
self.inputs = [v for k, v in all_states.items() if k in inputs]
self.parameters = [v for k, v in all_states.items() if k in parameters]
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, variables, values)
if len(self.delay_arguments) > 0:
self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, variables, values)
if options.get('reduce_affine_expression', False):
logger.info("Collapsing model into an affine expression")
for equation_list in ['equations', 'initial_equations']:
equations = getattr(self, equation_list)
if len(equations) > 0:
states = ca.veccat(*self._symbols(itertools.chain(self.states, self.der_states, self.alg_states, self.inputs)))
constants = ca.veccat(*self._symbols(self.constants))
parameters = ca.veccat(*self._symbols(self.parameters))
equations = ca.veccat(*equations)
Af = ca.Function('Af', [states, constants, parameters], [ca.jacobian(equations, states)])
bf = ca.Function('bf', [states, constants, parameters], [equations])
# Work around CasADi issue #172
if len(self.constants) == 0 or not ca.depends_on(equations, constants):
constants = 0
else:
logger.warning('Not all constants have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.')
if len(self.parameters) == 0 or not ca.depends_on(equations, parameters):
parameters = 0
else:
logger.warning('Not all parameters have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.')
A = Af(0, constants, parameters)
b = bf(0, constants, parameters)
# Replace veccat'ed states with brand new state vectors so as to avoid the value copy operations induced by veccat.
self._states_vector = ca.MX.sym('states_vector', sum([s.numel() for s in self._symbols(self.states)]))
self._der_states_vector = ca.MX.sym('der_states_vector', sum([s.numel() for s in self._symbols(self.der_states)]))
self._alg_states_vector = ca.MX.sym('alg_states_vector', sum([s.numel() for s in self._symbols(self.alg_states)]))
self._inputs_vector = ca.MX.sym('inputs_vector', sum([s.numel() for s in self._symbols(self.inputs)]))
states_vector = ca.vertcat(self._states_vector, self._der_states_vector, self._alg_states_vector, self._inputs_vector)
equations = [ca.reshape(ca.mtimes(A, states_vector), equations.shape) + b]
setattr(self, equation_list, equations)
if options.get('expand_mx', False):
logger.info("Expanded MX functions will be returned")
self._expand_mx_func = lambda x: x.expand()
logger.info("Finished model simplification")
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)
)
)
T_left_base = cs.vertcat(cs.horzcat(R_torso, t_left), cs.horzcat(0, 0, 0, 1))
T_right_base = cs.vertcat(cs.horzcat(R_torso, t_right), cs.horzcat(0, 0, 0, 1))
fk_vals1 = T_left_base@fk_vals1
fk_vals2 = T_right_base@fk_vals2
# fk_vals2[1,3] += 0.3 #accounting for the base offset in the y-direction
print(robot.fk(q0_1)[6])
print(robot2.fk(q0_2)[6])
#specifying the cartesian trajectory of the two robots as a function
#of the progress variable
rob1_start_pose = cs.DM([0.3, -0.24, 0.4])
rob1_end_pose = cs.DM([0.6, 0.2, 0.25])
traj1 = rob1_start_pose + cs.fmin(s_1, 1)*(rob1_end_pose - rob1_start_pose)
rob2_start_pose = cs.DM([0.3, 0.54, 0.4])
rob2_end_pose = cs.DM([0.6, 0.1, 0.25])
traj2 = rob2_start_pose + cs.fmin(s_2, 1)*(rob2_end_pose - rob2_start_pose)
#Highest priority: Hard constraints on joint velocity and joint position limits
# hlp.create_constraint(q1 + q_dot1*ts, 'lub', 0, options = {'lb':robot.joint_lb, 'ub':robot.joint_ub})
# hlp.create_constraint(q2 + q_dot2*ts, 'lub', 0, options = {'lb':robot.joint_lb, 'ub':robot.joint_ub})
hlp.create_constraint(s_1 + s_dot1*ts, 'lub', 0, options = {'lb':0, 'ub':1})
hlp.create_constraint(s_2 + s_dot2*ts, 'lub', 0, options = {'lb':0, 'ub':1})
#Adding bounds on the acceleration
q_dot1_prev = hlp.create_parameter(7)# hqp.opti.parameter(7, 1)
q_dot2_prev = hlp.create_parameter(7)#hqp.opti.parameter(7, 1)
q_torso_dot_prev = hlp.create_parameter(1)#hqp.opti.parameter(7, 1)
def simplify(self, options):
if options.get('replace_parameter_expressions', False):
logger.info("Replacing parameter expressions")
simple_parameters, symbols, values = [], [], []
for p in self.parameters:
value = ca.MX(p.value)
if value.is_constant():
simple_parameters.append(p)
else:
symbols.append(p.symbol)
values.append(value)
self.parameters = simple_parameters
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values), ca.veccat(*new_values), CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
# Replace parameter expressions in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in ast.Symbol.ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_expressions', False):
logger.info("Replacing constant expressions")
simple_constants, symbols, values = [], [], []
for c in self.constants:
value = ca.MX(c.value)
if value.is_constant():
simple_constants.append(c)
else:
symbols.append(c.symbol)
values.append(value)
self.constants = simple_constants
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values), ca.veccat(*new_values), CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
# Replace constant expressions in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in ast.Symbol.ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminate_constant_assignments', False):
logger.info("Elimating constant variable assignments")
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states:
constant = alg_states.pop(eq.name())
constant.value = 0.0
self.constants.append(constant)
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name() in alg_states and eq.dep(1).is_constant():
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name() in alg_states and eq.dep(0).is_constant():
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
constant = alg_states.pop(variable.name())
if eq.is_op(ca.OP_SUB):
constant.value = value
else:
constant.value = -value
self.constants.append(constant)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if options.get('replace_parameter_values', False):
logger.info("Replacing parameter values")
# N.B. Any parameter expression elimination must be done first.
unspecified_parameters, symbols, values = [], [], []
for p in self.parameters:
if ca.MX(p.value).is_constant() and ca.MX(p.value).is_regular():
symbols.append(p.symbol)
values.append(p.value)
else:
unspecified_parameters.append(p)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
self.parameters = unspecified_parameters
# Replace parameter values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in ast.Symbol.ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_values', False):
logger.info("Replacing constant values")
# N.B. Any parameter expression elimination must be done first.
symbols = self._symbols(self.constants)
values = [v.value for v in self.constants]
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
self.constants = []
# Replace constant values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in ast.Symbol.ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminable_variable_expression', None) is not None:
logger.info("Elimating variables that match the regular expression {}".format(options['eliminable_variable_expression']))
p = re.compile(options['eliminable_variable_expression'])
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
variables = []
values = []
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states and p.match(eq.name()):
variables.append(eq)
values.append(0.0)
del alg_states[eq.name()]
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name() in alg_states and p.match(eq.dep(0).name()):
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name() in alg_states and p.match(eq.dep(1).name()):
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
del alg_states[variable.name()]
variables.append(variable)
if eq.is_op(ca.OP_SUB):
values.append(value)
else:
values.append(-value)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, variables, values)
if options.get('expand_vectors', False):
logger.info("Expanding vectors")
symbols = []
values = []
for l in ['states', 'der_states', 'alg_states', 'inputs', 'parameters', 'constants']:
old_vars = getattr(self, l)
new_vars = []
for old_var in old_vars:
if old_var.symbol.numel() > 1:
rows = []
for i in range(old_var.symbol.size1()):
cols = []
for j in range(old_var.symbol.size2()):
if old_var.symbol.size1() > 1 and old_var.symbol.size2() > 1:
component_symbol = ca.MX.sym('{}[{},{}]'.format(old_var.symbol.name(), i, j))
elif old_var.symbol.size1() > 1:
component_symbol = ca.MX.sym('{}[{}]'.format(old_var.symbol.name(), i))
elif old_var.symbol.size2() > 1:
component_symbol = ca.MX.sym('{}[{}]'.format(old_var.symbol.name(), j))
else:
raise AssertionError
component_var = Variable(component_symbol, old_var.python_type)
for attribute in ast.Symbol.ATTRIBUTES:
value = ca.MX(getattr(old_var, attribute))
if value.size1() == old_var.symbol.size1() and value.size2() == old_var.symbol.size2():
setattr(component_var, attribute, value[i, j])
elif value.size1() == old_var.symbol.size1():
setattr(component_var, attribute, value[i])
elif value.size2() == old_var.symbol.size2():
setattr(component_var, attribute, value[j])
else:
assert value.size1() == 1
assert value.size2() == 1
setattr(component_var, attribute, value)
cols.append(component_var)
rows.append(cols)
symbols.append(old_var.symbol)
values.append(ca.vertcat(*[ca.horzcat(*[v.symbol for v in row]) for row in rows]))
new_vars.extend(itertools.chain.from_iterable(rows))
else:
new_vars.append(old_var)
setattr(self, l, new_vars)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
self.equations = list(itertools.chain.from_iterable(ca.vertsplit(ca.vec(eq)) for eq in self.equations))
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, symbols, values)
self.initial_equations = list(itertools.chain.from_iterable(ca.vertsplit(ca.vec(eq)) for eq in self.initial_equations))
# Replace values in metadata
for variable in itertools.chain(self.states, self.alg_states, self.inputs, self.parameters, self.constants):
for attribute in ast.Symbol.ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('detect_aliases', False):
logger.info("Detecting aliases")
states = OrderedDict([(s.symbol.name(), s) for s in self.states])
der_states = OrderedDict([(s.symbol.name(), s) for s in self.der_states])
alg_states = OrderedDict([(s.symbol.name(), s) for s in self.alg_states])
inputs = OrderedDict([(s.symbol.name(), s) for s in self.inputs])
all_states = OrderedDict()
all_states.update(states)
all_states.update(der_states)
all_states.update(alg_states)
all_states.update(inputs)
alias_rel = AliasRelation()
# For now, we only eliminate algebraic states.
do_not_eliminate = set(list(der_states) + list(states) + list(inputs))
reduced_equations = []
for eq in self.equations:
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB) or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(1).is_symbolic():
if eq.dep(0).name() in alg_states:
alg_state = eq.dep(0)
other_state = eq.dep(1)
elif eq.dep(1).name() in alg_states:
alg_state = eq.dep(1)
other_state = eq.dep(0)
else:
alg_state = None
other_state = None
# If both states are algebraic, we need to decide which to eliminate
if eq.dep(0).name() in alg_states and eq.dep(1).name() in alg_states:
# Most of the time it does not matter which one we eliminate.
# The exception is if alg_state has already been aliased to a
# variable in do_not_eliminate. If this is the case, setting the
# states in the default order will cause the new canonical variable
# to be other_state, unseating (and eliminating) the current
# canonical variable (which is in do_not_eliminate).
if alias_rel.canonical_signed(alg_state.name())[0] in do_not_eliminate:
# swap the states
other_state, alg_state = alg_state, other_state
if alg_state is not None:
# Check to see if we are linking two entries in do_not_eliminate
if alias_rel.canonical_signed(alg_state.name())[0] in do_not_eliminate and \
alias_rel.canonical_signed(other_state.name())[0] in do_not_eliminate:
# Don't do anything for now, we only eliminate alg_states
pass
else:
# Eliminate alg_state by aliasing it to other_state
if eq.is_op(ca.OP_SUB):
alias_rel.add(other_state.name(), alg_state.name())
else:
alias_rel.add(other_state.name(), '-' + alg_state.name())
# To keep equations balanced, drop this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
variables, values = [], []
for canonical, aliases in alias_rel:
canonical_state = all_states[canonical]
python_type = canonical_state.python_type
start = canonical_state.start
m, M = canonical_state.min, canonical_state.max
nominal = canonical_state.nominal
fixed = canonical_state.fixed
for alias in aliases:
if alias[0] == '-':
sign = -1
alias = alias[1:]
else:
sign = 1
alias_state = all_states[alias]
variables.append(alias_state.symbol)
values.append(sign * canonical_state.symbol)
# If any of the aliases has a nonstandard type, apply it to
# the canonical state as well
if alias_state.python_type != float:
python_type = alias_state.python_type
# If any of the aliases has a nondefault start value, apply it to
# the canonical state as well
alias_state_start = ca.MX(alias_state.start)
if alias_state_start.is_regular() and not alias_state_start.is_zero():
start = sign * alias_state.start
# The intersection of all bound ranges applies
m = ca.fmax(m, alias_state.min if sign == 1 else -alias_state.max)
M = ca.fmin(M, alias_state.max if sign == 1 else -alias_state.min)
# Take the largest nominal of all aliases
nominal = ca.fmax(nominal, alias_state.nominal)
# If any of the aliases is fixed, the canonical state is as well
fixed = ca.fmax(fixed, alias_state.fixed)
del all_states[alias]
canonical_state.aliases = aliases
canonical_state.python_type = python_type
canonical_state.start = start
canonical_state.min = m
canonical_state.max = M
canonical_state.nominal = nominal
canonical_state.fixed = fixed
self.states = [v for k, v in all_states.items() if k in states]
self.der_states = [v for k, v in all_states.items() if k in der_states]
self.alg_states = [v for k, v in all_states.items() if k in alg_states]
self.inputs = [v for k, v in all_states.items() if k in inputs]
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations, variables, values)
if options.get('reduce_affine_expression', False):
logger.info("Collapsing model into an affine expression")
for equation_list in ['equations', 'initial_equations']:
equations = getattr(self, equation_list)
if len(equations) > 0:
states = ca.veccat(*self._symbols(itertools.chain(self.states, self.der_states, self.alg_states, self.inputs)))
constants = ca.veccat(*self._symbols(self.constants))
parameters = ca.veccat(*self._symbols(self.parameters))
equations = ca.veccat(*equations)
Af = ca.Function('Af', [states, constants, parameters], [ca.jacobian(equations, states)])
bf = ca.Function('bf', [states, constants, parameters], [equations])
# Work around CasADi issue #172
if len(self.constants) == 0 or not ca.depends_on(equations, constants):
constants = 0
else:
logger.warning('Not all constants have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.')
if len(self.parameters) == 0 or not ca.depends_on(equations, parameters):
parameters = 0
else:
logger.warning('Not all parameters have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.')
A = Af(0, constants, parameters)
b = bf(0, constants, parameters)
# Replace veccat'ed states with brand new state vectors so as to avoid the value copy operations induced by veccat.
self._states_vector = ca.MX.sym('states_vector', sum([s.numel() for s in self._symbols(self.states)]))
self._der_states_vector = ca.MX.sym('der_states_vector', sum([s.numel() for s in self._symbols(self.der_states)]))
self._alg_states_vector = ca.MX.sym('alg_states_vector', sum([s.numel() for s in self._symbols(self.alg_states)]))
self._inputs_vector = ca.MX.sym('inputs_vector', sum([s.numel() for s in self._symbols(self.inputs)]))
states_vector = ca.vertcat(self._states_vector, self._der_states_vector, self._alg_states_vector, self._inputs_vector)
equations = [ca.reshape(ca.mtimes(A, states_vector), equations.shape) + b]
setattr(self, equation_list, equations)
if options.get('expand_mx', False):
logger.info("Expanding MX graph")
if len(self.equations) > 0:
self.equations = ca.matrix_expand(self.equations)
if len(self.initial_equations) > 0:
self.initial_equations = ca.matrix_expand(self.initial_equations)
logger.info("Finished model simplification")
def opt_mintime(reftrack: np.ndarray,
coeffs_x: np.ndarray,
coeffs_y: np.ndarray,
normvectors: np.ndarray,
pars: dict,
tpamap_path: str,
tpadata_path: str,
export_path: str,
print_debug: bool = False,
plot_debug: bool = False) -> tuple:
"""
Created by:
Fabian Christ
Documentation:
The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using
direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing
times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using
the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The
vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and
nonlinear tire model.
Please refer to our paper for further information:
Christ, Wischnewski, Heilmeier, Lohmann
Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients
Inputs:
reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m]
coeffs_x: coefficient matrix of the x splines with size (no_splines x 4)
coeffs_y: coefficient matrix of the y splines with size (no_splines x 4)
normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component]
pars: parameters dictionary
tpamap_path: file path to tpa map (required for friction map loading)
tpadata_path: file path to tpa data (required for friction map loading)
export_path: path to output folder for warm start files and solution files
print_debug: determines if debug messages are printed
plot_debug: determines if debug plots are shown
Outputs:
alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point
v_opt: velocity profile for the raceline
"""
# ------------------------------------------------------------------------------------------------------------------
# PREPARE TRACK INFORMATION ----------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# spline lengths
spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(
coeffs_x=coeffs_x, coeffs_y=coeffs_y)
# calculate heading and curvature (numerically)
kappa_refline = tph.calc_head_curv_num. \
calc_head_curv_num(path=reftrack[:, :2],
el_lengths=spline_lengths_refline,
is_closed=True,
stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"],
stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"],
stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"],
stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1]
# close track
kappa_refline_cl = np.append(kappa_refline, kappa_refline[0])
w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3])
w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2])
# step size along the reference line
h = pars["stepsize_opts"]["stepsize_reg"]
# optimization steps (0, 1, 2 ... end point/start point)
steps = [i for i in range(kappa_refline_cl.size)]
# number of control intervals
N = steps[-1]
# station along the reference line
s_opt = np.linspace(0.0, N * h, N + 1)
# interpolate curvature of reference line in terms of steps
kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps],
kappa_refline_cl)
# interpolate track width (left and right to reference line) in terms of steps
w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps],
w_tr_left_cl)
w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps],
w_tr_right_cl)
# describe friction coefficients from friction map with linear equations or gaussian basis functions
if pars["optim_opts"]["var_friction"] is not None:
w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \
approx_friction_map.approx_friction_map(reftrack=reftrack,
normvectors=normvectors,
tpamap_path=tpamap_path,
tpadata_path=tpadata_path,
pars=pars,
dn=pars["optim_opts"]["dn"],
n_gauss=pars["optim_opts"]["n_gauss"],
print_debug=print_debug,
plot_debug=plot_debug)
# ------------------------------------------------------------------------------------------------------------------
# DIRECT GAUSS-LEGENDRE COLLOCATION --------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# degree of interpolating polynomial
d = 3
# legendre collocation points
tau = np.append(0, ca.collocation_points(d, 'legendre'))
# coefficient matrix for formulating the collocation equation
C = np.zeros((d + 1, d + 1))
# coefficient matrix for formulating the collocation equation
D = np.zeros(d + 1)
# coefficient matrix for formulating the collocation equation
B = np.zeros(d + 1)
# construct polynomial basis
for j in range(d + 1):
# construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# evaluate polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation
p_der = np.polyder(p)
for r in range(d + 1):
C[j, r] = p_der(tau[r])
# evaluate integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
# ------------------------------------------------------------------------------------------------------------------
# STATE VARIABLES --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of state variables
nx = 5
# velocity [m/s]
v_n = ca.SX.sym('v_n')
v_s = 50
v = v_s * v_n
# side slip angle [rad]
beta_n = ca.SX.sym('beta_n')
beta_s = 0.5
beta = beta_s * beta_n
# yaw rate [rad/s]
omega_z_n = ca.SX.sym('omega_z_n')
omega_z_s = 1
omega_z = omega_z_s * omega_z_n
# lateral distance to reference line (positive = left) [m]
n_n = ca.SX.sym('n_n')
n_s = 5.0
n = n_s * n_n
# relative angle to tangent on reference line [rad]
xi_n = ca.SX.sym('xi_n')
xi_s = 1.0
xi = xi_s * xi_n
# scaling factors for state variables
x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s])
# put all states together
x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n)
# ------------------------------------------------------------------------------------------------------------------
# CONTROL VARIABLES ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of control variables
nu = 4
# steer angle [rad]
delta_n = ca.SX.sym('delta_n')
delta_s = 0.5
delta = delta_s * delta_n
# positive longitudinal force (drive) [N]
f_drive_n = ca.SX.sym('f_drive_n')
f_drive_s = 7500.0
f_drive = f_drive_s * f_drive_n
# negative longitudinal force (brake) [N]
f_brake_n = ca.SX.sym('f_brake_n')
f_brake_s = 20000.0
f_brake = f_brake_s * f_brake_n
# lateral wheel load transfer [N]
gamma_y_n = ca.SX.sym('gamma_y_n')
gamma_y_s = 5000.0
gamma_y = gamma_y_s * gamma_y_n
# scaling factors for control variables
u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s])
# put all controls together
u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n)
# ------------------------------------------------------------------------------------------------------------------
# MODEL EQUATIONS --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# extract vehicle and tire parameters
veh = pars["vehicle_params_mintime"]
tire = pars["tire_params_mintime"]
# general constants
g = pars["veh_params"]["g"]
rho = pars["veh_params"]["rho_air"]
mass = pars["veh_params"]["mass"]
# curvature of reference line [rad/m]
kappa = ca.SX.sym('kappa')
# drag force [N]
f_xdrag = pars["veh_params"]["dragcoeff"] * v**2
# rolling resistance forces [N]
f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh[
"wheelbase"]
f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh[
"wheelbase"]
f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh[
"wheelbase_front"] / veh["wheelbase"]
f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh[
"wheelbase_front"] / veh["wheelbase"]
f_xroll = tire["c_roll"] * mass * g
# static normal tire forces [N]
f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
# dynamic normal tire forces (aerodynamic downforces) [N]
f_zlift_fl = 0.5 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_fr = 0.5 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_rl = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2
f_zlift_rr = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2
# dynamic normal tire forces (load transfers) [N]
f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) -
veh["k_roll"] * gamma_y)
f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) +
veh["k_roll"] * gamma_y)
f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) -
(1.0 - veh["k_roll"]) * gamma_y)
f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) +
(1.0 - veh["k_roll"]) * gamma_y)
# sum of all normal tire forces [N]
f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl
f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr
f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl
f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr
# slip angles [rad]
alpha_fl = delta - ca.atan(
(v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z))
alpha_fr = delta - ca.atan(
(v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z))
alpha_rl = ca.atan(
(-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z))
alpha_rr = ca.atan(
(-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z))
# lateral tire forces [N]
f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl *
(1 + tire["eps_front"] * f_z_fl / tire["f_z0"]) *
ca.sin(tire["C_front"] *
ca.atan(tire["B_front"] * alpha_fl - tire["E_front"] *
(tire["B_front"] * alpha_fl -
ca.atan(tire["B_front"] * alpha_fl)))))
f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr *
(1 + tire["eps_front"] * f_z_fr / tire["f_z0"]) *
ca.sin(tire["C_front"] *
ca.atan(tire["B_front"] * alpha_fr - tire["E_front"] *
(tire["B_front"] * alpha_fr -
ca.atan(tire["B_front"] * alpha_fr)))))
f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl *
(1 + tire["eps_rear"] * f_z_rl / tire["f_z0"]) *
ca.sin(tire["C_rear"] *
ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"] *
(tire["B_rear"] * alpha_rl -
ca.atan(tire["B_rear"] * alpha_rl)))))
f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr *
(1 + tire["eps_rear"] * f_z_rr / tire["f_z0"]) *
ca.sin(tire["C_rear"] *
ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"] *
(tire["B_rear"] * alpha_rr -
ca.atan(tire["B_rear"] * alpha_rr)))))
# longitudinal tire forces [N]
f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh[
"k_brake_front"] - f_xroll_fl
f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh[
"k_brake_front"] - f_xroll_fr
f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (
1 - veh["k_brake_front"]) - f_xroll_rl
f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (
1 - veh["k_brake_front"]) - f_xroll_rr
# longitudinal acceleration [m/s²]
ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) -
(f_y_fl + f_y_fr) * ca.sin(delta) -
pars["veh_params"]["dragcoeff"] * v**2) / mass
# lateral acceleration [m/s²]
ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr +
(f_y_fl + f_y_fr) * ca.cos(delta)) / mass
# time-distance scaling factor (dt/ds)
sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta)))
# model equations for two track model (ordinary differential equations)
dv = (sf / mass) * (
(f_x_rl + f_x_rr) * ca.cos(beta) +
(f_x_fl + f_x_fr) * ca.cos(delta - beta) +
(f_y_rl + f_y_rr) * ca.sin(beta) -
(f_y_fl + f_y_fr) * ca.sin(delta - beta) - f_xdrag * ca.cos(beta))
dbeta = sf * (
-omega_z +
(-(f_x_rl + f_x_rr) * ca.sin(beta) +
(f_x_fl + f_x_fr) * ca.sin(delta - beta) +
(f_y_rl + f_y_rr) * ca.cos(beta) +
(f_y_fl + f_y_fr) * ca.cos(delta - beta) + f_xdrag * ca.sin(beta)) /
(mass * v))
domega_z = (sf / veh["I_z"]) * (
(f_x_rr - f_x_rl) * veh["track_width_rear"] / 2 -
(f_y_rl + f_y_rr) * veh["wheelbase_rear"] +
((f_x_fr - f_x_fl) * ca.cos(delta) +
(f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2 +
((f_y_fl + f_y_fr) * ca.cos(delta) +
(f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"])
dn = sf * v * ca.sin(xi + beta)
dxi = sf * omega_z - kappa
# put all ODEs together
dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s
# ------------------------------------------------------------------------------------------------------------------
# CONTROL BOUNDARIES -----------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad]
delta_max = veh["delta_max"] / delta_s # max. steer angle [rad]
f_drive_min = 0.0 # min. longitudinal drive force [N]
f_drive_max = veh[
"f_drive_max"] / f_drive_s # max. longitudinal drive force [N]
f_brake_min = -veh[
"f_brake_max"] / f_brake_s # min. longitudinal brake force [N]
f_brake_max = 0.0 # max. longitudinal brake force [N]
gamma_y_min = -np.inf # min. lateral wheel load transfer [N]
gamma_y_max = np.inf # max. lateral wheel load transfer [N]
# ------------------------------------------------------------------------------------------------------------------
# STATE BOUNDARIES -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_min = 1.0 / v_s # min. velocity [m/s]
v_max = pars["optim_opts"]["v_max"] / v_s # max. velocity [m/s]
beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad]
beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad]
omega_z_min = -0.5 * np.pi / omega_z_s # min. yaw rate [rad/s]
omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s]
xi_min = -0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad]
xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad]
# ------------------------------------------------------------------------------------------------------------------
# INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_guess = 20.0 / v_s
# ------------------------------------------------------------------------------------------------------------------
# HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# continuous time dynamics
f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'],
['dx', 'sf'])
# longitudinal tire forces [N]
f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
# lateral tire forces [N]
f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
# vertical tire forces [N]
f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])
# longitudinal and lateral acceleration [m/s²]
f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])
# ------------------------------------------------------------------------------------------------------------------
# FORMULATE NONLINEAR PROGRAM --------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# initialize NLP vectors
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# initialize ouput vectors
x_opt = []
u_opt = []
dt_opt = []
tf_opt = []
ax_opt = []
ay_opt = []
ec_opt = []
# initialize control vectors (for regularization)
delta_p = []
F_p = []
# boundary constraint: lift initial conditions
Xk = ca.MX.sym('X0', nx)
w.append(Xk)
n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s
n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
x_opt.append(Xk * x_s)
# loop along the racetrack and formulate path constraints & system dynamic
for k in range(N):
# add decision variables for the control
Uk = ca.MX.sym('U_' + str(k), nu)
w.append(Uk)
lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min])
ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max])
w0.append([0.0] * nu)
# add decision variables for the state at collocation points
Xc = []
for j in range(d):
Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx)
Xc.append(Xkj)
w.append(Xkj)
lbw.append([-np.inf] * nx)
ubw.append([np.inf] * nx)
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# loop over all collocation points
Xk_end = D[0] * Xk
sf_opt = []
for j in range(1, d + 1):
# calculate the state derivative at the collocation point
xp = C[0, j] * Xk
for r in range(d):
xp = xp + C[r + 1, j] * Xc[r]
# interpolate kappa at the collocation point
kappa_col = kappa_interp(k + tau[j])
# append collocation equations (system dynamic)
fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col)
g.append(h * fj - xp)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# add contribution to the end state
Xk_end = Xk_end + D[j] * Xc[j - 1]
# add contribution to quadrature function
J = J + B[j] * qj * h
# add contribution to scaling factor (for calculating lap time)
sf_opt.append(B[j] * qj * h)
# calculate used energy
dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2])
ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1])
# add new decision variables for state at end of the collocation interval
Xk = ca.MX.sym('X_' + str(k + 1), nx)
w.append(Xk)
n_min = (-w_tr_right_interp(k + 1) +
pars["optim_opts"]["width_opt"] / 2.0) / n_s
n_max = (w_tr_left_interp(k + 1) -
pars["optim_opts"]["width_opt"] / 2.0) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# add equality constraint
g.append(Xk_end - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# get tire forces
f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk)
f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk)
f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk)
# get accelerations (longitudinal + lateral)
axk, ayk = f_a(Xk, Uk)
# path constraint: limitied engine power
g.append(Xk[0] * Uk[1])
lbg.append([-np.inf])
ubg.append([veh["power_max"] / (f_drive_s * v_s)])
# get constant friction coefficient
if pars["optim_opts"]["var_friction"] is None:
mue_fl = pars["optim_opts"]["mue"]
mue_fr = pars["optim_opts"]["mue"]
mue_rl = pars["optim_opts"]["mue"]
mue_rr = pars["optim_opts"]["mue"]
# calculate variable friction coefficients along the reference line (regression with linear equations)
elif pars["optim_opts"]["var_friction"] == "linear":
# friction coefficient for each tire
mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1]
mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1]
mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1]
mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1]
# calculate variable friction coefficients along the reference line (regression with gaussian basis functions)
elif pars["optim_opts"]["var_friction"] == "gauss":
# gaussian basis functions
sigma = 2.0 * center_dist[k + 1, 0]
n_gauss = pars["optim_opts"]["n_gauss"]
n_q = np.linspace(-n_gauss, n_gauss,
2 * n_gauss + 1) * center_dist[k + 1, 0]
gauss_basis = []
for i in range(2 * n_gauss + 1):
gauss_basis.append(
ca.exp(-(Xk[3] * n_s - n_q[i])**2 / (2 * (sigma**2))))
gauss_basis = ca.vertcat(*gauss_basis)
mue_fl = ca.dot(w_mue_fl[k + 1, :-1],
gauss_basis) + w_mue_fl[k + 1, -1]
mue_fr = ca.dot(w_mue_fr[k + 1, :-1],
gauss_basis) + w_mue_fr[k + 1, -1]
mue_rl = ca.dot(w_mue_rl[k + 1, :-1],
gauss_basis) + w_mue_rl[k + 1, -1]
mue_rr = ca.dot(w_mue_rr[k + 1, :-1],
gauss_basis) + w_mue_rr[k + 1, -1]
else:
raise ValueError("No friction coefficients are available!")
# path constraint: Kamm's Circle for each wheel
g.append(((f_x_flk / (mue_fl * f_z_flk))**2 + (f_y_flk /
(mue_fl * f_z_flk))**2))
g.append(((f_x_frk / (mue_fr * f_z_frk))**2 + (f_y_frk /
(mue_fr * f_z_frk))**2))
g.append(((f_x_rlk / (mue_rl * f_z_rlk))**2 + (f_y_rlk /
(mue_rl * f_z_rlk))**2))
g.append(((f_x_rrk / (mue_rr * f_z_rrk))**2 + (f_y_rrk /
(mue_rr * f_z_rrk))**2))
lbg.append([0.0] * 4)
ubg.append([1.0] * 4)
# path constraint: lateral wheel load transfer
g.append((
(f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk +
(f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s)) * veh["cog_z"] /
((veh["track_width_front"] + veh["track_width_rear"]) / 2) -
Uk[3] * gamma_y_s)
lbg.append([0.0])
ubg.append([0.0])
# path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal)
g.append(Uk[1] * Uk[2])
lbg.append([-20000.0 / (f_drive_s * f_brake_s)])
ubg.append([0.0])
# path constraint: actor dynamic
if k > 0:
sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s)
g.append((Uk - w[1 + (k - 1) * nx]) /
(pars["stepsize_opts"]["stepsize_reg"] * sigma))
lbg.append([
delta_min / (veh["t_delta"]), -np.inf,
f_brake_min / (veh["t_brake"]), -np.inf
])
ubg.append([
delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]),
np.inf, np.inf
])
# path constraint: safe trajectories with acceleration ellipse
if pars["optim_opts"]["safe_traj"]:
g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"])**2 +
(ayk / pars["optim_opts"]["ay_safe"])**2)
g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"])**2 +
(ayk / pars["optim_opts"]["ay_safe"])**2)
lbg.append([0.0] * 2)
ubg.append([1.0] * 2)
# append controls (for regularization)
delta_p.append(Uk[0] * delta_s)
F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0)
# append outputs
x_opt.append(Xk * x_s)
u_opt.append(Uk * u_s)
tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk])
tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk])
ax_opt.append(axk)
ay_opt.append(ayk)
# boundary constraint: start states = final states
g.append(w[0] - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# path constraint: limited energy consumption
if pars["optim_opts"]["limit_energy"]:
g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0)
lbg.append([0])
ubg.append([pars["optim_opts"]["energy_limit"]])
# formulate differentiation matrix (for regularization)
diff_matrix = np.eye(N)
for i in range(N - 1):
diff_matrix[i, i + 1] = -1.0
diff_matrix[N - 1, 0] = -1.0
# regularization (delta)
delta_p = ca.vertcat(*delta_p)
Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p)
Jp_delta = ca.dot(Jp_delta, Jp_delta)
# regularization (f_drive + f_brake)
F_p = ca.vertcat(*F_p)
Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p)
Jp_f = ca.dot(Jp_f, Jp_f)
# formulate objective
J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"][
"penalty_delta"] * Jp_delta
# concatenate NLP vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# concatenate output vectors
x_opt = ca.vertcat(*x_opt)
u_opt = ca.vertcat(*u_opt)
tf_opt = ca.vertcat(*tf_opt)
dt_opt = ca.vertcat(*dt_opt)
ax_opt = ca.vertcat(*ax_opt)
ay_opt = ca.vertcat(*ay_opt)
ec_opt = ca.vertcat(*ec_opt)
# ------------------------------------------------------------------------------------------------------------------
# CREATE NLP SOLVER ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# fill nlp structure
nlp = {'f': J, 'x': w, 'g': g}
# solver options
opts = {
"expand": True,
"verbose": print_debug,
"ipopt.max_iter": 2000,
"ipopt.tol": 1e-7
}
# solver options for warm start
if pars["optim_opts"]["warm_start"]:
opts_warm_start = {
"ipopt.warm_start_init_point": "yes",
"ipopt.warm_start_bound_push": 1e-9,
"ipopt.warm_start_mult_bound_push": 1e-9,
"ipopt.warm_start_slack_bound_push": 1e-9,
"ipopt.mu_init": 1e-9
}
opts.update(opts_warm_start)
# load warm start files
if pars["optim_opts"]["warm_start"]:
try:
w0 = np.loadtxt(os.path.join(export_path, 'w0.csv'))
lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv'))
lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv'))
except IOError:
print('\033[91m' + 'WARNING: Failed to load warm start files!' +
'\033[0m')
sys.exit(1)
# check warm start files
if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw):
print(
'\033[91m' +
'WARNING: Warm start files do not fit to the dimension of the NLP!'
+ '\033[0m')
sys.exit(1)
# create solver instance
solver = ca.nlpsol("solver", "ipopt", nlp, opts)
# ------------------------------------------------------------------------------------------------------------------
# SOLVE NLP --------------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# start time measure
t0 = time.perf_counter()
# solve NLP
if pars["optim_opts"]["warm_start"]:
sol = solver(x0=w0,
lbx=lbw,
ubx=ubw,
lbg=lbg,
ubg=ubg,
lam_x0=lam_x0,
lam_g0=lam_g0)
else:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
# end time measure
tend = time.perf_counter()
# ------------------------------------------------------------------------------------------------------------------
# EXTRACT SOLUTION -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# helper function to extract solution for state variables, control variables, tire forces, time
f_sol = ca.Function(
'f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt],
['w'],
['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt'])
# extract solution
x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x'])
# solution for state variables
x_opt = np.reshape(x_opt, (N + 1, nx))
# solution for control variables
u_opt = np.reshape(u_opt, (N, nu))
# solution for tire forces
tf_opt = np.append(tf_opt[-12:], tf_opt[:])
tf_opt = np.reshape(tf_opt, (N + 1, 12))
# solution for time
t_opt = np.hstack((0.0, np.cumsum(dt_opt)))
# solution for acceleration
ax_opt = np.append(ax_opt[-1], ax_opt)
ay_opt = np.append(ay_opt[-1], ay_opt)
atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2))
# solution for energy consumption
ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0
# ------------------------------------------------------------------------------------------------------------------
# EXPORT SOLUTION --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# export data to CSVs
opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(
file_path=export_path,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
tf=tf_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
w0=sol["x"],
lam_x0=sol["lam_x"],
lam_g0=sol["lam_g"])
# ------------------------------------------------------------------------------------------------------------------
# PLOT & PRINT RESULTS ---------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
if plot_debug:
opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(
pars=pars,
reftrack=reftrack,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
tf=tf_opt,
ec=ec_opt_cum)
if print_debug:
print("INFO: Laptime: %.3fs" % t_opt[-1])
print("INFO: NLP solving time: %.3fs" % (tend - t0))
print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt))
print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt))
print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt))
print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt))
print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1])
return -x_opt[:-1, 3], x_opt[:-1, 0]
def Min(x, y):
return ca.fmin(x, y)
if load_dual:
with open(dual_location, 'r') as infile:
dual_vals = json.load(infile)
if len(dual_vals) != opti.ng:
raise Exception(
"Couldn't load the dual, since your problem has %i cons and the cached problem has %i cons."
% (opti.ng, len(dual_vals)))
for i in tqdm(range(opti.ng), desc="Loading dual variables:"):
opti.set_initial(opti.lam_g[i], dual_vals[i])
sind = lambda theta: cas.sin(theta * cas.pi / 180)
cosd = lambda theta: cas.cos(theta * cas.pi / 180)
tand = lambda theta: cas.tan(theta * cas.pi / 180)
atan2d = lambda y_val, x_val: cas.atan2(y_val, x_val) * 180 / np.pi
clip = lambda x, min, max: cas.fmin(cas.fmax(min, x), max)
def smoothmax(value1, value2, hardness):
"""
A smooth maximum between two functions.
Useful because it's differentiable and convex!
Great writeup by John D Cook here:
https://www.johndcook.com/soft_maximum.pdf
:param value1: Value of function 1.
:param value2: Value of function 2.
:param hardness: Hardness parameter. Higher values make this closer to max(x1, x2).
:return: Soft maximum of the two supplied values.
"""
value1 = value1 * hardness
value2 = value2 * hardness
def __init__(self, _mocp):
assert isinstance(_mocp, MOCP)
self.mocp = _mocp
self.phases = dict()
for phase_name in self.mocp.phases:
self.phases[phase_name] = TranscribedPhase(
self.mocp.phases[phase_name])
# Create overall objective
total_objective_sym = casadi.SX(0.0)
total_objective_sym += sum(self.mocp.objectives.values())
for phase_name in self.phases:
total_objective_sym += sum(
self.phases[phase_name].mean_objective_integrals.values())
# Collect all constraints
equality_constraints = [] # every entry == 0
inequality_constraints = [] # every entry <= 0
# ODE constraints
for phase_name in self.phases:
for trajectory_name in self.phases[phase_name].trajectories:
ode_node_defects = self.phases[phase_name].trajectories[
trajectory_name].ode_node_defects
if not ode_node_defects is None:
equality_constraints.append(ode_node_defects)
# Path constraints
for phase_name in self.phases:
equality_constraints.extend(
c.g_nodes
for c in self.phases[phase_name].constraints.values()
if c.ocp_constraint.is_equation)
inequality_constraints.extend(
c.g_nodes
for c in self.phases[phase_name].constraints.values()
if not c.ocp_constraint.is_equation)
# Other constraints
equality_constraints.extend(c.g
for c in self.mocp.constraints.values()
if c.is_equation)
inequality_constraints.extend(c.g
for c in self.mocp.constraints.values()
if not c.is_equation)
equality_constraints = casadi.vertcat(*equality_constraints)
inequality_constraints = casadi.vertcat(*inequality_constraints)
# Sort box constraints
equality_constraints_box = []
equality_constraints_other = []
inequality_constraints_box = []
inequality_constraints_other = []
for i in range(inequality_constraints.numel()):
b = self.parse_box_constraint(inequality_constraints[i])
if b is None:
inequality_constraints_other.append(inequality_constraints[i])
else:
inequality_constraints_box.append(b)
for i in range(equality_constraints.numel()):
b = self.parse_box_constraint(equality_constraints[i])
if b is None:
equality_constraints_other.append(equality_constraints[i])
else:
equality_constraints_box.append(b)
equality_constraints_other = casadi.vertcat(
*equality_constraints_other)
inequality_constraints_other = casadi.vertcat(
*inequality_constraints_other)
# Combine box constraints into vectors matching the variables (x) vector
x_symbol, x_value = self.pack_variables()
x_names = [e.name() for e in casadi.vertsplit(x_symbol)]
assert len(list(
set(x_names))) == len(x_names), 'Duplicate variable name!'
x_name_index_map = {e[1]: e[0] for e in enumerate(x_names)}
lower_bound = [casadi.SX(-casadi.inf)] * x_symbol.numel()
upper_bound = [casadi.SX(casadi.inf)] * x_symbol.numel()
for i in range(len(inequality_constraints_box)):
variable_index = x_name_index_map[
inequality_constraints_box[i].variable.name()]
if inequality_constraints_box[i].is_upper_bound:
upper_bound[variable_index] = casadi.fmin(
upper_bound[variable_index],
inequality_constraints_box[i].bound)
else:
lower_bound[variable_index] = casadi.fmax(
lower_bound[variable_index],
inequality_constraints_box[i].bound)
for i in range(len(equality_constraints_box)):
variable_index = x_name_index_map[
equality_constraints_box[i].variable.name()]
upper_bound[variable_index] = casadi.fmin(
upper_bound[variable_index], equality_constraints_box[i].bound)
lower_bound[variable_index] = casadi.fmax(
lower_bound[variable_index], equality_constraints_box[i].bound)
lower_bound = casadi.vertcat(*lower_bound)
upper_bound = casadi.vertcat(*upper_bound)
# Create standard form f,h,g,lb,ub functions
p_symbol, p_value = self.pack_parameters()
self.nlp_x = x_symbol
self.nlp_p = p_symbol
self.nlp_f = total_objective_sym
self.nlp_h = equality_constraints_other
self.nlp_g = inequality_constraints_other
self.nlp_lb = lower_bound
self.nlp_ub = upper_bound
self.nlp_fn_fhg = casadi.Function('nlp_fn_fhg',
[self.nlp_x, self.nlp_p],
[self.nlp_f, self.nlp_h, self.nlp_g],
['x', 'p'], ['f', 'h', 'g'])
self.nlp_fn_lb_ub = casadi.Function('nlp_fn_lb_ub', [self.nlp_p],
[self.nlp_lb, self.nlp_ub], ['p'],
['lb', 'ub'])
assert not self.nlp_fn_fhg.has_free()
assert not self.nlp_fn_lb_ub.has_free()
def clip(x, min, max): # Clip a value to a range [min, max].
return cas.fmin(cas.fmax(min, x), max)
def ode_rhs(self):
"""Muscle Model ODE rhs.
Returns
----------
ode_rhs: list<cas.SX>
description
"""
#: Bandpass l_ce
#b, a = signal.butter(2, 50, 'low', analog=True)
#l_ce_filt = signal.lfilter(b, a, self._l_ce.sym)
l_ce_tol = cas.fmax(self._l_ce.sym, 0.0)
_stim = cas.fmax(0.01, cas.fmin(self._stim.sym, 1.))
#: Algrebaic Equation
l_mtc = self._l_slack.val + self._l_opt.val + self._delta_length.sym
l_se = l_mtc - l_ce_tol
#: Muscle Acitvation Dynamics
self._dA.sym = (
_stim - self._activation.sym)/GeyerMuscle.tau_act
#: Muscle Dynamics
#: Series Force
_f_se = (self._f_max.val * (
(l_se - self._l_slack.val) / (
self._l_slack.val * self.e_ref))**2) * (
l_se > self._l_slack.val)
#: Muscle Belly Force
_f_be_cond = self._l_opt.val * (1.0 - self.w)
_f_be = (
(self._f_max.val * (
(l_ce_tol - self._l_opt.val * (1.0 - self.w)) / (
self._l_opt.val * self.w / 2.0))**2)) * (
l_ce_tol <= _f_be_cond)
#: Force-Length Relationship
val = cas.fabs(
(l_ce_tol - self._l_opt.val) / (self._l_opt.val * self.w))
exposant = GeyerMuscle.c * val**3
_f_l = cas.exp(exposant)
#: Force Parallel Element
_f_pe_star = (self._f_max.val * (
(l_ce_tol - self._l_opt.val) / (self._l_opt.val * self.w))**2)*(
l_ce_tol > self._l_opt.val)
#: Force Velocity Inverse Relation
_f_v_eq = ((
self._f_max.val * self._activation.sym * _f_l) + _f_pe_star)
f_v_cond = cas.logic_and(
_f_v_eq < self.tol, _f_v_eq > -self.tol)
_f_v = cas.if_else(f_v_cond, 0.0, (_f_se + _f_be) / ((
self._f_max.val * self._activation.sym * _f_l) + _f_pe_star))
f_v = cas.fmax(0.0, cas.fmin(_f_v, 1.5))
self._v_ce.sym = cas.if_else(
f_v < 1.0, self._v_max.sym * self._l_opt.val * (
1.0 - f_v) / (1.0 + f_v * GeyerMuscle.K),
self._v_max.sym*self._l_opt.val * (f_v - 1.0) / (
7.56 * GeyerMuscle.K *
(f_v - GeyerMuscle.N) + 1.0 - GeyerMuscle.N
))
#: Active, Passive, Tendon Force Computation
_f_v_ce = cas.if_else(
self._v_ce.sym < 0.,
(self._v_max.sym*self._l_opt.val - self._v_ce.sym) /
(self._v_max.sym*self._l_opt.val + GeyerMuscle.K * self._v_ce.sym),
GeyerMuscle.N + (GeyerMuscle.N - 1) * (
self._v_max.sym*self._l_opt.val + self._v_ce.sym
) / (
7.56 * GeyerMuscle.K * self._v_ce.sym - self._v_max.sym*self._l_opt.val
))
self._a_force = self._activation.sym * _f_v_ce * _f_l * self._f_max.val
self._p_force = _f_pe_star*_f_v - _f_be
self._t_force = _f_se
self._alg_tendon_force.sym = self._z_tendon_force.sym - self._t_force
self._alg_active_force.sym = self._z_active_force.sym - self._a_force
self._alg_passive_force.sym = self._z_passive_force.sym - self._p_force
self._alg_v_ce.sym = self._z_v_ce.sym - self._v_ce.sym
self._alg_l_mtc.sym = self._z_l_mtc.sym - l_mtc
self._alg_dact.sym = self._z_dact.sym - self._dA.sym
return True
