Inheritance diagram for HRICS::MultiHandOver:

Public Types
enum	SearchAlgo_t { NONE, RANDOM_SEARCH, LWA_SEARCH, WA_SEARCH }

Public Member Functions
	MultiHandOver (double cell_size=0.20)

void	testForReinit ()

void	createGrid ()

void	dumpVar ()

void	setDefaultEnvVar (void)

bool	initFromConfXML (std::istream &is)

bool	initFromConfXML (std::string path)

void	initToolSet ()
	Initialize the tool set for algorithm acording to the settings. More...

MHO::ToolSet *	getToolSet ()

MHO::MultiHandOverHeuristic *	createAstarHeuristic (void)
	returns a pointer to newly created Heuristic class for main A* search. More...

void	setDefaultHandOverTesters (std::vector< std::string > &some_defaults)

unsigned int	runScriptFromEnvVar ()
	reads the environment variable $MOVE3D_SCRIPT to get a script/conf file to execute

unsigned int	runScript (std::string filename)
	reads the given script and execute it

unsigned int	run ()
	Run function.

void	resetAgentsConf (void)
	reset the agents to their initial configuration

void	resetManipulablesConf (void)

Configuration	getAgentTargetConf (unsigned int a)

MultiAgentCell *	getAgentTargetCell (unsigned int a)
	returns 0 if have_final_position is set to false for that agent, a ptr to the cell otherwise.

Configuration	getAgentInitConf (unsigned int a)

MultiAgentCell *	getAgentInitCell (unsigned int a)

void	setToComputeHOTraj (bool activate, int solution=-1, int step=0)

void	computeLongestSolutionDistance ()

unsigned int	computeRawSolutions (float cost_majoration)
	compute possible handover successions More...

void	clearSolutions (void)

void	insert_solution_sorted (MHO::MultiHandOverSolution *sol)

void	searchSolutionLWAstar (bool no_lazy)
	searchSolutionLWAstar More...

void	postProcessSolution (MHO::MultiHandOverSolution *sol)

void	searchSolutions (void)
	The core function of this class, it search for an optimal solution. More...

bool	handOverPossible (unsigned int a1, unsigned int a2)

bool	handOverFindConfs (MHO::MultiHandOverSolution *solution, int frontier_index=-1)
	handOverFindConfs More...

bool	handOverFindConfs (FrontierPtr frontier)

MHO::MultiHandOverSolution *	changeHandOverSolution (MHO::MultiHandOverSolution *solution)

double	computeNavigationTrajectories (MHO::MultiHandOverSolution *solution, int action_index=-1)
	computeNavigationTrajectories More...

void	computeHandOverTrajectory (MHO::MultiHandOverSolution *solution, int step=-1)

void	computeSubTaskTrajectory (MHO::SubTask *sub_task)

MHO::MultiHandOverSolution *	optimizeSolution (MHO::MultiHandOverSolution *solution)

MHO::MultiHandOverSolution *	optimizeFrontier (MHO::MultiHandOverSolution *solution, unsigned int action_index, double keep_distance, double min_distance)
	optimize a frontier in a solution to minimize the cost. More...

MultiAgentCell *	findOptimalCellNullDistanceFrontier (MHO::MultiHandOverSolution *solution, unsigned int action_index)
	optimalCellNullDistanceFrontier More...

double	navigationRelatedCost (double dist, unsigned int agent)

double	navigationRelatedCostTime (double dist, unsigned int agent, double &time)

double	computeSolutionCost (MHO::MultiHandOverSolution *solution)

bool	getSolutionDataForMHP (std::vector< MHO::MultiHandOverMhpHO > &hand_overs, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &path, double &cost, double &duration, std::string &main_agent_name)

MHO::MultiHandOverSolution *	getLowCostRandomSolution (double sigma, int *index=0)
	get a randomly selected solution, most probably with low cost More...

MultiAgentGrid *	getPlanGrid (void)

initialisation
This groups contains initialization methods
unsigned int	findAgents ()

void	fetchAgentsConfsInitGoTo ()

unsigned int	findManipulableObjects ()

