PL_RGD_PRM Class Reference

#include <planners/atace/PL_RGD_Constraint.h>

Inherits PL_PRM.

Inheritance diagram for PL_RGD_PRM:

[legend]Collaboration diagram for PL_RGD_PRM:


Public Member Functions
	PL_RGD_PRM ()
	~PL_RGD_PRM ()
virtual bool	Plan ()
virtual void	SetCollisionDetector (CD_BasicStyle *collisionDetector)
bool	GetSatisfiedConfiguration (Configuration &conf)
void	SetConstraintType (int type)
void	SetUseJacobian (BOOL use)
void	SetUseGoalPose (BOOL use)
int	GetConstraintType ()
BOOL	GetUseJacobian ()
BOOL	GetUseGoalPose ()
double	Error (const Configuration &p1) const
double	Error1 (const Configuration &p1, const Frame &pose) const
double	Error2 (const Configuration &p1, const Frame &pose) const
double	Error3 (const Configuration &p1, const Frame &pose) const
double	Error4 (const Configuration &p1, const Frame &pose) const
double	Error5 (const Configuration &p1, const Frame &pose) const
double	Error6 (const Configuration &p1, const Frame &pose) const
Frame	GetToolFrame (const Configuration &config) const
virtual bool	GenerateRandomConfigForPose (Configuration &conf, Frame &endEffPose)
Protected Member Functions
bool	GetSatisfactoryConfiguration (Configuration &conf, Frame &pose)
virtual Configuration	GenerateRandomConfig ()
virtual Configuration	GenerateRandomConfig (const Configuration &seed, const double &std_dev)
virtual void	ConnectEdgesFull (const node &n1, const double &radius_squared)
virtual bool	IsInterfering (const Configuration &c1, const Configuration &c2)
virtual bool	IsInterfering (const Configuration &c1)
virtual SuccessResultType	TranslatePath ()
bool	Plan_As_Usual ()
void	GetRotAngles (const Matrix4x4 &frame, double &roll, double &pitch, double &yaw) const
bool	MakeItSatisfied (Configuration &conf)
bool	IsSatisfied (const Configuration &conf) const
void	UpdatePlanarConstraint ()
void	UpdateOrientConstraint ()
void	UpdateShpereConstraint ()
Protected Attributes
Fragment *	edgeFrag
CD_BasicStyle *	pCollisionDetector
int	typeOfConstraint
bool	useJacobian
bool	useGoalPose
Frame	planeDesired
Frame	orientDesired
Private Attributes
edge_map< Fragment >	edgePath
LocalPlannerClosed *	planner

Detailed Description

Definition at line 28 of file PL_RGD_Constraint.h.

Constructor & Destructor Documentation

PL_RGD_PRM::PL_RGD_PRM ( )

Definition at line 32 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_COLSURE, edgeFrag, edgePath, PL_GraphBase::G, pCollisionDetector, planner, typeOfConstraint, useGoalPose, and useJacobian.

PL_RGD_PRM::~PL_RGD_PRM ( )

Definition at line 45 of file PL_RGD_Constraint.cpp.
References edgeFrag, pCollisionDetector, and planner.

Member Function Documentation

void PL_RGD_PRM::ConnectEdgesFull ( const node & n1,

const double & radius_squared

) [protected, virtual]

Reimplemented from PL_PRM.
Definition at line 552 of file PL_RGD_Constraint.cpp.
References PL_PRM::baseConnectID, PL_PRM::config_seeds, PL_PRM::connectIDp, PL_GraphBase::Distance(), PL_PRM::edgeChecked, edgeFrag, edgePath, PL_GraphBase::G, PL_GraphBase::GetMidPoint(), IsInterfering(), PL_GraphBase::NIL_ID, PL_PRM::NodeInConnectionList(), TRUE, and PL_PRM::useMidPts.
Here is the call graph for this function:

double PL_RGD_PRM::Error ( const Configuration & p1 ) const

Definition at line 1005 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_ORIENTATION, CONSTRAINT_OTHER, CONSTRAINT_PLANAR, CONSTRAINT_SPHERE, Error2(), Error3(), Error4(), Error5(), Error6(), GetToolFrame(), orientDesired, planeDesired, and typeOfConstraint.
Referenced by GetSatisfiedConfiguration(), IsSatisfied(), and MakeItSatisfied().
Here is the call graph for this function:

