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. More...

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. More...

virtual bool	solve (planning_interface::MotionPlanDetailedResponse &res) override
	Solve the motion planning problem as defined in the motion request passed before hand. More...

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

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

Public Member Functions inherited from planning_interface::PlanningContext
const std::string &	getGroupName () const

const MotionPlanRequest &	getMotionPlanRequest () const

const std::string &	getName () const

const planning_scene::PlanningSceneConstPtr &	getPlanningScene () const

	PlanningContext (const std::string &name, const std::string &group)

void	setMotionPlanRequest (const MotionPlanRequest &request)

void	setPlanningScene (const planning_scene::PlanningSceneConstPtr &planning_scene)

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. More...

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. More...

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. More...

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. More...

bool	getStartAndGoal (Eigen::VectorXd &start, Eigen::VectorXd &goal)
	Gets the start and goal joint values from the motion plan request passed. More...

bool	jointTrajectorytoParameters (const trajectory_msgs::JointTrajectory &traj, Eigen::MatrixXd &parameters) const
	Converts from a joint trajectory to an Eigen Matrix. More...

bool	parametersToJointTrajectory (const Eigen::MatrixXd &parameters, trajectory_msgs::JointTrajectory &traj)
	Converts from an Eigen Matrix to to a joint trajectory. More...

void	setup ()
	planner setup

Protected Attributes
XmlRpc::XmlRpcValue	config_

utils::kinematics::IKSolverPtr	ik_solver_

ros::NodeHandlePtr	ph_

moveit::core::RobotModelConstPtr	robot_model_

std::shared_ptr< stomp_core::Stomp >	stomp_

stomp_core::StompConfiguration	stomp_config_

StompOptimizationTaskPtr	task_

Protected Attributes inherited from planning_interface::PlanningContext
std::string	group_

std::string	name_

planning_scene::PlanningSceneConstPtr	planning_scene_

MotionPlanRequest	request_

Detailed Description

The PlanningContext specialization that wraps the STOMP algorithm.

Examples:: All examples are located here STOMP MoveIt! examples

Definition at line 48 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.

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 705 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 557 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 516 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 configuration data for all planning groups.

Returns: true if succeeded, false otherwise.

Definition at line 751 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 297 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 585 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 497 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 454 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::solve ( planning_interface::MotionPlanResponse & res )

overridevirtual

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 159 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::solve ( planning_interface::MotionPlanDetailedResponse & res )

overridevirtual

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 175 of file stomp_planner.cpp.

bool stomp_moveit::StompPlanner::terminate ( )

overridevirtual

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 733 of file stomp_planner.cpp.

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