# D.12  Model Predictive Path Integral Controller

[Gif](https://navigation.ros.org/_images/mppi_demo.gif)

The MPPI Controller ( Model Predictive Path Integral ) ဆိုတာ Nav2 မှာပါတဲ့ predictive controller အမျိုးအစားအသစ်တစ်ခု။ ၄င်းအမျိုးအစားဟာ TEB & Pure path tracking MPC controllers အမျိုးအစားတွေကနေဆင်းသက်လာတာ ။

သူက sampling based approach နည်းလမ်းကို အသုံးပြုပြီးတော့ အသင့်တော်ဆုံး trajectories ကိုရွေးချယ်တယ်။ ပြီးတော့ ပိုကောင်း ပိုတိုတဲ့ လမ်းကြောင်းတစ်ခုဖြစ် အောင် successive iterations တွေကနေ velocity တွေကို  optimizingလုပ်ပေးတာဖြစ်ပါတယ်။

Plugin-based objective function တွေသုံပြီးတော့လည်း behaviors နှင့် behavioral attributes တွေကို customization and extension လုပ် ပေးပါတယ်။

လက်ရှိ Differential, Omidirectional and Ackermann robotsတွေမှာ MPPI controller ကို အသုံးပြုလို့ရပါတယ်။ hardware requirement အနေနှင့် MPPI controller ကို intel 4th gen i5 မာ 50+Hz နဲ့ စမ်းသပ်လို့အဆင် ပြေတယ်လို့ဆိုပါတယ်။

MPPI algorithm ဆိုတာ MPC ရဲ့ variant တစ်ခုပါ၊ robot ရဲ့ control velocity ကို iterative approach သုံးပြီးရှာပါတယ်။ previous time step's best control solution , the robot's current state , a set of randomly sampled perturbations from a Gaussian distribution စတာတွေအသုံးပြုထားတာပါ။

&#x20;process တစ်ခု ဟာထပ်ခါထပ်ခါ လည် ပတ်နေပီး ရလာတဲ့ output scores ဟာ နောက်လာမဲ့ process တစ်ုခုရဲ့ initial value တစ်ခု အနေနဲ့ ပြန်သုံးမယ်လို့ ဆိုပါတယ်။

<table><thead><tr><th width="192">MPPI Parameters</th><th width="361"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>motion_model</td><td>The desired motion model to use for trajectory planning. Options are DiffDrive, Omni, or Ackermann. Differential drive robots may use forward/reverse and angular velocities; Omni add in lateral motion; and Ackermann adds minimum curvature constraints.</td><td>string</td><td>“DiffDrive”</td></tr><tr><td>critics</td><td>A vector of critic plugin functions to use, without mppi::critic:: namespace which will be automatically added on loading.</td><td>string vector</td><td>N/A</td></tr><tr><td>iteration_count</td><td>Iteration count in the MPPI algorithm. Recommended to remain as 1 and instead prefer larger batch sizes.</td><td>int</td><td>1.0</td></tr><tr><td>batch_size</td><td>Count of randomly sampled candidate trajectories from current optimal control sequence in a given iteration. 1000 @ 50 Hz or 2000 @ 30 Hz seems to produce good results.</td><td>int</td><td>1000.0</td></tr><tr><td>Time steps</td><td>Number of time steps (points) in candidate trajectories</td><td>int</td><td>56.0</td></tr><tr><td>model_dt</td><td>Length of each time step’s dt timestep, in seconds. time_steps * model_dt is the prediction horizon.</td><td>double</td><td>0.1</td></tr><tr><td>vx_std</td><td>Sampling standard deviation for Vx</td><td>double</td><td>0.2</td></tr><tr><td>vy_std</td><td>Sampling standard deviation for Vy</td><td>double</td><td>0.2</td></tr><tr><td>wz_std</td><td>Sampling standard deviation for Wz (angular velocity)</td><td>double</td><td>0.2</td></tr><tr><td>vx_max</td><td>Target maximum forward velocity (m/s).</td><td>double</td><td>0.5</td></tr><tr><td>vy_max</td><td>Target maximum lateral velocity, if using Omni motion model (m/s).</td><td>double</td><td>0.5</td></tr><tr><td>vx_min</td><td>Maximum reverse velocity (m/s).</td><td>double</td><td>-0.35</td></tr><tr><td>wz_max</td><td>Maximum rotational velocity (rad/s).</td><td>double</td><td>1.9</td></tr><tr><td>temperature</td><td>Selectiveness of trajectories by their costs (The closer this value to 0, the “more” we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration.</td><td>double</td><td>0.3</td></tr><tr><td>gamma</td><td>A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won’t need to be changed from the default. See Section 3D-2 in “Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving” for detailed information.</td><td>double</td><td>0.0</td></tr><tr><td>visualize</td><td>Whether to publish debuggin trajectories for visualization. This can slow down the controller substantially (e.g. 1000 batches of 56 size every 30hz is alot of data).</td><td>bool</td><td>FALSE</td></tr><tr><td>retry_attempt_limit</td><td>Number of attempts to find feasible trajectory on failure for soft-resets before reporting total failure.</td><td>int</td><td>1.0</td></tr><tr><td>reset_period</td><td>Required time of inactivity to reset optimizer (only in Humble due to backport ABI policies).</td><td>double</td><td>1.0</td></tr></tbody></table>

<table><thead><tr><th width="234">Trajectory Visualization</th><th width="382"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>trajectory_step</td><td>The step between trajectories to visualize to downsample candidate trajectory pool.</td><td>int</td><td>5.0</td></tr><tr><td>time_step</td><td>The step between points on trajectories to visualize to downsample trajectory density.</td><td>int</td><td>3.0</td></tr></tbody></table>

<table><thead><tr><th width="269">Path Handler</th><th width="344"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>transform_tolerance</td><td>Time tolerance for data transformations with TF (s).</td><td>double</td><td>0.1</td></tr><tr><td>prune_distance</td><td>Distance ahead of nearest point on path to robot to prune path to (m).</td><td>double</td><td>1.5</td></tr><tr><td>max_robot_pose_search_dist</td><td>Max integrated distance ahead of robot pose to search for nearest path point in case of path looping</td><td>double</td><td>Costmap size / 2</td></tr><tr><td>enforce_path_inversion</td><td>If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner’s requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.</td><td>bool</td><td>FALSE</td></tr><tr><td>inversion_xy_tolerance</td><td>Cartesian proximity (m) to path inversion point to be considered “achieved” to pass on the rest of the path after path inversion.</td><td>double</td><td>0.2</td></tr><tr><td>inversion_yaw_tolerance</td><td>Angular proximity (radians) to path inversion point to be considered “achieved” to pass on the rest of the path after path inversion. 0.4 rad = 23 deg.</td><td>double</td><td>0.4</td></tr></tbody></table>

<table><thead><tr><th width="190">Ackermann Motion Model</th><th width="306"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>min_turning_r</td><td>The minimum turning radius possible for the vehicle platform (m).</td><td>double</td><td>0.2</td></tr></tbody></table>

<table><thead><tr><th width="195">Constraint Critic</th><th width="308"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>4.0</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1.0</td></tr></tbody></table>

<table><thead><tr><th width="197">Goal Angle Critic</th><th width="331"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>3.0</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr><tr><td>threshold_to_consider</td><td>Minimal distance (m) between robot and goal above which angle goal cost considered.</td><td>double</td><td>0.5</td></tr></tbody></table>

<table><thead><tr><th width="221">Goal Critic</th><th width="319"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>5.0</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr><tr><td>threshold_to_consider</td><td>Minimal distance (m) between robot and goal above which goal distance cost considered. It is wise to start with this as being the same as your prediction horizon to have a clean hand-off with the path follower critic.</td><td>double</td><td>1.4</td></tr></tbody></table>

<table><thead><tr><th width="186">Obstacles Critic</th><th width="381"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>critical_weight</td><td>Weight to apply to critic for near collisions closer than collision_margin_distance to prevent near collisions only as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions.</td><td>double</td><td>20.0</td></tr><tr><td>repulsion_weight</td><td>Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the inflation_radius distance from obstacles.</td><td>double</td><td>1.5</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr><tr><td>consider_footprint</td><td>Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.</td><td>bool</td><td>FALSE</td></tr><tr><td>collision_margin_distance</td><td>Margin distance (m) from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. Note that it will highly influence the controller not to enter spaces more confined than this, so ensure this parameter is set lower than the narrowest you expect the robot to need to traverse through.</td><td>double</td><td>0.10</td></tr><tr><td>near_goal_distance</td><td>Distance (m) near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.</td><td>double</td><td>0.50</td></tr><tr><td>cost_scaling_factor</td><td>Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only)</td><td>double</td><td>10.0</td></tr><tr><td>inflation_radius</td><td>Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only)</td><td>double</td><td>0.55</td></tr></tbody></table>

