#include <HRICS_Workspace.hpp>

Inheritance diagram for HRICS::Workspace:

Public Member Functions
	Workspace ()
	Constructors & Destructors.

	Workspace (Robot rob, Graph graph)

void	initGrid ()
	Init Associated Objects.

void	deleteGrid ()

void	initAgentGrids (double cellsize)

void	deleteAgentGrids ()

std::vector< AgentGrid * >	getAgentGrids ()

void	initDistance ()

void	initVisibility ()

void	initReachable ()

void	initNatural ()

void	initOtpPlanner ()

Robot *	getHuman ()
	Get Robot and Human.

std::vector< Robot * >	getHumans ()

Robot *	getRobot ()

Eigen::Vector3d	getVisball ()

bool	computeAStarIn3DGrid ()
	Computes A* in Grid. More...

double	pathCost ()

void	draw3dPath ()
	Draws the 3D path as a yellow line.

double	distanceToEntirePath ()
	Distance to 3D path. More...

double	distanceToCellPath ()
	Computes a distance to the Cells in the 3D Path Coarse grain compared to the above distance.

Grid *	getGrid ()
	Getters.

std::vector< Eigen::Vector3d >	get3DPath ()

std::vector< API::ThreeDCell * >	getCellPath ()

int	getIndexObjectDof ()

bool	initHriRRT ()
	Run RRT. More...

void	deactivateOnlyBaseCollision ()
	Choose best transfer point from the natural costspace associated to the human.

void	activateOnlyBaseCollision ()

bool	sampleRobotBase (std::tr1::shared_ptr< Configuration > q_base, const Eigen::Vector3d &WSPoint)
	Test if Jido (Base?) is in collision in this configuration.

bool	transPFromBaseConf (std::tr1::shared_ptr< Configuration > q_base, std::vector< Eigen::Vector3d > points)
	Test if Jido is in collision in this configuration (?)

bool	baseInSight (std::tr1::shared_ptr< Configuration > q_base)

void	setCurrentOTP (const Eigen::Vector3d &p)
	Set the OTP from given point.

Eigen::Vector3d	getCurrentOTP ()
	Return the OTP.

bool	testCol (std::tr1::shared_ptr< Configuration > q_base)
	testColision for the given configuration

void	initPR2RepoConf ()
	set Repo Pose of PR2

void	initPR2GiveConf ()
	set PR2 to Give configuration

void	initPR2AndHumanTest ()
	set PR2 and Human to testing position

void	computePR2GIK (bool move)
	Compute the position of the Robot Arm considering the current configuration.

void	ChangeRobotPos (double value)
	change Robot position to test PR2

void	drawCurrentOTP ()
	Draws the current OTP.

double *	testTransferPointToTrajectory (const Eigen::Vector3d &WSPoint, API::Trajectory &traj, unsigned int &id)
	Test straight lines between target point and the point of the trajectory.

bool	chooseBestTransferPoint (Eigen::Vector3d &transfPoint, bool move, unsigned int threshold)
	Choses a good transfer point. More...

bool	computeBestTransferPoint (Eigen::Vector3d &transfPoint)
	Compute a transfert point from a loaded grid, without taking into acount a possible colision. More...

bool	computeBestFeasableTransferPoint (Eigen::Vector3d &transfPoint)
	Compute a transfert point from a loaded grid, taking into acount a possible colision. More...

bool	ComputeTheObjectTransfertPoint (bool Move, int type, Eigen::Vector3d &transfPoint, int threshold)
	compute an object transfert point. More...

Eigen::Vector3d	computeOTPFromHandPose (bool rightHand)
	compute an object transfert point. More...

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 ()

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

Detailed Description

Workspace Motion Planners.

Member Function Documentation

bool Workspace::chooseBestTransferPoint	(	Eigen::Vector3d &	transfPoint,
		bool	move,
		unsigned int	threshold
	)

Choses a good transfer point.