unsigned int	computeAgentsGraph (void)

void	initGrid (void)

void	computeGrid (void)

void	computeFrontiers (void)

Agent parameters
getters and setters for the agent specific parameters
void	setAgentUseCost (unsigned int i, double cost)

double	getAgentUseCost (unsigned int i)

double	getAgentUseCost (Robot *r)

void	setAgentSpeed (unsigned int i, double speed)

double	getAgentSpeed (unsigned int i)

bool	isHuman (unsigned int i)

double	getTimeHO (unsigned int a1, unsigned int a2)

void	setTimesHO (double rr, double hh, double rh)

3d frontier checking
void	setCheckFrontierBefore (bool b)

bool	getCheckFrontiersBefore (void)

faster but not conservative void	setUseRrtInFrontierCheck (bool b)
	to use an RRT on the object as free flyer to check frontier or not. More...

void	setUseDummySmoothInFrontierCheck (bool b)

double	getShoulderHeight (unsigned int agent)

Eigen::Vector3d	getObjectMainDirection (void)

Configuration	confForBBAxisAlign (Robot *object, Eigen::Vector3d const &main_dir, Eigen::Vector3d const &second_dir)

void	setOrientationObjectForHO (Eigen::Vector3d &dir1)
	set the orientation of the object for a handover More...

void	checkAllMetaFrontiersWithObject (void)

bool	checkMetaFrontiersWithObject (unsigned int a1, unsigned int a2)

bool	checkMetaFrontierWithObject (unsigned int a1, unsigned int a2, MetaFrontierPtr mf)

bool	randomCheckFrontiersWithObject (std::tr1::shared_ptr< FrontierCells > &frontier_found, unsigned int a1, unsigned int a2, unsigned int max_it)
	check frontiers with object, stop when one if found to be OK, or max_it checks are made More...

bool	checkFrontierWithObject (FrontierCells &frontier)
	checkFrontierWithObject More...

Display
methods for displaying information, setting display parameters for move3d GUI...
void	printSolutionsForest (void)

void	printMainTimes (std::vector< double > times)

void	printLWAStats ()

void	setAgentIndexToDrawGrid (unsigned int index)

void	setDrawAgentAccessibilityGrid (bool b, bool boundariesOnly=false)

void	setDrawFrontiers (bool b, unsigned int i=0, unsigned int j=0)

void	drawObjectWhenFrontierChecking (bool b)

void	setDrawAgentsDuringSearch (bool b)

void	setDraw2dPaths (bool b)

void	setDrawAll2dPaths (bool b)

void	setDrawLWA (bool b)

MHO::MultiHandOverSolution::CostData	getSolutionDataStudio (int solution)

double	setSolutionStepRobotsConfs (int solution, int step, MHO::MultiHandOverSolution::CostData &cost_data)
	set the robots to their conf computed in searchSolutions() for the given solution and step More...

void	getConfHandOver (int solution, int step, std::string robot_name, confPtr_t &conf)

void	draw2dPath (void)

void	draw2dPath (std::vector< MultiAgentCell * > const &path, int color_id, bool alt=false)

void	setDrawOptimizeFrontier (bool b)
	sets the option of drawing the frontier optimization process as 2D cells representing the frontier currently being checked More...

void	playTaskStepByStep (MHO::Task *task)

void	playSolutionTrajSync (MHO::Task *task)

bool	playSolutionTaskSyncStep (int solution=-1)

void	computeTaskTrajs (MHO::Task *task)

void	setSubTaskTrajToRobot (MHO::SubTask *subtask)

void	playSolutionTask (int solution)

void	setPlaySolutionTask (int solution)

void	setPlaySolutionTaskTraj (int solution)

void	setPlaySolutionSyncStepByStep (int solution)

bool	isEndPlaySolutionTaskTraj ()

Public Member Functions inherited from HRICS::HumanAwareMotionPlanner
	HumanAwareMotionPlanner (Robot rob, Graph graph)

Distance *	getDistance ()

Visibility *	getVisibility ()

Natural *	getNaturality ()

Natural *	getReachability ()

void	setDistance (Distance *dist)

void	setVisibility (Visibility *visib)

void	setNatural (Natural *nat)

void	setReachability (Natural *nat)

unsigned int	run ()
	Run function.

Public Member Functions inherited from Planner
	Planner ()
	Plain Constructor of the class.

	Planner (Robot rob, Graph graph)
	Constructor of the class. More...

virtual	~Planner ()
	Destructeur de la classe.

virtual bool	trajFound ()
	test de trajectoire More...

Robot *	getActivRobot ()
	retourne le Robot activ More...

void	setRobot (Robot *R)
	place le Robot utilisé pour la planification More...

Graph *	getActivGraph ()
	obtient le Graph actif pour la planification More...

void	setGraph (Graph *G)
	modifie le Graph actif pour la planification More...

bool	setInit (confPtr_t Cs)
	place le Node initial de la planification More...

bool	setGoal (confPtr_t Cg)
	place le Node final de la planification More...

Node *	getInit ()
	obtient le Node intial de la planification More...

Node *	getGoal ()
	obtient le Node final de la planification More...

confPtr_t	getInitConf ()
	Get init configuration.

confPtr_t	getGoalConf ()
	Get goal configuration.

bool	getInitialized ()
	test si le Planner est initialisé pour la planification More...

void	setInitialized (bool b)
	modifie la valeur du Booleen de test d'initialisation More...

virtual unsigned	init ()
	Méthode d'initialisation du Planner.

int	getRunId ()
	Get the run Id.

void	setRunId (int id)
	Set the run Id.

double	getTime ()
	return time in algorithm this function must be called after ChronoTimeOfDayOn() More...

void	chronoStart ()

void	chronoStop ()

void	chronoUpdate ()

virtual API::Trajectory *	extractTrajectory ()

getter/setters
getters and setters for attributes of the class
enum	AStarHeuristic_t { DIST_BEST_AGENT =0, BEST_HANDOVER_SERIES }
	available heuristic types for [L][W]A*

void	setAstarHeuristic (MultiHandOver::AStarHeuristic_t heuristic)

AStarHeuristic_t	getAstarHeuristic (void)

void	setSearchAlgo (MultiHandOver::SearchAlgo_t algo)

MultiHandOver::SearchAlgo_t	getSearchAlgo ()

double	getEpsilonForLWAstar ()

void	setGridCellSize (double size)
	set the cell size for discretisation step in the grid

double	getGridCellSize (void)

void	setEpsilonForLWAstar (double e)

void	setCostsFactors (double dist, double time, double agent_costs, double hri)

void	getCostsFactors (double &dist, double &time, double &agent_costs, double &hri)

void	setSearchSolutionStopCondition (int max_it, int overheat, int time_limit_ms)

void	getSearchSolutionStopCondition (int &max_it, int &overheat, int &time_limit_ms)

FrontierCells::FrontierCheckStatus	getFrontierMaxCheckStatus ()

void	setFrontierMaxCheckStatus (FrontierCells::FrontierCheckStatus fcs)

const std::vector< Robot * > &	getAgents (void)

const std::vector< Robot * > &	getManipulableObjects (void)

MHO::MultiHandOverAgent *	getAgentStruct (unsigned int a)
	the MHO::MultiHandOverAgent corresponding to agent ID a More...

void	setFirstAgent (Robot *f)

Robot *	getFirstAgent (void)

void	setTargetAgent (Robot *f)

Robot *	getTargetAgent (void)

void	setTargetAgent (std::string name)

void	setFirstAgent (std::string name)

void	setTargetAgent (p3d_rob *rob)

void	setFirstAgent (p3d_rob *rob)

void	setRandomizeFirstTarget (bool b)

void	setObject (unsigned int i)

void	setObject (Robot *obj)

Robot *	getObject ()

void	setObject (p3d_rob *obj)

bool	usePostProcess ()

void	usePostProcess (bool b)

void	setFirstAgentNode (void)

void	setTargetAgentNode (void)

Additional Inherited Members
Protected Attributes inherited from HRICS::HumanAwareMotionPlanner
Distance *	m_DistanceSpace
	Distance and Visibility cost spaces.

Visibility *	m_VisibilitySpace

Natural *	m_ReachableSpace

Natural *	m_NaturalSpace

Protected Attributes inherited from Planner
int(*	_stop_func )()

void(*	_draw_func )()

confPtr_t	_q_start

confPtr_t	_q_goal

Node *	_Start
	Le Node initial de la planification.

Node *	_Goal
	Le Node final de la planification.

Robot *	_Robot
	Le Robot pour lequel la recherche va se faire.

Graph *	_Graph
	Le Graph qui va être utilisé

bool	_Init
	Le Planner a été initialisé

bool	m_fail

int	m_runId

double	m_time

Member Function Documentation

bool HRICS::MultiHandOver::checkFrontierWithObject ( FrontierCells & frontier )

checkFrontierWithObject

Parameters

a1
a2
frontier_index	uses the specified object (MultiHandOver::_object) and tries to find a simple path from agent 1 shoulder to agent 2 shoulder, each one of them standing on the frontier The path finding search for a simple linear path.

Todo:

(improve) set step size and distance threshold for object path finding

(improve) object orientation

improve the path search

double HRICS::MultiHandOver::computeNavigationTrajectories	(	MHO::MultiHandOverSolution *	solution,
		int	action_index = `-1`
	)

computeNavigationTrajectories

Parameters

solution

Returns

take agent use costs into account for cost computation (adds costs to nav_dists and cost in solution)

unsigned int HRICS::MultiHandOver::computeRawSolutions ( float cost_majoration )

compute possible handover successions

Parameters

cost_majoration keep a solution only if its cost is lower than the shortest(dijkstra) solution cost*cost_majoration

Returns: the number of solutions found

This is based on the graph built by computeAgentsGraph(). It enumerates all possible paths without circuits from the first to the target agent, and limits solutions to the ones that have a cost lower than the given factor times the shortest path cost.

Note: that here the cost are very imprecise, and a solution found here might not even be feasible.

MHO::MultiHandOverHeuristic * HRICS::MultiHandOver::createAstarHeuristic ( void )

returns a pointer to newly created Heuristic class for main A* search.

based on getAstarHeuristic()

MultiAgentCell * HRICS::MultiHandOver::findOptimalCellNullDistanceFrontier	(	MHO::MultiHandOverSolution *	solution,
		unsigned int	action_index
	)

optimalCellNullDistanceFrontier

Returns: the cell found.

Searches from a given starting cell a cell that optimizes navigation related costs from/to this cell to defined cell, for given agents

used by optimizeFrontier() in some mode

MHO::MultiHandOverAgent* HRICS::MultiHandOver::getAgentStruct ( unsigned int a )

inline

the MHO::MultiHandOverAgent corresponding to agent ID a

Parameters

a	the ID, must be valid (not checked)

MultiHandOverSolution * HRICS::MultiHandOver::getLowCostRandomSolution	(	double	sigma,
		int *	index = `0`
	)

get a randomly selected solution, most probably with low cost

Parameters

sigma

Returns

computes a normal distribution N(0,sigma) to select the solution in the sorted list of solutions (lower first)

bool HRICS::MultiHandOver::handOverFindConfs	(	MHO::MultiHandOverSolution *	solution,
		int	frontier_index = `-1`
	)

handOverFindConfs

Parameters

solution	the solution for which we will compute the transfert configurations
frontier_index	is used to specify the frontier/action in the solution for which we want to compute , default is -1, for all.

Returns: false if it failed at finding a possible configuration

void HRICS::MultiHandOver::initToolSet ( )

Initialize the tool set for algorithm acording to the settings.

See Also: MHO::ToolSet; setUseRrtInFrontierCheck(bool)

MHO::MultiHandOverSolution * HRICS::MultiHandOver::optimizeFrontier	(	MHO::MultiHandOverSolution *	solution,
		unsigned int	action_index,
		double	keep_distance,
		double	min_distance
	)

optimize a frontier in a solution to minimize the cost.

Parameters

solution
action_index
keep_distance
min_distance