<table><thead><tr><th width="228">Path Align Critic</th><th width="368"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>10.0</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr><tr><td>threshold_to_consider</td><td>Distance (m) between robot and goal to stop considering path alignment and allow goal critics to take over.</td><td>double</td><td>0.5</td></tr><tr><td>offset_from_furthest</td><td>Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.</td><td>int</td><td>20.0</td></tr><tr><td>max_path_occupancy_ratio</td><td>Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path’s intent in presence of dynamic objects in the scene. Between 0-1 for 0-100%.</td><td>double</td><td>0.07</td></tr><tr><td>use_path_orientations</td><td>Whether to consider path’s orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.</td><td>bool</td><td>FALSE</td></tr></tbody></table>

<table><thead><tr><th width="246">Path Angle Critic</th><th width="280"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>2.0</td></tr><tr><td>cost_power</td><td></td><td>int</td><td>1</td></tr><tr><td>threshold_to_consider</td><td>Distance (m) between robot and goal to stop considering path angles and allow goal critics to take over.</td><td>double</td><td>0.5</td></tr><tr><td>offset_from_furthest</td><td></td><td>int</td><td>20</td></tr><tr><td>max_angle_to_furthest</td><td>Angular distance (rad) between robot and goal above which path angle cost starts being considered</td><td>double</td><td>1.2</td></tr><tr><td>mode</td><td>Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot’s orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot’s orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path’s requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction.</td><td>int</td><td>0</td></tr></tbody></table>

<table><thead><tr><th width="232">Path Follow Critic</th><th width="327"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>5.0</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td></td><td></td></tr><tr><td>threshold_to_consider</td><td>Distance (m) between robot and goal to stop considering path following and allow goal critics to<br>take over. It is wise to start with this as being the same as your prediction<br>horizon to have a clean hand-off with the goal critic.</td><td></td><td></td></tr><tr><td>offset_from_furthest</td><td>Number of path points after furthest one any trajectory achieves to drive path tracking relative to.</td><td></td><td></td></tr></tbody></table>

<table><thead><tr><th width="233">Prefer Forward Critic</th><th width="310"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>5</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr><tr><td>threshold_to_consider</td><td>Distance (m) between robot and goal to stop considering path following and allow goal<br>critics to take over. It is wise to start with this as being the same as<br>your prediction horizon to have a clean hand-off with the goal critic.</td><td>double</td><td>0.5</td></tr></tbody></table>

<table><thead><tr><th width="172">Twirling Critic</th><th width="330"></th><th></th><th></th></tr></thead><tbody><tr><td>Name</td><td>Description</td><td>Type</td><td>Default</td></tr><tr><td>cost_weight</td><td>Weight to apply to critic term.</td><td>double</td><td>10</td></tr><tr><td>cost_power</td><td>Power order to apply to term.</td><td>int</td><td>1</td></tr></tbody></table>

နမူနာ

```yaml
controller_server:
  ros__parameters:
    controller_frequency: 30.0
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: -0.35
      vy_max: 0.5
      wz_max: 1.9
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: false
      reset_period: 1.0 # (only in Humble)
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstrains:
        min_turning_r: 0.2
      critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      ObstaclesCritic:
        enabled: true
        cost_power: 1
        repulsion_weight: 1.5
        critical_weight: 20.0
        consider_footprint: false
        collision_cost: 10000.0
        collision_margin_distance: 0.1
        near_goal_distance: 0.5
        inflation_radius: 0.55 # (only in Humble)
        cost_scaling_factor: 10.0 # (only in Humble)
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 3
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0
```

#### General Words of Wisdom

sdfsf
