velocity_optimization.src package

Submodules

velocity_optimization.src.IniSQPStatemachine module

class velocity_optimization.src.IniSQPStatemachine.IniSQP[source]

Bases: object

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.01.2020

Documentation: Class to check which initialization for the velocity SQP-solver is appropriate online.

get_v0(plan: str, action_id: str, m: int, b_print_sm: bool = False) → numpy.array[source]

Python version: 3.5 Created by: Thomas Herrmann Created on: 01.02.2020

Documentation: Get best initialization for velocity profile to be optimized based on past velocity profiles and time information.

Inputs: plan: follow ‘f’ or straight/left/right ‘slr’ action_id: ‘follow’, ‘straight’, ‘left’ or ‘right’ m: number of velocity points b_print_sm: print initialization statemachine decision logic

Outputs: v0: velocity initial guess [m/s]

set_vx(plan: str, action_id: str, vx: numpy.ndarray)[source]

Python version: 3.5 Created by: Thomas Herrmann Created on: 01.02.2020

Documentation: Stores the currently optimized velocity profile.

Inputs: plan: follow ‘f’ or straight/left/right ‘slr’ action_id: ‘follow’, ‘straight’, ‘left’ or ‘right’ vx: currently optimized velocity profile

velocity_optimization.src.SymQP module

class velocity_optimization.src.SymQP.SymQP(m: int, sid: str, params_path: str)[source]

Bases: object

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.11.2019

Documentation: Class to construct velocity SQP-optimizer symbolically.

Inputs: m: number of velocity points sid: ID of SQP object to be created ‘PerfSQP’ or ‘EmergSQP’ params_path: absolute path to folder containing config file .ini

D_mat_

E_mat_

F_cst

F_cst_jac

F_ini

F_ini_cst

F_ini_cst_jac

J_hess

J_jac

P_cst

P_cst_jac

P_max

Tre_cst1

Tre_cst1_jac

Tre_cst2

Tre_cst2_jac

Tre_cst3

Tre_cst3_jac

Tre_cst4

Tre_cst4_jac

ax_max

ay_max

dF_cst

dF_cst_jac

delta_s

fF_cst

fF_cst_jac

fF_ini_cst

fF_ini_cst_jac

fJ_Hess

fJ_jac

fP_cst

fP_cst_jac

fTre_cst1

fTre_cst1_jac

fTre_cst2

fTre_cst2_jac

fTre_cst3

fTre_cst3_jac

fTre_cst4

fTre_cst4_jac

fdF_cst

fdF_cst_jac

fv_cst

fv_cst_end

fv_cst_end_jac

fv_cst_jac

init_symbolics()[source]

Python version: 3.5 Created by: Thomas Herrmann Created on: 01.11.2019

Documentation: Initializes necessary symbolic expressions to construct velocity optimizer QP.

kappa

m

n

params_path

s_t

sid

sqp_stgs

subs_symbolics()[source]

Python version: 3.5 Created by: Thomas Herrmann Created on: 01.11.2019

Documentation: Substitutes hard coded parameters in symbolic expressions.

subs_syms(arg) → sympy.matrices.immutable.ImmutableDenseMatrix[source]

Python version: 3.5 Created by: Thomas Herrmann Created on: 01.11.2019

Documentation: Substitutes symbols by their respective numeric value.

Inputs: arg: symbolic expression

Outputs: arg: symbolic expression with replaced hard-coded parameters

sym_sc

sym_sc_

v

v_cst

v_cst_end

v_cst_end_jac

v_cst_jac

v_end

v_ini

v_max

velocity_optimization.src.VarPower module

class velocity_optimization.src.VarPower.VarPowerLimits(input_path: str)[source]

Bases: object

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.02.2020

Documentation: Class to store and interpolate variable power limits to be used during driving to feed into the variable P_max constraint.

Inputs: input_path: absolute path to folder containing variable vehicle and track information

f_pwr_intp

init_interface_recalc()[source]

input_path

rec_recalc

receive_esim_update()[source]: Function to receive ESIM update. Call this function manually instead of a while loop in order not to block the velocity planner function.

snd_recalc

trigger_recalc(s_meas: float, meas_diff: numpy.ndarray, phase: str = 'r', track: str = 'mnt', laps: int = 12, x0: numpy.ndarray = array([1.0, 0.0, 0.0, 0.5, 35.0, 35.0, 35.0, 35.0, 35.0]))[source]

Triggers energy strategy recalculation with measurement values.

Parameters

s_meas – global s-coordinate where vehicle currently is
meas_diff – measurement values expressed as difference to energy strtaegy values at current position

Only necessary for the initialization phase are the following parameters: :param phase: which ES phase to call: ‘r’, recalculation (default); ‘i’, init.; ‘v’, v-ref calculation :param track: which track: ‘mnt’, Monteblanco (default), … :param laps: how many laps: any integer, 12 (default) :param x0: starting values

