OMPL behaving suspiciously for some motions

[peik-etzell]

OMPL gives me colliding trajectories for From_start, but not other freespace queries:

trim.mp4

The trajectory is always quite weird too, somehow jumpy at the last 2 waypoints. The planning times are also very suspicious, From_start takes <<1s to plan (2x RRTConnect for 50 sols), while transitions and To_end often time out (30s) or at least take magnitudes longer.

Do you have some idea what is going on? I can try to create a public repo to recreate this if it is needed.

I'm using a sligtly modified RasterFtGlobalTask without TrajOpt for freespace to demo this

I've tried all CollisionEvaluatorTypes for OMPL, but all of them give the same result

EDIT:
To add, I've also tried RRTstar and LazyPRM, they all give almost the exact same behavior, and they also give that weird second-to-last waypoint slightly to the left!

[peik-etzell]

Okay so it's the descartes global seeding that causes the issues I think? Is it completely broken currently or is it possible to utilize it through some changes?

[Levi-Armstrong]

I took a look at the DOT graph posted and it appears that something is not correct with you yaml config. You can see in the global task the output key does not seemed to be used anywhere. Can you post your yaml config?

[peik-etzell]

Here's the used config, pretty much the default RasterFtGlobalTask/Pipeline, just changed out the CartesianPipeline to TrajOptPipeline (so you don't generate new seeds for the rasters), and Freespace -> OMPL for debugging this.

task_composer_plugins.yaml

task_composer_plugins:
  search_paths:
    - install/tesseract_task_composer/lib
  search_libraries:
    - tesseract_task_composer_factories
    - tesseract_task_composer_planning_factories
    - tesseract_task_composer_taskflow_factories
  executors:
    default: TaskflowExecutor
    plugins:
      TaskflowExecutor:
        class: TaskflowTaskComposerExecutorFactory
        config:
          threads: 5
  tasks:
    plugins:
      .
      .
      .
      RasterGlobalTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [ input_data ]
          outputs: [ output_data ]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [ input_data ]
                outputs: [ output_data ]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [ output_data ]
                outputs: [ output_data ]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [ output_data ]
                outputs: [ output_data ]
                freespace:
                  # task: FreespacePipeline
                  task: OMPLPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [ output_data ]
                    output_indexing: [ output_data ]
                raster:
                  # task: CartesianPipeline
                  task: TrajOptPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [ output_data ]
                    output_indexing: [ output_data ]
                transition:
                  # task: FreespacePipeline
                  task: OMPLPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [ output_data ]
                    output_indexing: [ output_data ]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ ErrorTask, DescartesMotionPlannerTask ]
            - source: DescartesMotionPlannerTask
              destinations: [ ErrorTask, RasterMotionTask ]
            - source: RasterMotionTask
              destinations: [ ErrorTask, DoneTask ]
          terminals: [ ErrorTask, DoneTask ]
      RasterGlobalPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [ input_data ]
          outputs: [ output_data ]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterGlobalTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [ AbortTask, DoneTask ]
          terminals: [ AbortTask, DoneTask ]

[Levi-Armstrong]

Everything looks fine, so I will need to dig deeper to see what the issue is. I am in the middle of refactoring things to leverage cmake components so not sure I will have time this week. If you have time, I recommend attaching a debugger and investigate the task_infos returned.

[peik-etzell]

Thank you, I'll try to investigate this a little myself 👍