double PL_RGD_PRM::Error1 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 848 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_ORIENTATION, CONSTRAINT_SPHERE, GetToolFrame(), Frame::GetTranslationVector(), Vector4::Magnitude(), Vector4::Normalize(), PZ, RGD_SPHERE_CX, RGD_SPHERE_CY, RGD_SPHERE_CZ, and typeOfConstraint.
Referenced by GenerateRandomConfigForPose().
Here is the call graph for this function:

double PL_RGD_PRM::Error2 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 917 of file PL_RGD_Constraint.cpp.
References GetToolFrame(), Frame::GetTranslationVector(), and Vector4::Magnitude().
Referenced by Error(), and GenerateRandomConfig().
Here is the call graph for this function:

double PL_RGD_PRM::Error3 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 928 of file PL_RGD_Constraint.cpp.
References GetToolFrame().
Referenced by Error(), and GenerateRandomConfig().
Here is the call graph for this function:

double PL_RGD_PRM::Error4 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 937 of file PL_RGD_Constraint.cpp.
References Vector4::Dot(), GetToolFrame(), and orientDesired.
Referenced by Error(), Error6(), and GenerateRandomConfig().
Here is the call graph for this function:

double PL_RGD_PRM::Error5 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 969 of file PL_RGD_Constraint.cpp.
References GetToolFrame(), Frame::GetTranslationVector(), Vector4::Magnitude(), Vector4::Normalize(), RGD_SPHERE_CX, RGD_SPHERE_CY, RGD_SPHERE_CZ, and RGD_SPHERE_RADIUS.
Referenced by Error(), and GenerateRandomConfig().
Here is the call graph for this function:

double PL_RGD_PRM::Error6 ( const Configuration & p1,

const Frame & pose

) const

Definition at line 999 of file PL_RGD_Constraint.cpp.
References Error4().
Referenced by Error(), and GenerateRandomConfig().
Here is the call graph for this function:

Configuration PL_RGD_PRM::GenerateRandomConfig ( const Configuration & seed,

const double & std_dev

) [protected, virtual]

Reimplemented from PL_GraphBase.
Definition at line 524 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_ORIENTATION, CONSTRAINT_PLANAR, CONSTRAINT_SPHERE, costFunc, Error2(), Error3(), Error4(), Error5(), PL_GraphBase::GenerateRandomConfig(), GetSatisfactoryConfiguration(), orientDesired, planeDesired, and typeOfConstraint.
Here is the call graph for this function:

Configuration PL_RGD_PRM::GenerateRandomConfig ( ) [protected, virtual]

Reimplemented from PL_GraphBase.
Definition at line 489 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_ORIENTATION, CONSTRAINT_OTHER, CONSTRAINT_PLANAR, CONSTRAINT_SPHERE, costFunc, Error2(), Error3(), Error4(), Error5(), Error6(), PL_GraphBase::GenerateRandomConfig(), GetSatisfactoryConfiguration(), orientDesired, planeDesired, and typeOfConstraint.
Referenced by PL_RGD_RRT::Plan().
Here is the call graph for this function:

bool PL_RGD_PRM::GenerateRandomConfigForPose ( Configuration & conf,

Frame & endEffPose

) [virtual]

Definition at line 476 of file PL_RGD_Constraint.cpp.
References costFunc, Error1(), PL_GraphBase::GenerateRandomConfig(), GetSatisfactoryConfiguration(), and IsInterfering().
Referenced by PL_RGD_RRT::ConnectToGoal(), and Plan().
Here is the call graph for this function:

int PL_RGD_PRM::GetConstraintType ( ) [inline]

Definition at line 42 of file PL_RGD_Constraint.h.
References typeOfConstraint.

void PL_RGD_PRM::GetRotAngles ( const Matrix4x4 & frame,

double & roll,

double & pitch,

double & yaw

) const [protected]

Definition at line 1031 of file PL_RGD_Constraint.cpp.
References Rad2Deg(), and Sqr().
Here is the call graph for this function:

bool PL_RGD_PRM::GetSatisfactoryConfiguration ( Configuration & conf,

Frame & pose

) [protected]

Definition at line 409 of file PL_RGD_Constraint.cpp.
References costFunc, DEF_ERR_TOLERANCE, DEF_NEIGHBOR, PL_GraphBase::GenerateRandomConfig(), PlannerBase::HasTimeLimitExpired(), MAX_ITERATION, and MAX_RETRY.
Referenced by GenerateRandomConfig(), and GenerateRandomConfigForPose().
Here is the call graph for this function:

