Motion planner interface additions to support custom planning task flows

[marip8]

The issue

I just created a few custom taskflow generators using the GraphTaskflowGenerator class, and I found it somewhat difficult to add motion planners as task generators. This is due to the fact that they need to know about the available planning profiles from the TaskInput structure, which is not available when I'm constructing my custom GraphTaskflowGenerator class. There currently exists a MotionPlannerTaskGenerator that would meet my needs, but it requires that a planner that is pre-configured with profiles by the TaskInput be passed to it (see here)

Example problem

tesseract_planning::TaskflowGenerator::UPtr createCustomTaskflow()
{
  using namespace tesseract_planning;

  // Create the graph task flow
  auto graph = std::make_unique<GraphTaskflow>();

  // TrajOpt task generator
  auto trajopt_planner = std::make_unique<TrajOptMotionPlanner>();

  // How do I set the profiles of the planner? They should be coming from the `TaskInput` which I don't have access to here
  // trajopt_planner->plan_profiles = ...

  int trajopt = graph->addNode(std::make_unique<MotionPlannerTaskGenerator>(trajopt_planner), true);
  
  ...

  return graph;
}

The resulting graph task flow might look like this:

Short-term solution

I can solve this by making a task generator class for each type of motion planner that:

1. Creates the motion planner object
2. Assigns the planner a default problem generator (not needed by all planners)
3. Configures the motion planner with the available profiles
4. Creates a MotionPlannerTaskGenerator given the planning object
5. Calls the MotionPlannerTaskGenerator::process and MotionPlannerTaskGenerator::conditionalProcess functions

This solution seems like a lot of repeat code for a task that seems conceptually generic across all current motion planners

Example code

class TrajOptTaskGenerator : public TaskGenerator
{
public:
  TrajOptTaskGenerator(const std::string name = "TrajOpt Task") : TaskGenerator(name)
  {
  }

protected:
  virtual void process(TaskInput input, std::size_t unique_id) const override
  {
    conditionalProcess(input, unique_id);
  }

  virtual int conditionalProcess(TaskInput input, std::size_t unique_id) const override
  {
    auto planner = std::make_shared<TrajOptMotionPlanner>();
    planner->problem_generator = &DefaultTrajoptProblemGenerator;
    if (input.profiles)
    {
      if (input.profiles->hasProfileEntry<TrajOptPlanProfile>())
        planner->plan_profiles = input.profiles->getProfileEntry<TrajOptPlanProfile>();

      if (input.profiles->hasProfileEntry<TrajOptCompositeProfile>())
        planner->composite_profiles = input.profiles->getProfileEntry<TrajOptCompositeProfile>();

      if (input.profiles->hasProfileEntry<TrajOptSolverProfile>())
        planner->solver_profiles = input.profiles->getProfileEntry<TrajOptSolverProfile>();
    }

    MotionPlannerTaskGenerator planning_task_gen(planner);
    return planning_task_gen.conditionalProcess(input, unique_id);
  }
};

Long-term solution?

It seems like the MotionPlanner base class should have a function for applying profiles. If this were the case, then we could simply modify the MotionPlannerTaskGenerator to apply known profiles to a planner that gets passed in.

As it stands, there are a few problems that prevent me from solving this issue in a generic way:

• I don't think there is a common base class for planner profiles
• The current profiles are templates which might make it difficult to integrate with pure virtual base classes.
• There seem to be multiple types of profiles (plan profile, composite profile, solver profile) that don't necessarily apply to every type of planner and don't share a common interface

I'm opening this discussion to brainstorm some ideas for unifying the high-level planning interfaces and making custom planning graphs easier to create.

Some questions to answer:

1. Can a common plan profile interface(s) work for every planner?
2. Are there multiple types of plan profile interfaces?
   • It seems to me that there are at least 2 distinct types of profiles:
     • Profiles applied at the solver: meta-information about how the solver works (e.g. number of iterations, planning time, etc.)
     • Profiles applied on a per-waypoint basis: meta-information that is specific to a particular waypoint (e.g. Descartes waypoint sampler/state evaluator, TrajOpt costs, etc.)
   • A per-waypoint profile is not relevant to some planners, notably OMPL. How would we handle that case?

