Number of states in OMPL planned trajectory not being reduced about moveit HOT 10 CLOSED

As suggested, it is happening in the interpolate. I added some more print statements and resulted in this:

ros.ompl: base[RRTkConfigDefault]: Starting planning with 1 states already in datastructure
ros.ompl: Beginning sampling thread computation
ros.ompl: base[RRTkConfigDefault]: Created 35 states
ros.ompl: Solution found in 0.011115 seconds
ros.ompl: Stopped goal sampling thread after 3 sampling attempts
ros.rosconsole_bridge.urdfdom: There were 34 valid motions and 6 invalid motions.
ros.ompl: SimpleSetup: Path simplification took 0.000003 seconds and changed from 2 to 2 states
ros.rosconsole_bridge.urdfdom: Solution with 2 states before interpolate
ros.rosconsole_bridge.urdfdom: length: 0.066027, max_solution_segment_length_: 0.068000, minimum_waypoint_count_: 1.000000
ros.rosconsole_bridge.urdfdom: Returning successful solution with 10 states after interpolate
ros.moveit_ros_planning: Running 'Add Time Parameterization'
ros.moveit_ros_planning: Motion planner reported a solution path with 10 states
ros.moveit_ros_planning: Planned path was found to be valid when rechecked
ros.moveit_ros_move_group.actionlib: Setting the current goal as succeeded
ros.moveit_ros_move_group.actionlib: Setting status to succeeded on goal, id: /rviz_matt_Laptop_15944_7985327355976614980-5-1470350168.149427723, stamp: 1470350168.15
ros.moveit_ros_move_group.actionlib: Publishing result for goal with id: /rviz_matt_Laptop_15944_7985327355976614980-5-1470350168.149427723 and stamp: 1470350168.15
ros.moveit_ros_move_group.actionlib: Publishing feedback for goal, id: /rviz_matt_Laptop_15944_7985327355976614980-5-1470350168.149427723, stamp: 1470350168.15
ros.moveit_ros_move_group.actionlib: Publishing feedback for goal with id: /rviz_matt_Laptop_15944_7985327355976614980-5-1470350168.149427723 and stamp: 1470350168.15

The pertinent function is

void ompl_interface::ModelBasedPlanningContext::interpolateSolution()
{
if (ompl_simple_setup_->haveSolutionPath())
{
og::PathGeometric &pg = ompl_simple_setup_->getSolutionPath();
logDebug("%s: length: %f, max_solution_segment_length_: %f, minimum_waypoint_count_: %f", getName().c_str(),
pg.length(), max_solution_segment_length_, minimum_waypoint_count_);
pg.interpolate(std::max((unsigned int)floor(0.5 + pg.length() / max_solution_segment_length_), minimum_waypoint_count_));
}
}

How did we decide that the trajectory should have
(std::max((unsigned int)floor(0.5 + pg.length() / max_solution_segment_length_), minimum_waypoint_count_)
states?

from moveit.

I'd check what is the value of minimum_waypoint_count_ and where does it get set within moveit?

from moveit.

I printed all the values, not sure where they get set yet. But according to these numbers it should result in interpolate(1).

ros.rosconsole_bridge.urdfdom: length: 0.066027, max_solution_segment_length_: 0.068000, minimum_waypoint_count_: 1.000000

from moveit.

looks like 0.5 is to make it round up? still this evaluates to 1 AFAIK so it shouldn't change the number of states..

from moveit.

True enough, almost has to be something inside the actual pg.interpolate() function in ompl, or how we are using it. Because I printed the number of states on both sides of that function like this:

    logDebug("%s: Solution with %lu states before interpolate", getName().c_str(),
             getOMPLSimpleSetup()->getSolutionPath().getStateCount());

    interpolateSolution();

    // fill the response
    logDebug("%s: Returning successful solution with %lu states after interpolate", getName().c_str(),
             getOMPLSimpleSetup()->getSolutionPath().getStateCount());

and it resulted in the following:

ros.rosconsole_bridge.urdfdom: Solution with 2 states before interpolate
ros.rosconsole_bridge.urdfdom: length: 0.066027, max_solution_segment_length_: 0.068000, minimum_waypoint_count_: 1.000000
ros.rosconsole_bridge.urdfdom: Returning successful solution with 10 states after interpolate

from moveit.

Even stranger it looks like pg.interpolate(1) should be returning immediately.

void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
{
    if (requestCount < states_.size() || states_.size() < 2)
        return;
...

from moveit.

@anderwm did you eventually get somewhere looking for the root of this problem?

from moveit.

Not exactly, I was confused by what appeared to be conflicting information:

1. A call to getOMPLSimpleSetup()->getSolutionPath().getStateCount() before and after interpolateSolution() definately returns a different number of states but...
2. pg.interpolate(1) should return immediately without changing the state variable at all

It feels like an abused pointer or a default value problem but I wasn't smart enough to follow the code flow and find it.

from moveit.

I have found that this property (minimum_waypoint_count) is a ros parameter and its default value is 10. If you look on your ROS param server for the parameter /move_group/ompl/minimum_waypoint_count, you will see that it is set to 10. I haven`t found a way to change this value during runtime. Changing the value on ROS param server is possible, however I do not want to do this manually everytime I start ROS server. This should be in one of the ompl config files but I do not know where...

from moveit.

@fatihirim the minimum_waypoint_count variable is declared in https://github.com/ros-planning/moveit/blob/aac8c0de00d5f01c2c3e908b8f4028c84756a920/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg#L8. If you want to change this value permanently, look into changing that file.

I don't think your question is related to this issue though: if you have questions that aren't bugs in MoveIt, they're best asked on Ros Answers. Please use that site if you have more questions in the future.

from moveit.