Authors: Thomas Herrmann <thomas.herrmann@tum.de>
Created on: 01.02.2020

velocity_optimization.src.VelQP module

class velocity_optimization.src.VelQP.VelQP(m: int, sid: str, params_path: str, input_path: str, logging_path: Optional[str] = None, ci: bool = False)[source]

Bases: object

Class to construct QP-optimizer by a manually designed vector matrix notation.

Parameters

m – number of velocity points
sid – optimized ID ‘PerfSQP’ or ‘EmergSQP’
params_path – absolute path to folder containing config file .ini
input_path – absolute path to folder containing variable vehicle and track information
ci – switch to construct an object of this class used within the CI/CD jobs

Authors

Thomas Herrmann <thomas.herrmann@tum.de>

Created on

01.11.2019

Am

Am_csc

EP_mat_red

E_mat_red

E_mat_redd

F_cst

F_cst_jac

F_ini_cst

F_ini_cst_jac

J_Hess

J_jac

P_cst

P_cst_jac

T_mat_inv

Tre_cst1

Tre_cst1_jac

Tre_cst2

Tre_cst2_jac

Tre_cst3

Tre_cst3_jac

Tre_cst4

Tre_cst4_jac

b_ini_done

dF_cst

dF_cst_jac

err

err_inf

ev_vel_w

get_osqp_mat(x0_v: numpy.ndarray, x0_s_t: numpy.ndarray, v_ini: float, v_max: numpy.ndarray, v_end: float, F_ini: float, kappa: numpy.ndarray, delta_s: numpy.ndarray, P_max: Optional[numpy.ndarray] = None, ax_max: Optional[numpy.ndarray] = None, ay_max: Optional[numpy.ndarray] = None, v_max_cstr: Optional[numpy.ndarray] = None) → tuple[source]

Constructs necessary QP matrices from the linearized versions of the physically necessary equations for the velocity optimization of a vehicle.

Parameters

x0_v – initial guess velocity [m/s]
x0_s_t – initial guess slack variables tire [-]
v_ini – initial hard constrained velocity [m/s]; currently unused here as we remove the first velocity point from the optimization problem and and set it manually afterwards
v_max – max. allowed velocity (in objective function) [m/s]
v_end – hard constrained max. allowed value of end velocity in optimization horizon [m/s]
F_ini – hard constrained initial force [kN]
kappa – curvature profile of given path [rad/m]
delta_s – discretization step length of given path [m]
P_max – max. allowed power [kW]
ax_max – max. allowed longitudinal acceleration [m/s^2]
ay_max – max. allowed lateral accelereation [m/s^2]
vmax_cstr – max. allowed spatially dependend velocity (hard constraint) [m/s]; can be used for e.g. adaptive cruise control (following)

Returns

P: Sparse version of Hessian matrix of nonlinear objective function J

q: Jacobian of nonlinear objective function J

Am_csc: Sparse version of jacobian of nonlinear constraints

lo: lower boundaries of linearized constraints

up: upper boundaries of linearized constraints

Authors

Thomas Herrmann <thomas.herrmann@tum.de>

Created on

01.11.2019

lo

logger_emerg

logger_perf

m

m_inv

n

ones_vec_red

osqp_init()[source]

Initializes the QP solver OSQP and does the solver settings.

OSQP solves the problem

