search

ros-industrial / stomp_ros

ROS packages for the STOMP planner (split out of industrial_moveit)
Apache License 2.0
37 stars 28 forks source link

Check seed for validity #24

Open simonschmeisser opened 4 years ago

simonschmeisser commented 4 years ago

StompSmoothingAdapter:

Currently validity (ie collision free) is only checked after the first iteration. This means that even though a valid seed is given, STOMP might invalidate this during it's first iteration and never recover. This has led to discussions about an additional option allowing the adapter to fail and return the seed as a fall-back solution.

I propose to

  1. calculate the cost of the seed and thereby see if it's valid
  2. discuss about whether num_iteration_after_valid: 0 should then be ignored and still at least one iteration performed
  3. add an option to fail if the cost of the trajectory did not improve, returning the seed iff valid otherwise, fail if it isn't

this is a continuation of the discussion in #7 and #20

jrgnicho commented 4 years ago

@simonschmeisser I'm on board with making stop evaluate the cost of the seed trajectory prior to the first iteration, it should be a matter of tweaking the main loop of stomp here