Seed selection for smoothing adapter

[marip8]

I have been experimenting with the MoveIt planning adapter proposed in #7 over the past couple days and have noticed some behavior in the underlying STOMP MoveIt planner that might need to be changed to accommodate that PR. In the solve method, the STOMP MoveIt planner attempts to get the seed parameters here. If it is unsuccessful, it creates its own seed trajectory using the start/goal states and the specified interpolation method. With regard to using STOMP as a planning adapter, it seems like there should be a way to force STOMP to use an input seed trajectory. Otherwise, the planning adapter would be creating an entirely different plan, not smoothing the input plan.

In my testing, I have noticed this many times when providing a pose target as the goal for the planner/adapter. Occasionally STOMP's IK solution for this pose is not within the allowable tolerance with respect to the last pose in the seed trajectory as calculated by this function.

Any thoughts about the implications of a change like this to the STOMP MoveIt planner?

[marip8]

There appears to be a bug in the validation of the Cartesian goal constraint here. The IK solver is using the start of the seed trajectory (rather than the end of the trajectory) as a seed pose. In the case of high-DOF systems (like a robot on a rail), this can result in very big differences when compared to the end state of the seed trajectory. It seems like the check should actually be:

Eigen::VectorXd start, goal;
...
goal = parameters.rightCols(1); // initializing goal;
...
Eigen::VectorXd solution;
- Eigen::VectorXd seed = start;
ik_solver_->setKinematicState(state);
- if(ik_solver_->solve(seed,tool_constraints.get(),solution))
+ if(ik_solver_->solve(goal,tool_constraints.get(),solution))
{
  goal = solution;
  found_goal = true;
  break;
}

[jrgnicho]

@marip8 yes the last position in the seed trajectory should be the ik seed to solve for the goal.

[marip8]

Okay, I'll put together a pull request to fix this

[marip8]

@jrgnicho I think there is still an unresolved issue here in that STOMP will still create it's own seed trajectory if it fails to initialize the input seed trajectory. This would be an issue for those using STOMP as a post-processor where the intent is to improve an existing trajectory rather than creating a new one. The bug I fixed in #21 probably catches many cases of failure to get the seed trajectory, but there may be others.

I think we should add a parameterizable flag (probably just for the planning adapter plugin) to indicate that STOMP should "fail" rather than generate a new seed trajectory when it is unable to initialize the seed trajectory.