Collection of convenience functions to help plan motion trajectories with MoveIt!. This class already maintains the necessary server clients and sends the requests. More...

#include <MoveItPlanner.h>

Public Member Functions
	MoveItPlanner (ros::NodeHandle &n, const std::string &_moveit_motion_plan_service="/plan_kinematic_path", const std::string &_moveit_check_state_validity_service="/check_state_validity")
moveit_msgs::MoveItErrorCodes	requestTrajectory (const geometry_msgs::PoseStamped &arm_base_pose, float arm_reach_span, const std::string &planning_group, const moveit_msgs::Constraints &goal_constraints, const moveit_msgs::Constraints *path_constraints, const sensor_msgs::JointState &start_state, moveit_msgs::RobotTrajectory &result_traj)
moveit_msgs::MoveItErrorCodes	requestTrajectoryForMobileRobot (const geometry_msgs::PoseStamped &start_pose, const geometry_msgs::PoseStamped &target_pose, float arm_reach_span, const std::string &planning_group, const moveit_msgs::Constraints &goal_constraints, const moveit_msgs::Constraints *path_constraints, const sensor_msgs::JointState &start_state, const std::string &virtual_joint_name, const std::string &target_frame_virtual_joint, moveit_msgs::RobotTrajectory &result_traj)
	~MoveItPlanner ()
Static Public Member Functions
static moveit_msgs::PositionConstraint	getBoxConstraint (const std::string &link_name, const geometry_msgs::PoseStamped &box_origin, const double &x, const double &y, const double &z)
static moveit_msgs::Constraints	getJointConstraint (const sensor_msgs::JointState &joint_state, const float &joint_tolerance)
static moveit_msgs::OrientationConstraint	getOrientationConstraint (const std::string &link_name, const geometry_msgs::QuaternionStamped &quat, const float &x_tolerance, const float &y_tolerance, const float &z_tolerance)
static moveit_msgs::Constraints	getPoseConstraint (const std::string &link_name, const geometry_msgs::PoseStamped &pose, double tolerance_pos, double tolerance_angle, int type)
static moveit_msgs::PositionConstraint	getSpherePoseConstraint (const std::string &link_name, const geometry_msgs::PoseStamped &target_pose, float arm_reach_span)
Private Member Functions
bool	init ()
bool	isValidState (const moveit_msgs::RobotState &robot_state, const std::string &group)
bool	makeWorkspace (const geometry_msgs::PoseStamped &from, const geometry_msgs::PoseStamped &to, float arm_reach_span, moveit_msgs::WorkspaceParameters &result) const
bool	makeWorkspace (const geometry_msgs::PoseStamped &origin, float arm_reach_span, moveit_msgs::WorkspaceParameters &wspace) const
moveit_msgs::MoveItErrorCodes	requestTrajectory (const std::string &group_name, const std::vector< moveit_msgs::Constraints > &goal_constraints, const sensor_msgs::JointState &from_state, const moveit_msgs::WorkspaceParameters &wspace, const sensor_msgs::MultiDOFJointState mdjs, const moveit_msgs::Constraints path_constraints, moveit_msgs::RobotTrajectory &result)
moveit_msgs::MotionPlanResponse	sendServiceRequest (moveit_msgs::MotionPlanRequest &request)
void	shutdown ()
Private Attributes
ros::ServiceClient	motion_plan_client
std::string	moveit_check_state_validity_service
std::string	moveit_motion_plan_service
ros::NodeHandle	node
ros::ServiceClient	state_validity_client

Detailed Description

Collection of convenience functions to help plan motion trajectories with MoveIt!. This class already maintains the necessary server clients and sends the requests.

Author:: Jennifer Buehler

Date:: March 2016

Definition at line 25 of file MoveItPlanner.h.

Constructor & Destructor Documentation

MoveItPlanner::MoveItPlanner	(	ros::NodeHandle &	n,
		const std::string &	_moveit_motion_plan_service = `"/plan_kinematic_path"`,
		const std::string &	_moveit_check_state_validity_service = `"/check_state_validity"`
	)

