Plans Initial Grasping States (IGSs) for each selected grasp strategy. More...

#include <fog/igs_planner.h>

Collaboration diagram for fog::IGSPlanner:

Public Member Functions
	IGSPlanner (const Params &p1)

	~IGSPlanner ()

std::vector< InitialGraspState >	run (const Models &models, const EnabledStrategies &enabled_strategies, const std::vector< Eigen::Vector3d > &feasible_directions)
	Returns the planned multiple IGSs given the models, the enabled strategies and the feasible directions. More...

trajectory_msgs::JointTrajectory	getJointTrajectorySolution ()
	Returns the joint trajectory for reaching the selected IGS. Assumes that fog::IGSPlanner::planIGS has already be called. More...

Private Member Functions
std::vector< InitialGraspState >	produceIGSsForStrategyAlpha (const std::vector< Eigen::Vector3d > &feasible_directions, const Object &object, const Surface &surface)
	Produces IGS for strategy alpha. More...

std::vector< InitialGraspState >	produceIGSsForStrategyBeta (const ObjectSurfaceCombination &combination, const Vector3 surface_normal, double h, double phi, double theta)
	Produces IGS for strategy beta. More...

std::vector< InitialGraspState >	produceIGSsForStrategyGamma (const Point3 &object_position, const Vector3 &surface_normal, const Vector3 &closest_surface_limit, double epsilon)
	Produces IGS for strategy gamma. More...

void	selectIGS (const std::vector< InitialGraspState > &igss, InitialGraspState &result, trajectory_msgs::JointTrajectory &solution, const std::string &group_name)
	Selects one IGSs given the total produced IGS, based on their weights. Also calculates the joint trajectory for reaching the selected IGS. More...

Eigen::Vector3d	calculateDMin (const Eigen::Vector3d &bounding_box, const Eigen::Vector3d &surface_normal, const Eigen::Quaterniond &object_orientation)
	Calculates the direction vector $\mathbf{d}_{min}$ used in Eq. 19 of the manuscript. More...

unsigned int	calcIndexOfClosestSurfaceEdgeFromObject (const Point3 &object_position, const Surface &surface)
	Calculates which of the four edges of the support surface is closest to the object. Used by strategy- $\gamma$ . More...

Private Attributes
std::string	name

trajectory_msgs::JointTrajectory	solution
	Stores the joint trajectory solution for the selected IGS. More...

Params	params

Detailed Description

Plans Initial Grasping States (IGSs) for each selected grasp strategy.

This class assumes that fog::ModelExtractor has extracted the models and that the fog::DecisionMaker has enabled the strategies.

See Also: fog::InitialGraspState

Constructor & Destructor Documentation

fog::IGSPlanner::IGSPlanner ( const Params & p1 )

inline

fog::IGSPlanner::~IGSPlanner ( )

Member Function Documentation

unsigned int fog::IGSPlanner::calcIndexOfClosestSurfaceEdgeFromObject	(	const Point3 &	object_position,
		const Surface &	surface
	)

private

Calculates which of the four edges of the support surface is closest to the object. Used by strategy- $\gamma$ .

Parameters

object_position	The position of the object
surface	The model of the surface

Returns: The index of the one of the four edges as the closest.

Eigen::Vector3d fog::IGSPlanner::calculateDMin	(	const Eigen::Vector3d &	bounding_box,
		const Eigen::Vector3d &	surface_normal,
		const Eigen::Quaterniond &	object_orientation
	)

private

Calculates the direction vector $\mathbf{d}_{min}$ used in Eq. 19 of the manuscript.

Parameters

bounding_box	The object's bounding box.
surface_normal	The normal of the support surface.
object_orientation	The orientation of the target object.

Returns: The vector $\mathbf{d}_{min}$ .

trajectory_msgs::JointTrajectory fog::IGSPlanner::getJointTrajectorySolution ( )

Returns the joint trajectory for reaching the selected IGS. Assumes that fog::IGSPlanner::planIGS has already be called.

Returns: The joint trajectory.

std::vector< InitialGraspState > fog::IGSPlanner::produceIGSsForStrategyAlpha	(	const std::vector< Eigen::Vector3d > &	feasible_directions,
		const Object &	object,
		const Surface &	surface
	)

private

Produces IGS for strategy alpha.

Parameters

feasible_directions	The feasible directions.
object	The target object.
surface	The support surface.

Returns: The set of IGSs for strategy alpha

Calculate the weight (manuscript v1.10 eq.18)

Here is the call graph for this function:

std::vector< InitialGraspState > fog::IGSPlanner::produceIGSsForStrategyBeta	(	const ObjectSurfaceCombination &	combination,
		const Vector3	surface_normal,
		double	h,
		double	phi,
		double	theta
	)

private

Produces IGS for strategy beta.

Returns: The set of IGSs for strategy beta

std::vector< InitialGraspState > fog::IGSPlanner::produceIGSsForStrategyGamma	(	const Point3 &	object_position,
		const Vector3 &	surface_normal,
		const Vector3 &	closest_surface_limit,
		double	epsilon
	)

private

Produces IGS for strategy gamma.

Parameters

object_position	The position of the object
surface_normal	The normal vector of the surface
surface_limit	The limit vector of the surface
epsilon	The parameter $\epsilon$ , see Eq. (30) [1]

Returns: The set of IGSs for strategy gamma

std::vector< InitialGraspState > fog::IGSPlanner::run	(	const Models &	models,
		const EnabledStrategies &	enabled_strategies,
		const std::vector< Eigen::Vector3d > &	feasible_directions
	)

Returns the planned multiple IGSs given the models, the enabled strategies and the feasible directions.

Parameters

models	The extracted models from the scene.
enabled_strategies	The enabled strategies from the Decision Maker.
feasible_directions	The feasible directions from the Decision Maker.

Returns: The IGSs for all the enabled strategies, sorted by their weight.

Here is the call graph for this function:

void fog::IGSPlanner::selectIGS	(	const std::vector< InitialGraspState > &	igss,
		InitialGraspState &	result,
		trajectory_msgs::JointTrajectory &	solution,
		const std::string &	group_name
	)

private

Selects one IGSs given the total produced IGS, based on their weights. Also calculates the joint trajectory for reaching the selected IGS.

Parameters

igss	The total number of IGSs as an input.
result	The selected IGS as a result.
solution	The joint trajectory for the selected IGS.
group_name	The MoveIt group name of the robot.

Member Data Documentation

std::string fog::IGSPlanner::name

private

Params fog::IGSPlanner::params

private

trajectory_msgs::JointTrajectory fog::IGSPlanner::solution

private

Stores the joint trajectory solution for the selected IGS.

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation