STOMP fails to find valid solution after seeding with a valid trajectory

[tykurtz]

Description

When seeding STOMP with a valid trajectory (from RRT), STOMP fails to find a solution by reaching the max number of iterations.

Background

My use-case is path planning in a highly constrained collision environment relative to other users. Below is the configuration in use. Note the absence of the polynomial filter.

stomp/iiwa_1:
  group_name: iiwa_1
  optimization:
    num_timesteps: 20
    num_iterations: 50
    num_iterations_after_valid: 15
    num_rollouts: 10
    max_rollouts: 100 
    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
    control_cost_weight: 0.0
  task:
    noise_generator:
      - class: stomp_moveit/NormalDistributionSampling
        stddev: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
    cost_functions:
      - class: stomp_moveit/CollisionCheck
        check_intermediate_collisions: false
        kernel_window_percentage: 0.07
        collision_penalty: 1.0
        cost_weight: 1.0
        longest_valid_joint_move: 0.005
    noisy_filters:
      - class: stomp_moveit/JointLimits
        lock_start: True
        lock_goal: True 

Expected behavior

Based on the above settings, I would expect STOMP to always find a valid trajectory and reach at most 15 iterations.

Cause

https://github.com/ros-industrial/industrial_moveit/blob/kinetic-devel/stomp_moveit/src/stomp_planner.cpp#L429

The polynomial smoothing for seeded inputs ends up throwing the trajectory into a collision, and even though I'm not using the polynomial update filter, it can't always find its way back to a collision-free trajectory after a significant number of iterations. This behavior was why I removed the polynomial update filter in the first place.

When I do remove the polynomial filter on the seeded trajectory, I do get the expected behavior I want, but now I have a trade-off of STOMP running a very low number of iterations given the same amount of time

Obviously the polynomial smoothing has a huge impact on the running efficiency of the algorithm, but I don't have a good understanding of why. (I didn't see any reference to polynomial filtering in the STOMP white paper)

Proposed solution

I would suggest only applying the polynomial filter on the seeded trajectory if STOMP is configured to use the polynomial update filter. I can contribute a pull request to get this done. However, this doesn't reconcile the issue with poor run-time efficiency, so I will need some guidance on how to deal with that (if anything can be done at all)

EDIT : Disregard my comments on the run-time of using the polynomial filter on the seed trajectory. The poor run-time was due to setting the longest_valid_joint_move from 0.05 to 0.005, which obviously turned up the intermediate collision checking quite a bit. Classic user error of tweaking two things at once. STOMP is completely working as expected after commenting out the seed trajectory bit and resetting that value.

[jrgnicho]

Excellent, thanks for reporting this.
The rationale for smoothing the seed trajectory is that we wanted to start the optimization with a smooth trajectory, otherwise the end result would not be a smooth trajectory. This is because our old filter algorithm only smoothed out the updates to the trajectory instead of the trajectory itself. Eventually the polynomial filter was added, which smooths the trajectory, and so now smoothing the seed is no longer necessary. It'd be great if you could submit a PR for this.

[Levi-Armstrong]

Have you tried using this update filter instead of the polynomial filter. I have had good success when smoothing on JERK, but not sure if this is exposed through the yaml to be configured.

update_filters:
- class: stomp_moveit/ControlCostProjectionMatrix

[tykurtz]

Hi Levi, thanks for the feedback. To keep the conversation in one place, I'll respond to your comment on the PR here as well. I absolutely defer to your and Jorge's expertise here, so if I make an incorrect assumption feel free to correct me.

I believe a smoothing step is required for the algorithm.

That makes sense to me so long as there's a guarantee that the smoothing step does positive work. For example, when running a single iteration in stomp.cpp, one of the last steps is to check to see if positive work has been done,

   if(current_lowest_cost_ > parameters_total_cost_)
  {
    current_lowest_cost_ = parameters_total_cost_;
  }
  else
  {
    // reverting updates as no improvement was made
    parameters_optimized_ -= parameters_updates_;
  }

So even if the update_filter knocks the trajectory into collision, at the very least the algorithm catches the fact that it did negative work and reverts it. There is no such check for the case of the pre-processing step in stomp_planner.cpp, which is why the current setup is 'bouncing' a large portion of my seeded trajectories into collision, making otherwise valid trajectories invalid.

I would vote to change it from using the polynomial smoother to the other smoother since it has recently been fixed.

Doesn't this have the same potential to 'bounce' valid trajectories into collision, therefore doing negative work? I would also make the argument that doing any kind of pre-processing work might fall outside the scope of the STOMP planner. Removing the smoother from the STOMP planner for seeded plans doesn't mean that the user loses that functionality; rather it means that applying the smoothing filter to the seeded trajectory is now their responsibility prior to passing it to the STOMP planner.

There's also a second issue of the fact that polynomial order was hardcoded. The ControlCostProjectMatrix has a similar problem with determining the derivative order. How should this parameter be determined and be easily applicable for all users? Scratch that, I see now that this is already configurable under optimization: smoothing_derivative_order and can be set independently of using the update_filter

If you pass it a non smooth trajectory you will get a non smooth trajectory out since the smoothing is only applied to the update.

I wouldn't expect that to be the case. Given that control_cost_weight is set to be something non-zero, wouldn't smoothing of the base trajectory tend to happen naturally as a result of the gradient descent update?

Have you tried using this update filter instead of the polynomial filter. I have had good success when smoothing on JERK, but not sure if this is exposed through the yaml to be configured.
update_filters:

• class: stomp_moveit/ControlCostProjectionMatrix

I will experiment with this a bit more as both an update_filter and pre-processor step and let you know how it goes. I see that this setup follows the STOMP paper closer than the polynomial filter did.

[Levi-Armstrong]

That makes sense to me so long as there's a guarantee that the smoothing step does positive work. For example, when running a single iteration in stomp.cpp, one of the last steps is to check to see if positive work has been done,

I think in this case there are two option because the algorithm does require that the initial trajectory is smooth to insure that the final trajectory is smooth. The first it could just fail if after the smoothing of the seed trajectories cost is lower than the original input. The second is allow it to smooth even if the cost is worse and let the optimization find a trajectory that is valid. I vote for the latter.

Doesn't this have the same potential to 'bounce' valid trajectories into collision, therefore doing negative work? I would also make the argument that doing any kind of pre-processing work might fall outside the scope of the STOMP planner. Removing the smoother from the STOMP planner for seeded plans doesn't mean that the user loses that functionality; rather it means that applying the smoothing filter to the seeded trajectory is now their responsibility prior to passing it to the STOMP planner.

Yes it does have the possibility to put the trajectory into collision but the purpose of the seed is to just provide an better starting point for the optimization other than the currently available (Joint Interpolated, Lowest Cost). If the planner is configured for the problem even if it does push it into collision is should be able to find a solution.

I wouldn't expect that to be the case. Given that control_cost_weight is set to be something non-zero, wouldn't smoothing of the base trajectory tend to happen naturally as a result of the gradient descent update?

So STOMP is a Stochastic optimization planner which is not gradient decent. It generates a set of noisy update to the starting trajectory applies every update to the start trajectory for a given iteration. Then calculate the cost for every state and use the cost to calculate a probability. It then uses this probability to perform a convex sum of the noisy trajectories at each state to create the update to the trajectory. Before the update is applied it is ran through the smoothing step and then applied. After it is applied it calculates the cost of the trajectory. Now the cost is two parts, a cost for smoothness and then the secondary costs which is only collision right now.

I will experiment with this a bit more as both an update_filter and pre-processor step and let you know how it goes. I see that this setup follows the STOMP paper closer than the polynomial filter did.

I would also recommend changing the parameters below and you may have better results if you increase stddev values. The current values you have suggest to me that you have a more or less open environment with few obstacle because you are saving 90% of the noisy roll outs generated and using them during the convex sum operation. In most case I have found you only want to keep a maximum of 10% of the previous trajectories.

Current:

num_rollouts: 10
max_rollouts: 100 

Recommend

num_rollouts: 90
max_rollouts: 100

[tykurtz]

Thanks for the valuable feedback Levi, I'll make sure that my parameters are correctly tweaked for my workspace and let you know how that impacts the planner success rate. In order to close this issue, what I'd like to do is build something to benchmark the different scenarios and report back. That way selecting the pre-processing (none, polynomial, control cost) step can be a bit more of an informed decision, rather than assuming the ControlCost pre-processing step will be better than the polynomial filter.

I'm thinking the test setup would look something like this,

Test Cases

• No pre-processing step, no update filter
• Polynomial pre-processing step, no update filter
• ControlCost pre-processing step, no update filter
• No pre-processing step, ControlCost update filter
• Polynomial pre-processing step, polynomial update filter
• ControlCost pre-processing step, ControlCost update filter

Outputs

• Success rate of entire set
• Smoothness (sum of all joint accelerations?)
• Convergence rate by number of iterations

Also, off-topic but related to your comment about the STOMP parameters, are there any existing toolsets to aid in deriving appropriate planning parameters given a set of likely planning scenarios? Setting up a test bench for the above problem would be a step towards building a tool like that, so I'm keeping that goal in mind. My naive solution would be wrapping the set of planning problems around a non-linear solver like Ceres, set the STOMP noise generator values (or rollouts, or timesteps, or all) as input parameters, and set success rate, smoothness, and convergence rate as cost functions. Then let that run in the background for a day or two. Seems like it would be easier than trying to hand-tune it, and help reduce the barrier to entry for other users in regards to setting appropriate STOMP parameters.

[Levi-Armstrong]

Looks good to me. What are your thought @jrgnicho?