Parameters:

_moveit_motion_plan_service	motion plan service of moveit (to plan a kinematic path)
_moveit_check_state_validity_service	state validity checking service of moveit

Definition at line 14 of file MoveItPlanner.cpp.

MoveItPlanner::~MoveItPlanner ( )

Definition at line 24 of file MoveItPlanner.cpp.

Member Function Documentation

moveit_msgs::PositionConstraint MoveItPlanner::getBoxConstraint	(	const std::string &	link_name,
		const geometry_msgs::PoseStamped &	box_origin,
		const double &	x,
		const double &	y,
		const double &	z
	)		`[static]`

Builds a position constraint for link_name with a box at box_origin of the given x/y/z dimensions.

Definition at line 323 of file MoveItPlanner.cpp.

moveit_msgs::Constraints MoveItPlanner::getJointConstraint	(	const sensor_msgs::JointState &	joint_state,
		const float &	joint_tolerance
	)		`[static]`

Builds the joint constraint such that it corresponds to the passed joint_state. It is recommended to pass only values between -PI and PI in the position of joint_state to ensure compatibility with MoveIt!.

Definition at line 348 of file MoveItPlanner.cpp.

moveit_msgs::OrientationConstraint MoveItPlanner::getOrientationConstraint	(	const std::string &	link_name,
		const geometry_msgs::QuaternionStamped &	quat,
		const float &	x_tolerance,
		const float &	y_tolerance,
		const float &	z_tolerance
	)		`[static]`

Builds a constraint at which link_name has the given orientation and tolerances.

Definition at line 364 of file MoveItPlanner.cpp.

moveit_msgs::Constraints MoveItPlanner::getPoseConstraint	(	const std::string &	link_name,
		const geometry_msgs::PoseStamped &	pose,
		double	tolerance_pos,
		double	tolerance_angle,
		int	type
	)		`[static]`

Builds goal constraint for the link to be at this pose.

Parameters:

type	if 0, only position is considered. If 1, position and orientation are considered, and if 2 then only orientation is considered.

Definition at line 410 of file MoveItPlanner.cpp.

moveit_msgs::PositionConstraint MoveItPlanner::getSpherePoseConstraint	(	const std::string &	link_name,
		const geometry_msgs::PoseStamped &	target_pose,
		float	arm_reach_span
	)		`[static]`

Builds a position constraint for link_name with a sphere around the target_pose

Parameters:

arm_reach_span the maximum span the arm can reach (radius of the reaching sphere). Can be an overestimation of it, but should not underestimate.

Definition at line 380 of file MoveItPlanner.cpp.

bool MoveItPlanner::init ( ) [private]

Definition at line 29 of file MoveItPlanner.cpp.

bool MoveItPlanner::isValidState	(	const moveit_msgs::RobotState &	robot_state,
		const std::string &	group
	)		`[private]`

Definition at line 241 of file MoveItPlanner.cpp.

bool MoveItPlanner::makeWorkspace	(	const geometry_msgs::PoseStamped &	from,
		const geometry_msgs::PoseStamped &	to,
		float	arm_reach_span,
		moveit_msgs::WorkspaceParameters &	result
	)		const `[private]`

Creates a moveit! workspace that suits the robot having to move from the current position (from) to reach to position 'to'. The workspace will be generated as a box between robot_pose and target_pose, which will be fruther extended by arm_reach_span

Parameters:

arm_reach_span	maximum distance that the robot can reach out from the current position (frame_id in 'from')
from	the position where the robot currently is located (this should be the base frame ID of the robot)
to	the position where the robot should reach. This can be in any frame.

Returns:: false if transform between poses is not possible

Definition at line 200 of file MoveItPlanner.cpp.

bool MoveItPlanner::makeWorkspace	(	const geometry_msgs::PoseStamped &	origin,
		float	arm_reach_span,
		moveit_msgs::WorkspaceParameters &	wspace
	)		const `[private]`

Creates a workspace as an axis-aligned box around , where distance from origin along each axis to the box boundary is arm_reach_span

Definition at line 226 of file MoveItPlanner.cpp.

moveit_msgs::MoveItErrorCodes MoveItPlanner::requestTrajectory	(	const geometry_msgs::PoseStamped &	arm_base_pose,
		float	arm_reach_span,
		const std::string &	planning_group,
		const moveit_msgs::Constraints &	goal_constraints,
		const moveit_msgs::Constraints *	path_constraints,
		const sensor_msgs::JointState &	start_state,
		moveit_msgs::RobotTrajectory &	result_traj
	)

Returns a trajectory planned by MoveIt!.

Parameters:

arm_base_pose	the pose of the base of the arm. This is required to generate the MoveIt! workspace.
arm_reach_span	maximum distance from frame in arm_base_pose to the end effector when it is stretched out at its furthest. This value will be used to further extend the workspace so that the full space of the arm reachability is covered.
path_constraints	path constraints for motion planning, or NULL if none are to be used.
planning_group	name of the planning group
goal_constraints	the goal constraints for planning
start_state	the start joint state to plan from. Only joints in the planning group may be in the joint state!

Definition at line 44 of file MoveItPlanner.cpp.

moveit_msgs::MoveItErrorCodes MoveItPlanner::requestTrajectory	(	const std::string &	group_name,
		const std::vector< moveit_msgs::Constraints > &	goal_constraints,
		const sensor_msgs::JointState &	from_state,
		const moveit_msgs::WorkspaceParameters &	wspace,
		const sensor_msgs::MultiDOFJointState *	mdjs,
		const moveit_msgs::Constraints *	path_constraints,
		moveit_msgs::RobotTrajectory &	result
	)		`[private]`

Helper function to compute a trajectory including specification of the current MultiDOFJointState of a robot.

Parameters:

mdjs	the MultiDOFJointState of the robot, if it is a mobile robot which has a virtual joint. NULL if this is not to be used.
group_name	name of the planning group
goal_constraints	the goal constraints for planning
start_state	the start joint state to plan from. Only joints in the planning group may be in the joint state!

Definition at line 114 of file MoveItPlanner.cpp.

moveit_msgs::MoveItErrorCodes MoveItPlanner::requestTrajectoryForMobileRobot	(	const geometry_msgs::PoseStamped &	start_pose,
		const geometry_msgs::PoseStamped &	target_pose,
		float	arm_reach_span,
		const std::string &	planning_group,
		const moveit_msgs::Constraints &	goal_constraints,
		const moveit_msgs::Constraints *	path_constraints,
		const sensor_msgs::JointState &	start_state,
		const std::string &	virtual_joint_name,
		const std::string &	target_frame_virtual_joint,
		moveit_msgs::RobotTrajectory &	result_traj
	)

Like requestTrajectory(), but does so for mobile robots with a virtual joint. TODO: This needs another test run as it's a copy from previously used code.

Parameters:

robot_pose	the robot pose to plan from. This is required to generate the MoveIt! workspace and the robot's path to navigate. See also target_pose.
target_pose	the target pose which the robot should be able to reach. This is only used for creating the workspace, so it can be the pose furthest from robot_pose which the robot can navigate to, or reach the arm out to. The workspace will be generated as a box between robot_pose and target_pose, which will be fruther extended by arm_reach_span
arm_reach_span	maximum distance from frame in start_pose to the end effector when it is stretched out at its furthest. This value will be used to further extend the workspace so that the full space of the arm reachability is covered.
path_constraints	path constraints for motion planning, or NULL if none are to be used.
planning_group	name of the planning group
goal_constraints	the goal constraints for planning
start_state	the start joint state to plan from. Only joints in the planning group may be in the joint state!
virtual_joint_name	the name of the virtual joint
target_frame_virtual_joint	MoveIt! likes to have the virtual joint state in the frame which it was specified in its configuration. Specify this frame here, e.g. "odom".