Uses an interpolated IK approach from pregrasp to final grasp. More...

#include <GraspSequenceValidator.h>

Inheritance diagram for GraspSequenceValidator:

Classes
struct	GraspSequenceDetails
Public Member Functions
void	getGroupJoints (const std::string &group_name, std::vector< std::string > &group_links)
void	getGroupLinks (const std::string &group_name, std::vector< std::string > &group_links)
	GraspSequenceValidator (planning_environment::CollisionModels *cm=NULL, const std::string &plugin_name="")
	Also adds a grasp marker at the pre-grasp location.
void	setPlanningSceneState (planning_models::KinematicState *state)
virtual void	testGrasp (const object_manipulation_msgs::PickupGoal &pickup_goal, const object_manipulation_msgs::Grasp &grasp, object_manipulator::GraspExecutionInfo &execution_info)
virtual void	testGrasps (const object_manipulation_msgs::PickupGoal &pickup_goal, const std::vector< object_manipulation_msgs::Grasp > &grasps, std::vector< object_manipulator::GraspExecutionInfo > &execution_info, bool return_on_first_hit)
virtual void	testGrasps (const object_manipulation_msgs::PickupGoal &pickup_goal, const std::vector< object_manipulation_msgs::Grasp > &grasps, double twistAngle, std::vector< GraspSequenceDetails > &execution_info, bool return_on_first_hit)
	~GraspSequenceValidator ()
Public Attributes
pluginlib::ClassLoader < kinematics::KinematicsBase >	kinematics_loader_
Protected Member Functions
planning_environment::CollisionModels *	getCollisionModels ()
bool	getInterpolatedIK (const std::string &arm_name, const tf::Transform &first_pose, const tf::Vector3 &direction, const double &distance, const std::vector< double > &ik_solution, const bool &reverse, const bool &premultiply, trajectory_msgs::JointTrajectory &traj)
bool	getInterpolatedIK (const std::string &arm_name, const tf::Transform &initialTransform, const tf::Quaternion &rot, const std::vector< double > &ikSolutionAtStart, std::vector< double > &ikSolutionAtEnd, trajectory_msgs::JointTrajectory &traj)
planning_models::KinematicState *	getPlanningSceneState ()
virtual std::vector < arm_navigation_msgs::LinkPadding >	linkPaddingForGrasp (const object_manipulation_msgs::PickupGoal &pickup_goal)
	Dynamic link padding to be used for grasp operation.
Protected Attributes
planning_environment::CollisionModels *	cm_
double	consistent_angle_
std::map< std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware * >	ik_solver_map_
unsigned int	num_points_
unsigned int	redundancy_
planning_models::KinematicState *	state_
ros::Publisher	vis_marker_array_publisher_
ros::Publisher	vis_marker_publisher_

Detailed Description

Uses an interpolated IK approach from pregrasp to final grasp.

Initial check consists of IK for the pre-grasp, followed by generation of an interpolated IK path from pre-grasp to grasp. This is then followed by computation of an interpolated IK path from grasp to lift.

Execution consists of using move arm to go to the pre-grasp, then execution of the interpolated IK paths for grasp. Lift is executed separately, called from the higher level executor.

Note that we do not use the pre-grasp from the database directly; rather, the pre-grasp is obtained by backing up the grasp by a pre-defined amount. This allows us more flexibility in choosing the pre-grasp. However, we can no longer always assume the pre-grasp is collision free, which we could if we used the pre-grasp from the database directly.

In the most recent database version, the pre-grasp was obtained by backing up 10 cm, so we know at least that that is not colliding with the object.

Definition at line 36 of file GraspSequenceValidator.h.

Constructor & Destructor Documentation

GraspSequenceValidator::GraspSequenceValidator	(	planning_environment::CollisionModels *	cm = `NULL`,
		const std::string &	plugin_name = `""`
	)

Also adds a grasp marker at the pre-grasp location.

Definition at line 19 of file GraspSequenceValidator.cpp.

GraspSequenceValidator::~GraspSequenceValidator ( )

Definition at line 60 of file GraspSequenceValidator.cpp.

Member Function Documentation

planning_environment::CollisionModels * GraspSequenceValidator::getCollisionModels ( void ) [protected]

Definition at line 65 of file GraspSequenceValidator.cpp.

void GraspSequenceValidator::getGroupJoints	(	const std::string &	group_name,
		std::vector< std::string > &	group_links
	)

Definition at line 114 of file GraspSequenceValidator.cpp.

void GraspSequenceValidator::getGroupLinks	(	const std::string &	group_name,
		std::vector< std::string > &	group_links
	)

Definition at line 104 of file GraspSequenceValidator.cpp.

bool GraspSequenceValidator::getInterpolatedIK	(	const std::string &	arm_name,
		const tf::Transform &	first_pose,
		const tf::Vector3 &	direction,
		const double &	distance,
		const std::vector< double > &	ik_solution,
		const bool &	reverse,
		const bool &	premultiply,
		trajectory_msgs::JointTrajectory &	traj
	)		`[protected]`

Definition at line 124 of file GraspSequenceValidator.cpp.

bool GraspSequenceValidator::getInterpolatedIK	(	const std::string &	arm_name,
		const tf::Transform &	initialTransform,
		const tf::Quaternion &	rot,
		const std::vector< double > &	ikSolutionAtStart,
		std::vector< double > &	ikSolutionAtEnd,
		trajectory_msgs::JointTrajectory &	traj
	)		`[protected]`

Definition at line 165 of file GraspSequenceValidator.cpp.

planning_models::KinematicState * GraspSequenceValidator::getPlanningSceneState ( ) [protected]

Definition at line 77 of file GraspSequenceValidator.cpp.

std::vector< arm_navigation_msgs::LinkPadding > GraspSequenceValidator::linkPaddingForGrasp ( const object_manipulation_msgs::PickupGoal & pickup_goal ) [protected, virtual]

Dynamic link padding to be used for grasp operation.

Zero padding on fingertip links

Definition at line 96 of file GraspSequenceValidator.cpp.

void GraspSequenceValidator::setPlanningSceneState ( planning_models::KinematicState * state ) [inline]

Definition at line 92 of file GraspSequenceValidator.h.

virtual void GraspSequenceValidator::testGrasp	(	const object_manipulation_msgs::PickupGoal &	pickup_goal,
		const object_manipulation_msgs::Grasp &	grasp,
		object_manipulator::GraspExecutionInfo &	execution_info
	)		`[inline, virtual]`

Definition at line 103 of file GraspSequenceValidator.h.

void GraspSequenceValidator::testGrasps	(	const object_manipulation_msgs::PickupGoal &	pickup_goal,
		const std::vector< object_manipulation_msgs::Grasp > &	grasps,
		std::vector< object_manipulator::GraspExecutionInfo > &	execution_info,
		bool	return_on_first_hit
	)		`[virtual]`

Definition at line 226 of file GraspSequenceValidator.cpp.

void GraspSequenceValidator::testGrasps	(	const object_manipulation_msgs::PickupGoal &	pickup_goal,
		const std::vector< object_manipulation_msgs::Grasp > &	grasps,
		double	twistAngle,
		std::vector< GraspSequenceDetails > &	execution_info,
		bool	return_on_first_hit
	)		`[virtual]`