bool PL_RGD_PRM::GetSatisfiedConfiguration ( Configuration & conf )

Definition at line 444 of file PL_RGD_Constraint.cpp.
References DEF_ERR_TOLERANCE, DEF_NEIGHBOR, Error(), PL_GraphBase::GenerateRandomConfig(), MAX_ITERATION, MAX_RETRY, UpdateOrientConstraint(), UpdatePlanarConstraint(), and UpdateShpereConstraint().
Referenced by PL_RGD_RRT::Extend().
Here is the call graph for this function:

Frame PL_RGD_PRM::GetToolFrame ( const Configuration & config ) const

Definition at line 200 of file PL_RGD_Constraint.cpp.
References PL_HasCollisionDetector::collisionDetector, CD_BasicStyle::DOF(), CD_BasicStyle::GetFrameManager(), FrameManager::GetFrameRef(), CD_BasicStyle::GetRobot(), R_OpenChain::GetToolFrame(), FrameManager::GetTransformRelative(), FrameManager::GetWorldFrame(), CD_BasicStyle::JointFrameNum(), and CD_BasicStyle::SetConfiguration().
Referenced by PL_RGD_RRT::ConnectToGoal(), Error(), Error1(), Error2(), Error3(), Error4(), Error5(), Plan(), and UpdatePlanarConstraint().
Here is the call graph for this function:

BOOL PL_RGD_PRM::GetUseGoalPose ( ) [inline]

Definition at line 44 of file PL_RGD_Constraint.h.
References FALSE, TRUE, and useGoalPose.

BOOL PL_RGD_PRM::GetUseJacobian ( ) [inline]

Definition at line 43 of file PL_RGD_Constraint.h.
References FALSE, TRUE, and useJacobian.

bool PL_RGD_PRM::IsInterfering ( const Configuration & c1 ) [protected, virtual]

Reimplemented from PL_GraphBase.
Definition at line 804 of file PL_RGD_Constraint.cpp.
References PL_GraphBase::IsInterfering(), and IsSatisfied().
Here is the call graph for this function:

bool PL_RGD_PRM::IsInterfering ( const Configuration & c1,

const Configuration & c2

) [protected, virtual]

Reimplemented from PL_GraphBase.
Definition at line 744 of file PL_RGD_Constraint.cpp.
References DEF_DIST_TOLERANCE, PL_GraphBase::Distance(), edgeFrag, PlannerBase::HasTimeLimitExpired(), PL_GraphBase::IsInterfering(), MakeItSatisfied(), MAX_ITERATION, MAX_RETRY, and MAX_RETRY2.
Referenced by ConnectEdgesFull(), PL_RGD_RRT::ConnectToGoal(), PL_RGD_RRT::Extend(), and GenerateRandomConfigForPose().
Here is the call graph for this function:

bool PL_RGD_PRM::IsSatisfied ( const Configuration & conf ) const [protected]

Definition at line 739 of file PL_RGD_Constraint.cpp.
References DEF_ERR_TOLERANCE, and Error().
Referenced by IsInterfering().
Here is the call graph for this function:

bool PL_RGD_PRM::MakeItSatisfied ( Configuration & conf ) [protected]

Definition at line 707 of file PL_RGD_Constraint.cpp.
References DEF_ERR_TOLERANCE, DEF_NEIGHBOR, Error(), PL_GraphBase::GenerateRandomConfig(), PlannerBase::HasTimeLimitExpired(), MAX_ITERATION, and MAX_RETRY.
Referenced by IsInterfering().
Here is the call graph for this function:

bool PL_RGD_PRM::Plan ( ) [virtual]

Reimplemented from PL_PRM.
Reimplemented in PL_RGD_RRT, and PL_RGD_Constraint.
Definition at line 70 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_ORIENTATION, CONSTRAINT_PLANAR, CONSTRAINT_SPHERE, GenerateRandomConfigForPose(), PlannerBase::GetGoalConfig(), PL_Boolean_Output::GetPath(), PlannerBase::GetStartConfig(), PlannerBase::GetTimeElapsedInSeconds(), PlannerBase::GetTimeLimitInSeconds(), GetToolFrame(), PlannerBase::HasTimeLimitExpired(), PL_Boolean_Output::path, LocalPlannerClosed::Plan(), Plan_As_Usual(), planner, PL_PRM::SetGoalConfig(), PlannerBase::SetGoalConfig(), IK_Jacobian::SetHomogeneousGain(), IK_Jacobian::SetNumObstaclePt(), IK_Jacobian::SetObstacleTolerance(), IK_Jacobian::SetOrientation(), IK_Jacobian::SetPathTolerence(), IK_Jacobian::SetPosition(), PlannerBase::SetStartConfig(), PlannerBase::SetTimeLimitInSeconds(), PlannerBase::StartTimer(), typeOfConstraint, UpdateOrientConstraint(), UpdatePlanarConstraint(), UpdateShpereConstraint(), useGoalPose, and useJacobian.
Referenced by PL_RGD_Constraint::Plan().
Here is the call graph for this function:

