OMPL Almost Always Simplifies

[marrts]

tesseract_planning/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

Lines 214 to 233 in ad73309

	if (p->simplify)
	{
	p->simple_setup->simplifySolution();
	}
	else
	{
	// Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested
	auto num_output_states = static_cast<unsigned>(p->n_output_states);
	if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
	{
	p->simple_setup->getSolutionPath().interpolate(num_output_states);
	}
	else
	{
	// Now try to simplify the trajectory to get it under the requested number of output states
	// The interpolate function only executes if the current number of states is less than the requested
	p->simple_setup->simplifySolution();
	if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
	p->simple_setup->getSolutionPath().interpolate(num_output_states);
	}

What is the point of the else block in this section? I think this is a leftover remnant from older versions of tesseract where with the old taskflow approach we needed to maintain a certain number of expected input/output waypoints otherwise it could cause issues with later planners, but that is no longer the case.

Also, as this stands, the user doesn't actually really have control over this number of waypoints parameter which is set when calling createSubProblem here:

tesseract_planning/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

Lines 470 to 490 in ad73309

	// Transform plan instructions into ompl problem
	int index = 0;
	int num_output_states = 1;
	MoveInstructionPoly start_instruction = move_instructions.front().get().as<MoveInstructionPoly>();
	for (std::size_t i = 1; i < move_instructions.size(); ++i)
	{
	++num_output_states;
	const auto& instruction = move_instructions[i].get();
	assert(instruction.isMoveInstruction());
	const auto& move_instruction = instruction.as<MoveInstructionPoly>();
	const auto& waypoint = move_instruction.getWaypoint();
	if (waypoint.isCartesianWaypoint() || waypoint.isStateWaypoint() ||
	(waypoint.isJointWaypoint() && waypoint.as<JointWaypointPoly>().isConstrained()))
	{
	problems.push_back(createSubProblem(
	request, composite_mi, manip, start_instruction, move_instruction, num_output_states, index++));
	start_instruction = move_instruction;
	num_output_states = 1;
	}
	}

I think this essentially leads to the num_output_states being set to 2 in the vast majority of cases. This means simplify is always called, making the simplify parameter in the profile essentially useless. I think given the new setup we should just drop the else block in the if/else case above and only call simplify if the user specifies it.

If someone wants to upsample to a set number of waypoints that should be handled with another task after OMPL is called.

[Levi-Armstrong]

I believe this was left to handle the case where you want to feed ompl back into trajopt and want to preserve the number of state between. Example pipelines: simple_planner->ompl->trajopt and if we just simplify then you would need to run the simple planner again in some cases or just change the structure ompl->simple_planner->trajopt. I am not opposed to this change as it does simplify the planner but put the user must understand what they are doing when configuring pipeline which is currently the case anyway. @marip8 What are your thoughts?

[marip8]

I think simplification should be allowed to occur whether or not you decide to interpolate the trajectory

@Levi-Armstrong and I had a discussion about this yesterday, and we had two trains of thought about how to handle this for the use case where the OMPL planner is called after the interpolation has occurred (e.g., on failure of trajopt, etc):

Implementation within the OMPL planner:

Process:

• Count the number of intermediate (i.e., unconstrained) joint waypoints that were generated by interpolation in between original (i.e., fully constrained) joint waypoints
• Run OMPL between each pair of original (i.e., fully constrained) joint waypoints, ignoring the intermediate waypoints
• Simplify the OMPL path (if desired)
• If the number of states in the new trajectory is less than that in the original input, re-interpolate the path using the OMPL API and overwrite the values of the intermediate waypoints.

Pros:

• More integrated, fewer blocks needed in a task composer diagram
• Less "API" breaking for users

Cons

• Less intuitive to the user that OMPL is going to skip over intermediate waypoints

Implementation outside the OMPL planner

Process:

• Create a task composer task that removes intermediate (i.e., unconstrained) joint waypoints
• Update the task composer graph to run the task that removes intermediate waypoints before running OMPL
• Run OMPL where simplification can be run (if desired) but the path is not interpolated at a higher discretization than what is returned by the OMPL planner by default
• Run the simple planner after OMPL before providing to trajopt

Pros:

• OMPL functions explicitly as planning paths between all pairs of input waypoints (e.g., no skipping of intermediate waypoints)
• Task composer graph is much more explicit about how inputs must be prepared for OMPL

Cons:

• More complicated task composer graph
• Possibly more computation required to remove intermediate waypoints and resample them outside OMPL

For the time being, we were leaning towards the first option (implementation within OMPL planner) since it's closer to the current workflow and probably less of a burden on end-users specifying the task composer graph. I think the way this gets handled will be cleaned up quite a bit in #540, so maybe we see how that shakes out first to see if more work needs to be done

[marrts]

I think simplification should be allowed to occur whether or not you decide to interpolate the trajectory

I agree with this. I just want to be able to turn off simplification. Currently to turn off simplification I have to set it to false in the profile and then add an interpolation step before OMPL to add a bunch of intermediate waypoints and hope I added more than the OMPL solution will generate, and then I am stuck with that number of waypoints after OMPL does its own interpolation.

For the time being, we were leaning towards the first option (implementation within OMPL planner) since it's closer to the current workflow and probably less of a burden on end-users specifying the task composer graph

I can get behind interpolation staying in the process for users that run interpolation before OMPL (or if that gets added to the OMPL profile). I just don't want my hand forced on simplification because I don't do that.

[marrts]

• Create a task composer task that removes intermediate (i.e., unconstrained) joint waypoints
• Update the task composer graph to run the task that removes intermediate waypoints before running OMPL

Not arguing against your conclusions, but it is unclear to me why you would need these new steps. Your freespace pipeline can immediately attempt OMPL given your input Composite Instruction that will likely be two state waypoints and then you can run Simple Planner afterwards. I feel the comparison between the two processes as one where Simple Planner is run before OMPL and one where it is run after OMPL.

OMPL planner is called after the interpolation has occurred (e.g., on failure of trajopt, etc):

I read your scenario after writing that above paragraph, but I think my point still stands. In the case you described I think you should be using unique names the output of each task. That way you can directly pass your input CI to OMPL rather than reusing a CI that has already been modified or been attempted to be modified by other tasks. That more comes down to a best practice for how to make your taskflows though. I converted Scan N Plan to be structured like this in this yet to be merged PR and it's a massive help in debugging. Additionally, it allows better control of how things are processed. If a task expects a certain kind of input (like you describe above) you don't have to add those two extra steps of removing unconstrained waypoints. It's allowed me to do a lot of creative things that wouldn't be possible if I kept all the program output names the same.

[Levi-Armstrong]

I think a this should be left up to those integrating this into their system. I think the planners should preserve the structure of the program provided because it can include other instructions. The planners should only ever introduce additional interpolation states but never reduce. If the previous interpolation states are not needed then either a task should be added to prune them before OMPL or store the program in the data storage prior to interpolation to feed OMPL as you mentioned. All of this is dependent on what you are trying to get out of your pipeline. I think the current state of the motion planners should remain as they are and respect the structure provided.

[marrts]

That all makes sense to me, but I think as it stands it currently does not do what you want.

Currently it is:

    if (p->simplify)
    {
      p->simple_setup->simplifySolution();
    }
    else
    {
      // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested
      auto num_output_states = static_cast<unsigned>(p->n_output_states);
      if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
      {
        p->simple_setup->getSolutionPath().interpolate(num_output_states);
      }
      else
      {
        // Now try to simplify the trajectory to get it under the requested number of output states
        // The interpolate function only executes if the current number of states is less than the requested
        p->simple_setup->simplifySolution();
        if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
          p->simple_setup->getSolutionPath().interpolate(num_output_states);
      }
    }

In the scenario that someone specifies to simplify, then this task has the potential to reduce the number of instructions down below the number of input instructions since interpolate is never called. Additionally, this is forcing users to try and get down to the input number of instructions if they do not request it to simplify, but adding instructions should be allowed, since sometimes it's potentially required. Based off this discussion I think it should Instead be:

    // Simplify if the user requests it
    if (p->simplify)
    {
      p->simple_setup->simplifySolution();
    }

    // Interpolate if either simplification or the solution found has too few instructions
    if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
    {
      p->simple_setup->getSolutionPath().interpolate(num_output_states);
    }

In this scenario it ensures you match at least the input number of states (fixing the scenario where the input number of states is too low after a simplify, and only calls simplify is the user actually wants it.

[Levi-Armstrong]

Yea I agree with your proposed change