Wrapper for easy access to DART planning tools via standard Robowflex interface. More...

#include <planner.h>

Inheritance diagram for robowflex::darts::DARTPlanner:

Public Member Functions
	DARTPlanner (const robowflex::RobotPtr &robot, const std::string &name="DARTPlanner")
	Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot. More...

void	preRun (const robowflex::SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	This function is called before benchmarking. Here, it is used to setup the DART scene before the solve method. More...

planning_interface::MotionPlanResponse	plan (const robowflex::SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	Plan a motion given a request and a scene. More...

std::vector< std::string >	getPlannerConfigs () const override
	Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan(). More...

Public Member Functions inherited from robowflex::Planner
	Planner (const RobotPtr &robot, const std::string &name="")
	Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot. More...

	Planner (Planner const &)=delete

void	operator= (Planner const &)=delete

virtual std::map< std::string, ProgressProperty >	getProgressProperties (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) const
	Retrieve the planner progress property map for this planner given a specific request. More...

const RobotPtr	getRobot () const
	Return the robot for this planner. More...

const std::string &	getName () const
	Get the name of the planner. More...

Public Attributes
PlanBuilderPtr	builder
	DART Motion Plan Builder. More...

Private Types
using	PlannerAllocator = std::function< ompl::base::PlannerPtr()>
	A funciton that returns an allocated planner. More...

Private Member Functions
template<typename T , typename... Args>
PlannerAllocator	makePlanner (Args &&... args)
	Macro for creating and setting up an OMPL planner. More...

void	setupPlanners ()
	Setup planner allocators. More...

Private Attributes
std::map< std::string, PlannerAllocator >	planner_allocators_
	Named planner allocators. More...

SceneConstPtr	scene_
	Current planning request scene. More...

StructurePtr	dart_scene_
	DART version of current planning request scene. More...

RobotPtr	dart_robot_
	DART version of the robot. More...

WorldPtr	world_
	DART world containing robot and scene. More...

ompl::base::GoalPtr	goal_
	Current motion planning goal. More...

Additional Inherited Members
Public Types inherited from robowflex::Planner
using	ProgressProperty = std::function< std::string()>
	A function that returns the value of a planner property over the course of a run. More...

Protected Attributes inherited from robowflex::Planner
RobotPtr	robot_
	The robot to plan for. More...

IO::Handler	handler_
	The parameter handler for the planner. More...

const std::string	name_
	Namespace for the planner. More...

Detailed Description

Wrapper for easy access to DART planning tools via standard Robowflex interface.

Definition at line 25 of file planner.h.

Member Typedef Documentation

◆ PlannerAllocator

using robowflex::darts::DARTPlanner::PlannerAllocator = std::function<ompl::base::PlannerPtr()>

private

A funciton that returns an allocated planner.

Definition at line 64 of file planner.h.

Constructor & Destructor Documentation

◆ DARTPlanner()

DARTPlanner::DARTPlanner	(	const robowflex::RobotPtr &	robot,
		const std::string &	name = `"DARTPlanner"`
	)

Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot.

Parameters

[in]	robot	The robot to plan for.
[in]	name	Optional namespace for planner.

Definition at line 34 of file planner.cpp.

   : robowflex::Planner(robot, name)
   , dart_robot_(std::make_shared<Robot>(robot))
   , world_(std::make_shared<World>())
 {
     world_->addRobot(dart_robot_);
     setupPlanners();
     builder = std::make_shared<PlanBuilder>(world_);
 }

Member Function Documentation

◆ getPlannerConfigs()

std::vector< std::string > DARTPlanner::getPlannerConfigs ( ) const

overridevirtual

Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan().

Returns: A vector of strings of planner configuration names.

Implements robowflex::Planner.

Definition at line 157 of file planner.cpp.

 {
     std::vector<std::string> configs;
     for (const auto &pair : planner_allocators_)
         configs.emplace_back(pair.first);
  
     return configs;
 }

◆ makePlanner()

template<typename T , typename... Args>

PlannerAllocator robowflex::darts::DARTPlanner::makePlanner ( Args &&... args )

inlineprivate

Macro for creating and setting up an OMPL planner.

Parameters

[in] args Arguments to pass to the planner.

Template Parameters

Definition at line 72 of file planner.h.

             {
                 return [&]() -> ompl::base::PlannerPtr {
                     if (builder)
                     {
                         auto p = std::make_shared<T>(builder->info, std::forward<Args>(args)...);
                         p->setup();
                         return p;
                     }
  
                     return nullptr;
                 };
             }

◆ plan()

planning_interface::MotionPlanResponse DARTPlanner::plan	(	const robowflex::SceneConstPtr &	scene,
		const planning_interface::MotionPlanRequest &	request
	)

overridevirtual

Plan a motion given a request and a scene.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	The motion planning request to solve.

Returns: The motion planning response generated by the planner.

Implements robowflex::Planner.

Definition at line 102 of file planner.cpp.

 {
     // if scene and preRun scene mismatch, recompute
     if (scene != scene_)
         preRun(scene, request);
  
     // start sampling if necessary
     auto gls = std::dynamic_pointer_cast<ompl::base::GoalLazySamples>(goal_);
     if (gls)
         gls->startSampling();
  
     // solve the plan, time the request
     auto start = std::chrono::steady_clock::now();
     ompl::base::PlannerStatus solved = builder->ss->solve(request.allowed_planning_time);
     auto end = std::chrono::steady_clock::now();
  
     auto time = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
  
     // kill sampling
     if (gls)
         gls->stopSampling();
  
     // extract request
     planning_interface::MotionPlanResponse response;
     response.planning_time_ = 1e6 * (double)time;
     if (solved == ompl::base::PlannerStatus::EXACT_SOLUTION)
         response.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
     else
         response.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
  
     // extract trajectory
     auto path = builder->getSolutionPath();
  
     response.trajectory_ =
         std::make_shared<robot_trajectory::RobotTrajectory>(robot_->getModel(), request.group_name);
  
     for (const auto &s : path.getStates())
     {
         // set world state
         builder->rspace->setWorldState(world_, s);
  
         // copy over world state to moveit state
         auto ms = robot_->allocState();
         dart_robot_->setMoveItStateFromState(*ms);
  
         response.trajectory_->addSuffixWayPoint(ms, 0);
     }
  
     // compute time parameterization
     robowflex::Trajectory::computeTimeParameterization(*response.trajectory_);
  
     return response;
 }

◆ preRun()

void DARTPlanner::preRun	(	const robowflex::SceneConstPtr &	scene,
		const planning_interface::MotionPlanRequest &	request
	)

overridevirtual

This function is called before benchmarking. Here, it is used to setup the DART scene before the solve method.

Parameters

[in]	scene	Scene to plan for.
[in]	request	Planning request.

Reimplemented from robowflex::Planner.

Definition at line 78 of file planner.cpp.

 {
     // remove old scene, if one exists
     if (dart_scene_)
         world_->removeStructure(dart_scene_);
  
     // convert scene representation
     scene_ = scene;
     dart_scene_ = std::make_shared<Structure>("scene", scene);
     world_->addStructure(dart_scene_);
  
     // setup planning request and get desired goal
     goal_ = builder->fromMessage(robot_->getName(), request);
  
     // find and setup planner
     auto it = planner_allocators_.find(request.planner_id);
     if (it == planner_allocators_.end())
         throw std::runtime_error("Invalid planner ID");
  
     builder->ss->setPlanner(it->second());
     builder->setup();
 }

◆ setupPlanners()

void DARTPlanner::setupPlanners ( )

private

Setup planner allocators.

Definition at line 44 of file planner.cpp.

 {
     // rrts
     planner_allocators_.emplace("rrt::RRT", makePlanner<ompl::geometric::RRT>());
     planner_allocators_.emplace("rrt::RRTConnect", makePlanner<ompl::geometric::RRTConnect>());
     planner_allocators_.emplace("rrt::RRTConnectIntermediate",
                                 makePlanner<ompl::geometric::RRTConnect>(true));
     planner_allocators_.emplace("rrt::RRT*", makePlanner<ompl::geometric::RRTstar>());
     planner_allocators_.emplace("rrt::LazyRRT", makePlanner<ompl::geometric::LazyRRT>());
  
     // ests
     planner_allocators_.emplace("est::EST", makePlanner<ompl::geometric::EST>());
     planner_allocators_.emplace("est::BiEST", makePlanner<ompl::geometric::BiEST>());
     planner_allocators_.emplace("est::ProjEST", makePlanner<ompl::geometric::ProjEST>());
  
     // prms
     planner_allocators_.emplace("prm::PRM", makePlanner<ompl::geometric::PRM>());
     planner_allocators_.emplace("prm::PRM*", makePlanner<ompl::geometric::PRMstar>());
     planner_allocators_.emplace("prm::LazyPRM", makePlanner<ompl::geometric::LazyPRM>());
     planner_allocators_.emplace("prm::LazyPRM*", makePlanner<ompl::geometric::LazyPRMstar>());
     planner_allocators_.emplace("spars::SPARS", makePlanner<ompl::geometric::SPARS>());
     planner_allocators_.emplace("spars::SPARS2", makePlanner<ompl::geometric::SPARStwo>());
  
     // fmts
     planner_allocators_.emplace("fmt::FMT", makePlanner<ompl::geometric::FMT>());
     planner_allocators_.emplace("fmt::BFMT", makePlanner<ompl::geometric::BFMT>());
  
     // pdst / kpiece
     planner_allocators_.emplace("pdst::PDST", makePlanner<ompl::geometric::PDST>());
     planner_allocators_.emplace("kpiece::KPIECE", makePlanner<ompl::geometric::KPIECE1>());
     planner_allocators_.emplace("kpiece::BKPIECE", makePlanner<ompl::geometric::BKPIECE1>());
     planner_allocators_.emplace("kpiece::LBKPIECE", makePlanner<ompl::geometric::LBKPIECE1>());
 }

Member Data Documentation

◆ builder

PlanBuilderPtr robowflex::darts::DARTPlanner::builder

DART Motion Plan Builder.

Definition at line 59 of file planner.h.

◆ dart_robot_

RobotPtr robowflex::darts::DARTPlanner::dart_robot_

private

DART version of the robot.

Definition at line 93 of file planner.h.

◆ dart_scene_

StructurePtr robowflex::darts::DARTPlanner::dart_scene_

private

DART version of current planning request scene.

Definition at line 92 of file planner.h.

◆ goal_

ompl::base::GoalPtr robowflex::darts::DARTPlanner::goal_

private

Current motion planning goal.

Definition at line 95 of file planner.h.

◆ planner_allocators_

std::map<std::string, PlannerAllocator> robowflex::darts::DARTPlanner::planner_allocators_

private

Named planner allocators.

Definition at line 90 of file planner.h.

◆ scene_

SceneConstPtr robowflex::darts::DARTPlanner::scene_

private

Current planning request scene.

Definition at line 91 of file planner.h.

◆ world_

WorldPtr robowflex::darts::DARTPlanner::world_

private

DART world containing robot and scene.

Definition at line 94 of file planner.h.

The documentation for this class was generated from the following files:

Public Member Functions

Public Attributes

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ PlannerAllocator

Constructor & Destructor Documentation

◆ DARTPlanner()

Member Function Documentation

◆ getPlannerConfigs()

◆ makePlanner()

◆ plan()

◆ preRun()

◆ setupPlanners()

Member Data Documentation

◆ builder

◆ dart_robot_

◆ dart_scene_

◆ goal_

◆ planner_allocators_

◆ scene_

◆ world_