The PlanningContext specialization that wraps the STOMP algorithm. More...

#include <stomp_planner.h>

Inheritance diagram for stomp_moveit::StompPlanner:

Public Member Functions
bool	canServiceRequest (const moveit_msgs::MotionPlanRequest &req) const
	Checks some conditions to determine whether it is able to plan given for this planning request.
virtual void	clear () override
	Clears results from previous plan.
virtual bool	solve (planning_interface::MotionPlanResponse &res) override
	Solve the motion planning problem as defined in the motion request passed before hand.
virtual bool	solve (planning_interface::MotionPlanDetailedResponse &res) override
	Solve the motion planning problem as defined in the motion request passed before hand.
	StompPlanner (const std::string &group, const XmlRpc::XmlRpcValue &config, const moveit::core::RobotModelConstPtr &model)
	StompPlanner constructor.
virtual bool	terminate () override
	Thread-safe method that request early termination, if a solve() function is currently computing plans.
virtual	~StompPlanner ()
Static Public Member Functions
static moveit_msgs::TrajectoryConstraints	encodeSeedTrajectory (const trajectory_msgs::JointTrajectory &seed)
	From a trajectory_msgs::JointTrajectory create a set of trajectory constraints that stomp can use later as a 'seed' for the optimization planning.
static bool	getConfigData (ros::NodeHandle &nh, std::map< std::string, XmlRpc::XmlRpcValue > &config, std::string param=std::string("stomp"))
	Convenience method to load extract the parameters for each supported planning group.
Protected Member Functions
bool	extractSeedTrajectory (const moveit_msgs::MotionPlanRequest &req, trajectory_msgs::JointTrajectory &seed) const
	Populates a seed joint trajectory from the 'trajectory_constraints' moveit_msgs::Constraints[] array. each entry in the array is considered to be joint values for that time step.
bool	getSeedParameters (Eigen::MatrixXd &parameters) const
	This function 1) gets the seed trajectory from the active motion plan request, 2) checks to see if the given seed trajectory makes sense in the context of the user provided goal constraints, 3) modifies the seed's first and last point to 'fix' it for small deviations in the goal constraints and 4) applies a smoothing method to the seed.
bool	getStartAndGoal (Eigen::VectorXd &start, Eigen::VectorXd &goal)
	Gets the start and goal joint values from the motion plan request passed.
bool	jointTrajectorytoParameters (const trajectory_msgs::JointTrajectory &traj, Eigen::MatrixXd &parameters) const
	Converts from a joint trajectory to an Eigen Matrix.
bool	parametersToJointTrajectory (const Eigen::MatrixXd &parameters, trajectory_msgs::JointTrajectory &traj)
	Converts from an Eigen Matrix to to a joint trajectory.
void	setup ()
	planner setup
Protected Attributes
XmlRpc::XmlRpcValue	config_
ros::NodeHandlePtr	ph_
moveit::core::RobotModelConstPtr	robot_model_
boost::shared_ptr < stomp_core::Stomp >	stomp_
stomp_core::StompConfiguration	stomp_config_
StompOptimizationTaskPtr	task_

Detailed Description

The PlanningContext specialization that wraps the STOMP algorithm.

Examples:: All examples are located here stomp_core examples

Definition at line 47 of file stomp_planner.h.

Constructor & Destructor Documentation

stomp_moveit::StompPlanner::StompPlanner	(	const std::string &	group,
		const XmlRpc::XmlRpcValue &	config,
		const moveit::core::RobotModelConstPtr &	model
	)

StompPlanner constructor.

Parameters:

group	The planning group for which this instance will plan.
config	The parameter containing the configuration data for this planning group, includes plugins specifications.
model	A pointer to the robot model.

Definition at line 105 of file stomp_planner.cpp.

stomp_moveit::StompPlanner::~StompPlanner ( ) [virtual]

Definition at line 115 of file stomp_planner.cpp.

Member Function Documentation

bool stomp_moveit::StompPlanner::canServiceRequest ( const moveit_msgs::MotionPlanRequest & req ) const

Checks some conditions to determine whether it is able to plan given for this planning request.

Returns:: true if succeeded, false otherwise.

Definition at line 672 of file stomp_planner.cpp.

void stomp_moveit::StompPlanner::clear ( void ) [override, virtual]

Clears results from previous plan.

Implements planning_interface::PlanningContext.

Definition at line 711 of file stomp_planner.cpp.

moveit_msgs::TrajectoryConstraints stomp_moveit::StompPlanner::encodeSeedTrajectory ( const trajectory_msgs::JointTrajectory & seed ) [static]

From a trajectory_msgs::JointTrajectory create a set of trajectory constraints that stomp can use later as a 'seed' for the optimization planning.

Parameters:

seed	The trajectory to encode into 'seed' trajectory constraints

Returns:: The encoded trajectory constraints which can be added directly to a moveit planning request. In the case of failure, may throw a std::runtime_error.

Definition at line 536 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::extractSeedTrajectory	(	const moveit_msgs::MotionPlanRequest &	req,
		trajectory_msgs::JointTrajectory &	seed
	)		const `[protected]`

Populates a seed joint trajectory from the 'trajectory_constraints' moveit_msgs::Constraints[] array. each entry in the array is considered to be joint values for that time step.

Parameters:

req	The motion plan request containing the seed trajectory in the 'trajectory_constraints' field.
seed	The output seed trajectory which is used to initialize the STOMP optimization

Returns:: true if succeeded, false otherwise.

Definition at line 495 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::getConfigData	(	ros::NodeHandle &	nh,
		std::map< std::string, XmlRpc::XmlRpcValue > &	config,
		std::string	param = `std::string("stomp")`
	)		`[static]`

Convenience method to load extract the parameters for each supported planning group.

Parameters:

nh	A ros node handle.
config	A map containing the configuration data for each planning group found.
param	The parameter name containing the confuration data for all planning groups.

Returns:: true if succeeded, false otherwise.

Definition at line 716 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::getSeedParameters ( Eigen::MatrixXd & parameters ) const [protected]

This function 1) gets the seed trajectory from the active motion plan request, 2) checks to see if the given seed trajectory makes sense in the context of the user provided goal constraints, 3) modifies the seed's first and last point to 'fix' it for small deviations in the goal constraints and 4) applies a smoothing method to the seed.

Parameters:

parameters Output argument containing the seed parameters

Returns:: True if the seed state is deemed to be valid. False otherwise.

Definition at line 294 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::getStartAndGoal	(	Eigen::VectorXd &	start,
		Eigen::VectorXd &	goal
	)		`[protected]`

Gets the start and goal joint values from the motion plan request passed.

Parameters:

start	The start joint values
goal	The goal joint values

Returns:: true if succeeded, false otherwise.

Definition at line 564 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::jointTrajectorytoParameters	(	const trajectory_msgs::JointTrajectory &	traj,
		Eigen::MatrixXd &	parameters
	)		const `[protected]`

Converts from a joint trajectory to an Eigen Matrix.

Parameters:

traj	The input trajectory in joint space.
parameters	The matrix of size [num joints][num_timesteps] containing the trajectory joint values.

Returns:: true if succeeded, false otherwise.

Definition at line 476 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::parametersToJointTrajectory	(	const Eigen::MatrixXd &	parameters,
		trajectory_msgs::JointTrajectory &	traj
	)		`[protected]`

Converts from an Eigen Matrix to to a joint trajectory.

Parameters:

parameters	The input matrix of size [num joints][num_timesteps] containing the trajectory joint values.
traj	A trajectory in joint space.

Returns:: true if succeeded, false otherwise.

Definition at line 437 of file stomp_planner.cpp.

void stomp_moveit::StompPlanner::setup ( ) [protected]

planner setup

Definition at line 119 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::solve ( planning_interface::MotionPlanResponse & res ) [override, virtual]

Solve the motion planning problem as defined in the motion request passed before hand.

Parameters:

res	Contains the solved planned path.

Returns:: true if succeeded, false otherwise.

Implements planning_interface::PlanningContext.

Definition at line 158 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::solve ( planning_interface::MotionPlanDetailedResponse & res ) [override, virtual]

Solve the motion planning problem as defined in the motion request passed before hand.

Parameters:

res	Contains the solved planned path.

Returns:: true if succeeded, false otherwise.

Implements planning_interface::PlanningContext.

Definition at line 173 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::terminate ( ) [override, virtual]

Thread-safe method that request early termination, if a solve() function is currently computing plans.

Returns:: true if succeeded, false otherwise.

Implements planning_interface::PlanningContext.

Definition at line 698 of file stomp_planner.cpp.

Member Data Documentation

XmlRpc::XmlRpcValue stomp_moveit::StompPlanner::config_ [protected]

Definition at line 163 of file stomp_planner.h.

ros::NodeHandlePtr stomp_moveit::StompPlanner::ph_ [protected]

Definition at line 170 of file stomp_planner.h.

moveit::core::RobotModelConstPtr stomp_moveit::StompPlanner::robot_model_ [protected]

Definition at line 167 of file stomp_planner.h.

boost::shared_ptr< stomp_core::Stomp> stomp_moveit::StompPlanner::stomp_ [protected]

Definition at line 161 of file stomp_planner.h.

stomp_core::StompConfiguration stomp_moveit::StompPlanner::stomp_config_ [protected]

Definition at line 164 of file stomp_planner.h.

StompOptimizationTaskPtr stomp_moveit::StompPlanner::task_ [protected]

Definition at line 162 of file stomp_planner.h.

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

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation