MPC

MPC Visualizer¶

The mpc controller visualizer allows to display the planned controller trajectory and the selected reference trajectory on the center line. If requested, the trajectory is rendered as arrows which also highlightes the (planned) yaw angles. Finally, the planned trajectory is color coded (blue-red-green) in order to visualize the planned velocities.

Showing reference trajectory¶

Without reference trajectory¶

Safety Filter¶

The MPC Safety Filter allows to use a possible unsafe controller within the CRS framework. It works by applying an input that is as close as possible to the provided reference input from the unsafe controller while still ensuring that the car will not leave the track.

In order to run the safety filter, make sure to create a file in your experiment folder and pass the argument use_safety_filter:=true to the .launch file.

If you chose to use the mpc visualizer with the safety filter, the planned trajectory of the safety filter will be shown whenever the filtered input differs from the input provided by the original controller.

Full Yaml Config Structure

safety_filter.yaml

state_type: pacejka_car
input_type: pacejka_car
safety_filter_type: mpc_filter

config:
  solver_type: acados # solver that is used
  reference_method: uniform # method that is used to generate reference trajectory (uniform or const_input)
  dist_targ_multiplier: 3 # Multiplier that maps an applied torque to the expected driven distance
  min_terminal_dist: 1 # Min distance the car must drive
  max_terminal_dist: 5 # Max distance the car can drive
  use_torque_filter: true # If true, make sure driving distance does only decrease by max dist_decrement_max in each iteration
  dist_decrement_max: 0.01 # Max distence decrementin each iteration
  loookahead_time: 0.1 # By how much the state should be advanced 

  threshold: 0.01 # Threshold. If the input norm was modified more than this threhsold, a visual hint is shown with the planned trajectory

  # Cost
  cost_torque: 0.08 
  cost_steer: 0.08
  cost_delta_torque: 0
  cost_delta_steer: 0

visualizer:
  type: mpc
  rate: 15

  # Visualizer specific  parameters
  frame_id: crs_frame # default
  namespace: mpc # default

  use_arrows: false # If true, use arrows to visualize planned and reference yaw angle. This option is a lot slower and may introduce visual lags

  double_min_velocity: 1.5 # Min velocity for visualization (this or below gets mapped to blue)
  double_max_velocity: 3 # Max velocity for visualization (this or above gets mapped to red)

  planned:
    size_x: 0.04 # 0.07 for arrows
    size_y: 0.04 # 0.02 for arrows
    size_z: 0.04 # 0.02 for arrows

  reference:
    r: 1
    g: 0
    b: 0
    a: 1
    size_x: 0.04 # 0.07 for arrows
    size_y: 0.04 # 0.02 for arrows
    size_z: 0.04 # 0.02 for arrows

ROS Information

Node Names	Description
safety_filter_node	Main entry point of the safety filter

Launch File Arguments

Argument	Default	Description
car_state_topic	safety_filter_node/state	Topic where state of the car is published.
safe_car_input_topic	safety_filter_node/safe_car_input_topic	Topic where the safe control input is published
car_input_topic	safety_filter_node/car_input_topic	Topic where the (unsafe) controller input is published
safety_filter_config	$(find ros_safety_framework)/config/collision_avoidance.yaml	Path to safety filter config.yaml

Subscribed Topics	Description
safety_filter_node/state	Topic where state of the car is published
safety_filter_node/car_input_topic	Topic where unsafe input to the car is published

Published Topics	Description
safety_filter_node/safe_car_input_topic	Topic where the safe input is published

Parameters	Description
track/*	All track informations as defined in the Track Manager
model/*	Shared model parameters for all components
safety_filter_node/visualizer/*	MPC Visualizer specific configuration
safety_filter_node/state_type	State type
safety_filter_node/input_type	Input type
safety_filter_node/safety_filter_type	Safety filter type (e.g. mpc_filter)
safety_filter_node/config/solver_type	solver that is used
safety_filter_node/config/reference_method	method that is used to generate reference trajectory (uniform or const_input)
safety_filter_node/config/dist_targ_multiplier	Multiplier that maps an applied torque to the expected driven distance
safety_filter_node/config/min_terminal_dist	Min distance the car must drive
safety_filter_node/config/max_terminal_dist	Max distance the car can drive
safety_filter_node/config/use_torque_filter	If true, make sure driving distance does only decrease by max dist_decrement_max in each iteration
safety_filter_node/config/dist_decrement_max	Max distence decrementin each iteration
safety_filter_node/config/loookahead_time	By how much the state should be advanced
safety_filter_node/config/threshold	Threshold. If the input norm was modified more than this threhsold, a visual hint is shown with the planned trajectory
safety_filter_node/config/cost_torque	Cost for torque missmatch
safety_filter_node/config/cost_steer	Cost for steer missmatch
safety_filter_node/config/cost_delta_torque	Cost for change in torque over the trajectory
safety_filter_node/config/cost_delta_steer	Cost for change in steer over the trajectory

Example Configuration¶

Example Configuration

# Visualization parameters for mpc visualizer
visualizer:
  type: mpc
  rate: 10

  # Visualizer specific  parameters
  frame_id: crs_frame # default
  namespace: mpc # default

  use_arrows: false # If true, use arrows to visualize planned and reference yaw angle. This option is a lot slower and may introduce visual lags

  double_min_velocity: 1.5 # Min velocity for visualization (this or below gets mapped to blue)
  double_max_velocity: 2.5 # Max velocity for visualization (this or above gets mapped to red)

  planned:
    size_x: 0.04 # 0.07 for arrows
    size_y: 0.04 # 0.02 for arrows
    size_z: 0.04 # 0.02 for arrows

  reference:
    r: 1
    g: 0
    b: 0
    a: 1
    size_x: 0.04 # 0.07 for arrows
    size_y: 0.04 # 0.02 for arrows
    size_z: 0.04 # 0.02 for arrows

ROS Information¶

Launch File

Node Names	Description
controller_node	Main entry point of the visualizer

Launch File Arguments

Argument	Default	Description
car_state_topic	controller_node/state	Topic where state of the car is published.
car_input_topic	controller_node/control_input	Topic where the controller input is published
controller_config_file	$(find ros_controllers)/config/ff_fb_controller.yaml	Path to controller config.yaml
visualizer_topic	controller_node/controller_visualization	Topic, where controller visualizations are published (e.g. planned and reference trajectory)

Subscribers and Publishers

Subscribed Topics	Description
controller_node/state	Topic where state of the car is published

Published Topics	Description
controller_node/control_input	Topic where the controller published the input command

Parameter Descriptions

Parameters	Description
track/*	All track informations as defined in the Track Manager
model/*	Shared model parameters for all components
controller_node/max_rate	Max rate in Hz of this controller
controller_node/visualizer/*	Visualizer specific parameters, check the Controller Visualizer Class
controller_node/input_type	Type of the input (e.g. pacejka_car)
controller_node/state_type	Type of the state (e.g. pacejka_car)
controller_node/controller_type	Must be MPCC to use mpcc controller
controller_node/controller_params/Q1	Contouring Cost
controller_node/controller_params/Q2	Lag Cost
controller_node/controller_params/R1	dTorque Cost
controller_node/controller_params/R2	dSteer cost
controller_node/controller_params/R3	darclength cost
controller_node/controller_params/q	arclength cost
controller_node/controller_params/lag_compensation_time	lag compensation time
controller_node/controller_params/solver_type	Which solver to use (e.g. ACADOS)
controller_node/model/*	Model specific parameters to overwrite certain parameters of the model