bool PL_RGD_PRM::Plan_As_Usual ( ) [protected]

Definition at line 223 of file PL_RGD_Constraint.cpp.
References PA_Points::AppendPoint(), PL_PRM::BuildInitRoadMap(), PA_Points::Clear(), PL_PRM::diagonal_squared, PL_PRM::EnhanceRoadMap(), FALSE, PL_PRM::FindPath(), PL_GraphBase::GetCspaceRange(), PlannerBase::GetStartConfig(), PL_GraphBase::goalNode, PL_GraphBase::graphPath, PlannerBase::guid, PL_GraphBase::IsInterfering(), PL_PRM::lastPlanningState, Semaphore::Lock(), PASS, PL_Boolean_Output::path, PRM_BUILD_INIT_ROADMAP, PRM_DONE, PRM_ENHANCE_ROADMAP, PRM_FAILURE, PRM_FIND_PATH, PRM_START, PRM_TRANSLATE_PATH, PRM_VERIFY_PATH, PL_GraphBase::startNode, TIMER_EXPIRED, TranslatePath(), Semaphore::Unlock(), and PL_PRM::VerifyPath().
Referenced by Plan().
Here is the call graph for this function:

void PL_RGD_PRM::SetCollisionDetector ( CD_BasicStyle * collisionDetector ) [virtual]

Reimplemented from PL_PRM.
Definition at line 57 of file PL_RGD_Constraint.cpp.
References PL_HasCollisionDetector::collisionDetector, CD_BasicStyle::DeactivateFrames(), CD_BasicStyle::DOF(), CollisionDetectorBase::GetUniverse(), pCollisionDetector, planner, IK_Jacobian::SetCollisionDetector(), and PL_PRM::SetCollisionDetector().
Referenced by ServerBase::RefreshPlanner().
Here is the call graph for this function:

void PL_RGD_PRM::SetConstraintType ( int type ) [inline]

Definition at line 38 of file PL_RGD_Constraint.h.
References typeOfConstraint.

void PL_RGD_PRM::SetUseGoalPose ( BOOL use ) [inline]

Definition at line 40 of file PL_RGD_Constraint.h.
References TRUE, and useGoalPose.

void PL_RGD_PRM::SetUseJacobian ( BOOL use ) [inline]

Definition at line 39 of file PL_RGD_Constraint.h.
References TRUE, and useJacobian.

SuccessResultType PL_RGD_PRM::TranslatePath ( ) [protected, virtual]

Reimplemented from PL_GraphBase.
Definition at line 1066 of file PL_RGD_Constraint.cpp.
References PA_Points::AppendPoint(), PA_Points::Clear(), FAIL, PlannerBase::GetStartConfig(), PL_GraphBase::graphPath, and PL_Boolean_Output::path.
Referenced by Plan_As_Usual().
Here is the call graph for this function:

void PL_RGD_PRM::UpdateOrientConstraint ( ) [protected]

Definition at line 819 of file PL_RGD_Constraint.cpp.
References CONSTRAINT_OTHER, Matrix4x4::Identity(), orientDesired, Matrix4x4::Rotate2(), and typeOfConstraint.
Referenced by GetSatisfiedConfiguration(), and Plan().
Here is the call graph for this function:

void PL_RGD_PRM::UpdatePlanarConstraint ( ) [protected]

Definition at line 814 of file PL_RGD_Constraint.cpp.
References PlannerBase::GetStartConfig(), GetToolFrame(), and planeDesired.
Referenced by GetSatisfiedConfiguration(), and Plan().
Here is the call graph for this function:

void PL_RGD_PRM::UpdateShpereConstraint ( ) [protected]

Definition at line 842 of file PL_RGD_Constraint.cpp.
Referenced by GetSatisfiedConfiguration(), and Plan().