\[ \begin{align}\begin{aligned}1/2~x^T P x + q^T x \\\mathrm{s.t.} \quad l \leq Ax \leq u # noqa: W605\end{aligned}\end{align} \]

Authors: Thomas Herrmann <thomas.herrmann@tum.de>
Created on: 01.11.2019

osqp_solve() → tuple[source]

Solves the constructed QP and returns the solution as well as OSQP status information.

Returns

sol.x: optimization variable values

sol.info.iter: iteration number of OSQP solver

sol.info.status_val: status of OSQP solver

Authors

Thomas Herrmann <thomas.herrmann@tum.de>

Created on

01.11.2019

osqp_update_online(x0_v: numpy.ndarray, x0_s_t: numpy.ndarray, v_ini: float, v_max: numpy.ndarray, v_end: float, F_ini: float, kappa: numpy.ndarray, delta_s: numpy.ndarray, P_max: Optional[numpy.ndarray] = None, ax_max: Optional[numpy.ndarray] = None, ay_max: Optional[numpy.ndarray] = None, vmax_cstr: Optional[numpy.ndarray] = None)[source]

Updates the matrices in the constructed QP.

Parameters

x0_v – initial guess velocity [m/s]
x0_s_t – initial guess slack variables tire [-]
v_ini – initial hard constrained velocity [m/s]
v_max – max. allowed velocity (in objective function) [m/s]
v_end – hard constrained max. allowed value of end velocity in optimization horizon [m/s]
F_ini – hard constrained initial force [kN]
kappa – curvature profile of given path [rad/m]
delta_s – discretization step length of given path [m]
P_max – max. allowed power [kW]
ax_max – max. allowed longitudinal acceleration [m/s^2]
ay_max – max. allowed lateral accelereation [m/s]
vmax_cstr – max. allowed spatially dependend velocity (hard constraint) [m/s^2]; can be used for e.g. adaptive cruise control (following)

Authors

Thomas Herrmann <thomas.herrmann@tum.de>

Created on

01.11.2019

params_path

sid

slack_every_v

sol_osqp

sparsity_pat

sqp_stgs

sym_sc_

up

v_cst_end

v_cst_end_jac

vpl

x0

x0_s_t

velocity_optimization.src.get_sparsity module

velocity_optimization.src.get_sparsity.calc_sparsity(params_path: str, logging_path: str, m_perf: int = 115, m_emerg: int = 50)[source]

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.03.2020

Documentation: Derives the sparsity pattern of the constraint matrix A that the OSQP solver needs. The pattern is derived from the symbolic expressions of the linearized constraints. The pattern is used online to directly fill the sparse CSC A-matrix for faster and more stable performance.

Inputs: m_perf: number of velocity points in performance profile m_emerg: number of velocity points in emergency profile

velocity_optimization.src.get_sparsity.log_sparsity(symbolics: sympy.matrices.immutable.ImmutableDenseMatrix, name: str, logger: logging.Logger)[source]

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.03.2020

Documentation: Writes sparsity pattern to file.

Inputs: symbolics: symbolic matrices of the constraint expressions name: name of constraint logger: logger to be used to write pattern to external file

velocity_optimization.src.online_qp module

velocity_optimization.src.online_qp.online_qp(velqp: velocity_optimization.src.VelQP.VelQP, v_ini: float, kappa: numpy.ndarray, delta_s: numpy.ndarray, P_max: Optional[numpy.array] = None, ax_max: Optional[numpy.array] = None, ay_max: Optional[numpy.array] = None, x0_v: Optional[numpy.ndarray] = None, v_max: Optional[numpy.ndarray] = None, v_end: Optional[float] = None, F_ini: Optional[float] = None, s_glob: Optional[float] = None, v_max_cstr: Optional[numpy.ndarray] = None) → tuple[source]

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.11.2019

Documentation: Creates an SQP that optimizes a velocity profile for a given path.

Inputs: velqp: QP solver object used within the SQP v_ini: initial velocity hard constraint [m/s] kappa: kappa profile of given path [rad/m] delta_s: discretization step length [m] P_max: max. allowed power [kW] ax_max: max. allowed longitudinal acceleration [m/s^2] ay_max: max. allowed ongitudial acceleration [m/s^2] x0_v: initial guess of optimal velocity [m/s] v_max: max. should velocity (objective function) [m/s] v_end: constrained end velocity in optimization horizon [m/s] F_ini: initial force constraint [kN] s_glob: global s coordinate of current vehicle position [m] v_max_cstr: max. must velocity (hard constraint) [m/s]

Outputs: v_op: optimized velocity using OSQP as QP solver [m/s] s_t_op: optimized slack values [-] qp_status: status of last QP within SQP [-]

velocity_optimization.src.params_vp_sqp module

velocity_optimization.src.params_vp_sqp.params_vp_sqp(m: int, sid: str, params_path: str) → tuple[source]

Python version: 3.5 Created by: Thomas Herrmann (thomas.herrmann@tum.de) Created on: 01.12.2019

Documentation: Creates some random initialization values for the velocity planner SQP.

Inputs: m: number of velocity points sid: ID of optimizer object ‘PerfSQP’ or ‘EmergSQP’

Outputs: sqp_stgs: SQP settings v_ini: initial velocity constraint [m/s] v_max: max. velocity (objective function) [m/s] v_end: end velocity in optimization horizon [m/s] x0_v: initial velocity guess [m/s] x0_s_t: initial slack guess [-] F_ini: initial force constraint [kN] kappa: curvature profile for path [rad/m] delta_s: discretization step length [m] P_max: max. allowed power [kW] ax_max: max. longitudinal acceleration [m/s^2] ay_max: max. lateral acceleration [m/s^2] err: SQP iteration RMSE [-] err_inf: SQP infinity error [-]

velocity_optimization.src package

Submodules

velocity_optimization.src.IniSQPStatemachine module

velocity_optimization.src.SymQP module

velocity_optimization.src.VarPower module

velocity_optimization.src.VelQP module

velocity_optimization.src.get_sparsity module

velocity_optimization.src.online_qp module

velocity_optimization.src.params_vp_sqp module

Module contents