Returns

The optimization process can have differents comportements, which are selected through class members.

Todo:: optimization comportement selection

The first one is the slowest in most cases. It's an optimization process where we search for a neighbor frontier, check if we can make a handover there, compute the handover, then the full cost of the solution with this new frontier, computing new trajectories when needed (consequence is a call to A* for each solution we want to check). If the new cost is better, accept this solution and do the same on it, until no better is found.
The second one is a little bit more efficient as it precompute the navigation paths, with a flooding algorithm provided by the grid, avoiding lots of calls to A*.
The third one might be very efficient as it first search for a good solution in term of distances (based on flooding algorithm as previously) with no consideration to 3D environment (which spare lot of computational cost). When an optimal solution is found in the 2D problem, it search from that point using the previous method.
It's also possible to find a cell in the 2d grid that minimizes the cost for any agent to travel from/to that point (like a 0-distance frontier) and then find a real frontier from that point, which will be used as a starting point for the algorithm of the 2nd item
See Also
optimalCellNullDistanceFrontier()

bool HRICS::MultiHandOver::randomCheckFrontiersWithObject	(	std::tr1::shared_ptr< FrontierCells > &	frontier_found,
		unsigned int	a1,
		unsigned int	a2,
		unsigned int	max_it
	)

check frontiers with object, stop when one if found to be OK, or max_it checks are made

Parameters

[out]	frontier_found	pointer to the frontierCell found to be OK, NULL if not found
	a1	first agent index
	a2	second agent index
	max_it	max number of frontiers that will be tested if no one is found to be OK

Returns: true if a frontier is found to be OK

See Also: HRICS::MultiHandOver::checkFrontierWithObject()

void HRICS::MultiHandOver::searchSolutionLWAstar ( bool no_lazy )

searchSolutionLWAstar

Lazy Weighted A* is a search-based algorithm. We search here in the 2D grid for a path through 2d cells and frontiers. reference: Planning Single-arm Manipulations with N-Arm Robots, Cohen B. et al. RSS 2014 http://www.roboticsproceedings.org/rss10/p33.html

void HRICS::MultiHandOver::searchSolutions ( void )

The core function of this class, it search for an optimal solution.

This function search for new solutions, improve them, change them in order to find an optimal solution. Its comportement is driven by several parameters, besides the problem definition. These are time and iteration limit, overheat temperature (when nothing is improved for a long time, we call this overheat), and other details in algorithm selection.

void HRICS::MultiHandOver::setDrawOptimizeFrontier ( bool b )

inline

sets the option of drawing the frontier optimization process as 2D cells representing the frontier currently being checked

Note: if DRAW_OPTIMIZE_FRONTIER macro is not defined, does nothing

void HRICS::MultiHandOver::setOrientationObjectForHO ( Eigen::Vector3d & dir1 )

set the orientation of the object for a handover

Parameters

dir1	the 'direction' of the handover (can be a vector from giver agent shoulder to receiver agent shoulder)

Todo:

implement different ways of choosing the direction, current is having the smallest BB surface orthogonal to the direction, and the biggest surface in a vertical plane.

See Also: confForBBAxisAlign()

double HRICS::MultiHandOver::setSolutionStepRobotsConfs	(	int	solution,
		int	step,
		MHO::MultiHandOverSolution::CostData &	cost_data
	)

set the robots to their conf computed in searchSolutions() for the given solution and step

Parameters

solution	the index of the solution (-1 refers to the best solution)
step	the frontier/action/handover (all the same) index of the solution

Returns: the cost of the solution if parameters refers to inexistent frontier, does nothing

* faster but not conservative* void HRICS::MultiHandOver::setUseRrtInFrontierCheck ( bool b )

inline

to use an RRT on the object as free flyer to check frontier or not.

Parameters

b	use RRT when true.

When not using RRT, uses only a direct path check between origin and target

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

src/HRI_costspace/HRICS_MultiHandOver.h
src/HRI_costspace/HRICS_MultiHandOver.cpp

Public Types

Public Member Functions

getter/setters

Additional Inherited Members

Member Function Documentation