Doubts on the SMAC lattice planner rotation penalty

[shannonwarren]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble
• Version or commit hash:
• DDS implementation:
  • Cyclone

Steps to reproduce issue

I have a query regarding the rotation penalty and it's expected behavior. I have an omni directional robot using smac lattice planner with a turning radius of 0.1m navigating aisles in a grocery store. Given a high rotation penalty and a goal point which may be positioned either behind the robot or beyond the next aisle (the shortest path might involve multiple rotations), our expectation is that given a high rotation penalty, the planner chooses a longer path around the aisle instead of rotating in place. I could probably achieve this by increasing the turning radius but this causes other complications to our system and would to like to avoid making changes to the turning radius. Any suggestions on this? I have experimented with rotation penalties of 1 up to 10000. Attaching my planner_server configs

planner_server:
  ros__parameters:
    use_sim_time: true
    expected_planner_frequency: 10.0
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerLattice"
      tolerance: 0.10
      allow_unknown: false                 # Allow traveling in unknown space
      max_iterations: 1000000             # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_planning_time: 5.0              # Max time in s for planner to plan, smooth
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 0.50  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      reverse_penalty: 1.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.05                # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 2.05          # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.5               # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      rotation_penalty: 100.0              # Penalty to apply to in-place rotations, if minimum control set contains them
      retrospective_penalty: 0.015
      lattice_filepath: "lattice_planner/output.json"                # The filepath to the state lattice graph
      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      allow_reverse_expansion: false   # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
      smooth_path: true  
      use_final_approach_orientation: true
      cost_travel_multiplier: 2.0
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

Example of the planner with rotation penalty of 1000 Screencast from 04-15-2024 12:43:28 PM.webm

Expected behavior

Actual behavior

Additional information

---

Feature request

Feature description

Implementation considerations

[shannonwarren]

Screencast from 04-15-2024 12:43:28 PM.webm

[SteveMacenski]

I'm having a hard time figuring out what is happening since the path near the robot is small. What's going on with that path locally? I'm seeing some kind of figure-8 maneuver which is kind of interesting. Are you sure that its actually the rotate at place which is causing you issue or is there something up with your lattice potentially?

It looks like the robot is trying to rotate due to RPP's carrot being behind it rather than Smac Lattice - no? It definitely could be both, but its hard to tell from those videos. That path looks weird but I can't tell if it actually is weird or just something else in Rviz is weird

Also

allow_reverse_expansion: false

You probably want that to true for an omni vehicle. That might solve a number of your concerns if the robot can just properly back out a bit rather than doing that possibly hacky figure-8 just trying to change heading in order to get to the goal

[SteveMacenski]

Hey, just following up on this - any update or issue continues?

[SteveMacenski]

Closing since user has not followed up and an explanation was provided