Goal tolerance vs GoalGuidedMultivariateGaussian

[simonschmeisser]

I noticed that Stomp often fails to find a valid solution for me if using GoalGuidedMultivariateGaussian. Looking at the intermediates via

noisy_filters:
     - [...]
      - class: stomp_moveit/MultiTrajectoryVisualization
        line_width: 0.02
        rgb: [0, 255, 0]
        marker_array_topic: stomp_trajectories
        marker_namespace: noisy

I observe that the goals (visualized as spheres in rviz) are quite far apart and do not seem to obey tolerances requested from MoveIt. Switching to NormalDIstributionSampling as noise generator works fine however.

@mamoll maybe this is the root of your issues as well?

[jrgnicho]

In theory the Goal Guided noise generator can create goals that are far apart if the constraints allow for that, however for a fully constrained goal the result should be the same as the NormalDIstributionSampling one