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