[mpowelson]

This is probably a relevant discussion. As far as I know are currently 3 ways to set profiles

1. Preconfigure them

    auto ompl_planner = std::make_shared<OMPLMotionPlanner>();
    auto profile = std::make_shared<OMPLDefaultPlanProfile>();
    ompl_planner->plan_profiles["FREESPACE"] = profile;
    auto ompl_generator = std::make_unique<MotionPlannerTaskGenerator>(ompl_planner);

2. Send them in a profile override for a specific instruction

    PlanInstruction plan_via(wp, PlanInstructionType::FREESPACE, composite_profile);
    plan_via.profile_overrides->addProfile<TimeOptimalTrajectoryGenerationProfile>(
        "TOTG",
        std::make_shared<TimeOptimalTrajectoryGenerationProfile>(scaling_factor, 1.0, 0.01, 0.05, 0.001));

3. Pass them in with the input like you described.

It seems like 1 is what you are wanting. Is that incorrect? I am currently using 1 and 2 to create a GraphTaskflow.

[marip8]

For my project I need to run my own planning server directly with custom planner profiles and custom task flows. This is what I was hoping my work flow would be:

1. Construct planning server object (ProcessPlanningServer)
2. Construct various custom* planning task flow generators and add them to the planning server (registerProcessPlanner)
   • * custom in the sense that they don't exist yet in this repository, but not custom in the sense that they need information about how my planning server has been set up
   • It seems to me like this step should not depend on knowing what profiles have already been added to the server. I would like to be able to create task flow generators generically
3. Construct various custom planner profiles and add them to the planning server (addProfile)
4. Create planning request where each move instruction references the name of a profile that has been added to the planning server
5. Send the planning request
   • When a new taskflow is generated, the MotionPlannerTaskGenerator would create a new planner object and add the profiles defined in the TaskInput object to the planner

I'm not sold on either alternative:

1. Preconfigure them

The planning profiles are part of the input data of a specific task, so it seems like pre-configuring planners is duplication of effort and really just a hack for doing something that should self-contained within the task flow.

2. Send them in a profile override for a specific instruction

What is the purpose of a profile override? Can't you just add the "override" profile as an additional profile with a different name and use that name on the instructions you care about? This also seems like duplication of effort unless I'm missing some aspect of the override

[mpowelson]

Oh I see. I don't personally use the profile dictionary in the planning server. I just configure the planners as I create the taskflows rather than adding them to the server and relying on it to do it. I'll let Levi comment on what he was thinking for all this. I do know that the profile dictionary being added to the TaskInput came later out of that discussion above.

The overrides were added to give the user more flexibility for things that they might want to change when constructing the Instructions rather than when setting up the planners. In my case, I change the velocity scaling factor for time parameterization for every instruction at runtime. So while I could do it by adding a ton of profiles to the server and remapping all of them back to a few for all tasks but time parameterization, the overrides make it a essentially a one-liner from a user standpoint.

[marip8]

In my case, I change the velocity scaling factor for time parameterization for every instruction at runtime.

That makes sense. I haven't used any profiles that could be changed in a meaningful way at run-time, so I didn't think about that use-case.

I think I'm going to create a few UML-style diagrams that show the motion planning and process planning interfaces and how they relate to one another. It was challenging for me to understand the various layers of motion planning that exist currently, so I think a diagram would help in that respect. Hopefully it can help trigger some ideas for making the process planning pipeline more intuitive and easier to use as well. At the very least we can have some documentation about a few common workflows for setting up process planning pipelines, like mine using the planning server and yours configuring planners and profiles at run-time

[Levi-Armstrong]

I looked through everything and think the best solution would be to add the profile dictionary to the planning request and remove the storing of profiles from the generators. Then the planners will use this to find the correct profiles simplifying things.