double* Workspace::testTransferPointToTrajectory( const Vector3d& WSPoint, API::Trajectory& traj , unsigned int& id) { cout << " * Start testing for new OTP ***************" << endl; ChronoOn();

int armId = 0; ArmManipulationData& armData = (*_Robot->getRobotStruct()->armManipulationData)[armId]; ManipulationConfigs manipConf(_Robot->getRobotStruct()); gpGrasp grasp; double confCost = -1;

vector<double> target(6);

target.at(0) = WSPoint(0); target.at(1) = WSPoint(1); target.at(2) = WSPoint(2); target.at(3) = P3D_HUGE; target.at(4) = P3D_HUGE; target.at(5) = P3D_HUGE;

double* q = manipConf.getFreeHoldingConf(NULL, armId, grasp, armData.getCcCntrt()->Tatt, confCost, target, NULL);

1 - Create Agent HRI_AGENTS * agents = hri_create_agents(); HRI_AGENTS * agents = m_Agents;

2 - Select Task HRI_GIK_TASK_TYPE task = GIK_RATREACH; // Left Arm GIK

3 - Select Agent HRI_AGENT* agent = hri_get_one_agent_of_type(agents, HRI_JIDOKUKA);

p3d_vector3 Tcoord; Tcoord[0] = WSPoint[0]; Tcoord[1] = WSPoint[1]; Tcoord[2] = WSPoint[2];

double distance_tolerance = 0.10;

showConfig_2(q); double* q;

if ( true ) { vector< pair<double,target_info> > costOfPoint;

for(unsigned int i=0;i<(unsigned int)traj.getNbPaths();i++) { p3d_set_and_update_this_robot_conf(agent->robotPt, traj.getLocalPathPtrAt(i)->getEnd()->getConfigStruct());

q = p3d_get_robot_config(agent->robotPt);

bool succeed = hri_agent_single_task_manip_move(agent, task, &Tcoord, distance_tolerance, &q);

if(succeed) { shared_ptr<Configuration> q_target(new Configuration(_Robot,q));

p3d_update_virtual_object_config_for_arm_ik_constraint(_Robot->getRobotStruct(), 0, q);

ManipulationUtils::checkConfigForCartesianMode(q,NULL);

    if (p3d_is_collision_free(_Robot->getRobotStruct(),q)) 
    {
      showConfig_2(q);

      LocalPath path(traj.getLocalPathPtrAt(i)->getEnd(),q_target);

      if (path.isValid()) 
      {
        pair<double,target_info> costOfTarget;
        target_info targ;

        targ.id = i;
        targ.q = p3d_copy_config(agent->robotPt,q);

        costOfPoint.push_back( make_pair(path.cost(),targ));
      }
    }
  }
  else {
    p3d_destroy_config(_Robot->getRobotStruct(),q);
  }
}

if (!costOfPoint.empty()) 
{

sort(costOfPoint.begin(),costOfPoint.end()); pair<double,target_info> costOfTargetTmp; costOfTargetTmp.first = P3D_HUGE;

for (unsigned int i=0; i<costOfPoint.size(); i++) { if (costOfPoint[i].first < costOfTargetTmp.first ) { costOfTargetTmp = costOfPoint[i]; } }

id = costOfTargetTmp.second.id; ChronoPrint(""); ChronoOff(); cout << " * End testing for new OTP (Succed) ***********" << endl; return costOfTargetTmp.second.q; } else { p3d_destroy_config(_Robot->getRobotStruct(),q); } } ChronoPrint(""); ChronoOff(); cout << " * End testing for new OTP (Error) ***************" << endl; return NULL; }

bool Workspace::computeAStarIn3DGrid ( )

Computes A* in Grid.

Takes the robot initial config and calls the solve A* to compute the 3D path.

bool Workspace::computeBestFeasableTransferPoint ( Eigen::Vector3d & transfPoint )

Compute a transfert point from a loaded grid, taking into acount a possible colision.

If a trandfert point is found, return true and put the vector into transfPoint else return false

bool Workspace::computeBestTransferPoint ( Eigen::Vector3d & transfPoint )

Compute a transfert point from a loaded grid, without taking into acount a possible colision.

If a trandfert point is found, return true and put the vector into transfPoint else return false

Eigen::Vector3d Workspace::computeOTPFromHandPose ( bool rightHand )

compute an object transfert point.

The pose if the left or right hand of Herakles is used depending on the flag given as input

Parameters

use	the rightHand if true

Returns: a position vector in world coordiate

bool Workspace::ComputeTheObjectTransfertPoint	(	bool	Move,
		int	type,
		Eigen::Vector3d &	transfPoint,
		int	threshold
	)

compute an object transfert point.

the first variable make the human go to the posture or just show it the second one allow to choose different computing fonctions taking into account or not the environment

double Workspace::distanceToEntirePath ( )

Distance to 3D path.

Computes a distance from the robot Current config to the 3D path.

bool Workspace::initHriRRT ( )

Run RRT.

Runs a HRI RRT.

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

src/HRI_costspace/HRICS_Workspace.hpp
src/HRI_costspace/HRICS_Workspace.cpp

Public Member Functions

Additional Inherited Members

Detailed Description

Member Function Documentation