PL_PRM_ClosedChain Class Reference

#include <planners/atace/PL_PRM_Constrained.h>

Inherits PL_PRM_ClosedRGD, PL_PRM_ClosedAPDecomp, PL_PRM_ClosedJacobian, PL_PRM_ClosedLocalJacobian, and PL_PRM.

Inherited by PL_RRT_ClosedChain, and PL_RRT_ClosedChain.

Inheritance diagram for PL_PRM_ClosedChain:

[legend]Collaboration diagram for PL_PRM_ClosedChain:


Public Member Functions
	PL_PRM_ClosedChain ()
	~PL_PRM_ClosedChain ()
virtual bool	Plan ()
virtual void	SetCollisionDetector (CD_BasicStyle *collisionDetector)
bool	GetClosedConfiguration (Configuration &conf)
void	SetUseJacobian (BOOL use)
void	SetUsePlanarConstraint (BOOL use)
void	SetUseGoalPose (BOOL use)
void	SetUseOrientConstraint (BOOL use)
BOOL	GetUseJacobian ()
BOOL	GetUsePlanarConstraint ()
BOOL	GetUseGoalPose ()
BOOL	GetUseOrientConstraint ()
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
Frame	GetToolFrame (const Configuration &config) const
virtual bool	GenerateRandomConfigForPose (Configuration &conf, Frame &endEffPose)
	PL_PRM_ClosedChain ()
	~PL_PRM_ClosedChain ()
virtual bool	Plan ()
virtual bool	MakeItClosed (Configuration &conf)
virtual bool	IsInterfering (const Configuration &c1, const Configuration &c2)
virtual void	SetCollisionDetector (CD_BasicStyle *collisionDetector)
void	SetAlgorithm (ClosedChainAlgorithm alg)
ClosedChainAlgorithm	GetAlgorithm ()
Protected Member Functions
bool	GetSatisfactoryConfiguration (Configuration &conf, Frame &pose) const
virtual Configuration	GenerateRandomConfig () const
virtual Configuration	GenerateRandomConfig (const Configuration &seed, const double &std_dev) const
virtual void	ConnectEdgesFull (const node &n1, const double &radius_squared)
virtual bool	IsInterfering (const Configuration &c1, const Configuration &c2) const
virtual bool	IsInterfering (const Configuration &c1) const
virtual SuccessResultType	TranslatePath ()
bool	Plan_As_Usual ()
void	GetRotAngles (const Matrix4x4 &frame, double &roll, double &pitch, double &yaw) const
bool	MakeItClosed (Configuration &conf) const
bool	IsClosed (const Configuration &conf) const
void	UpdatePlanarConstraint ()
void	UpdateOrientConstraint ()
Protected Attributes
Fragment *	edgeFrag
CD_BasicStyle *	pCollisionDetector
bool	useJacobian
bool	useGoalPose
bool	usePlanarConstraint
bool	useOrientConstraint
Frame	planeDesired
Frame	orientDesired
ClosedChainAlgorithm	m_nAlgorithm
Private Attributes
edge_map< Fragment >	edgePath
LocalPlannerClosed *	planner

Detailed Description

Definition at line 30 of file PL_PRM_Constrained.h.

Constructor & Destructor Documentation

PL_PRM_ClosedChain::PL_PRM_ClosedChain ( )

Definition at line 23 of file PL_PRM_Constrained.cpp.
References edgeFrag, edgePath, PL_GraphBase::G, pCollisionDetector, planner, useGoalPose, useJacobian, useOrientConstraint, and usePlanarConstraint.

PL_PRM_ClosedChain::~PL_PRM_ClosedChain ( )

Definition at line 37 of file PL_PRM_Constrained.cpp.
References edgeFrag, pCollisionDetector, and planner.

PL_PRM_ClosedChain::PL_PRM_ClosedChain ( )

PL_PRM_ClosedChain::~PL_PRM_ClosedChain ( )

Member Function Documentation

void PL_PRM_ClosedChain::ConnectEdgesFull ( const node & n1,

const double & radius_squared

) [protected, virtual]

Reimplemented from PL_PRM.
Definition at line 522 of file PL_PRM_Constrained.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_PRM_ClosedChain::Error ( const Configuration & p1 ) const

Reimplemented from PL_PRM_ClosedBase.
Definition at line 913 of file PL_PRM_Constrained.cpp.
References Error3(), GetRotAngles(), GetToolFrame(), Frame::GetTranslationVector(), Vector4::Magnitude(), orientDesired, useOrientConstraint, and usePlanarConstraint.
Referenced by GetClosedConfiguration(), IsClosed(), and MakeItClosed().
Here is the call graph for this function:

double PL_PRM_ClosedChain::Error1 ( const Configuration & p1,

const Frame & pose

) const

*
Definition at line 799 of file PL_PRM_Constrained.cpp.
References GetToolFrame(), Frame::GetTranslationVector(), Vector4::Magnitude(), Vector4::MagSquared(), PX, PY, PZ, and useOrientConstraint.
Referenced by GenerateRandomConfigForPose().
Here is the call graph for this function:

double PL_PRM_ClosedChain::Error2 ( const Configuration & p1,

const Frame & pose

) const

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

double PL_PRM_ClosedChain::Error3 ( const Configuration & p1,

const Frame & pose

) const

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

double PL_PRM_ClosedChain::Error4 ( const Configuration & p1,

const Frame & pose

) const

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

Configuration PL_PRM_ClosedChain::GenerateRandomConfig ( const Configuration & seed,

const double & std_dev

) const [protected, virtual]

Definition at line 498 of file PL_PRM_Constrained.cpp.
References costFunc, Error2(), Error3(), Error4(), PL_GraphBase::GenerateRandomConfig(), GetSatisfactoryConfiguration(), orientDesired, planeDesired, useOrientConstraint, and usePlanarConstraint.
Here is the call graph for this function:

Configuration PL_PRM_ClosedChain::GenerateRandomConfig ( ) const [protected, virtual]

Definition at line 473 of file PL_PRM_Constrained.cpp.
References costFunc, Error2(), Error3(), Error4(), PL_GraphBase::GenerateRandomConfig(), GetSatisfactoryConfiguration(), orientDesired, planeDesired, useOrientConstraint, and usePlanarConstraint.
Referenced by PL_RRT_ClosedChain::CompareJacobianAndRGD(), PL_RRT_ClosedChain::CompareLocalPlanners(), and PL_RRT_ClosedChain::Plan().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::GenerateRandomConfigForPose ( Configuration & conf,

Frame & endEffPose

) [virtual]

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

ClosedChainAlgorithm PL_PRM_ClosedChain::GetAlgorithm ( ) [inline]

Definition at line 143 of file PL_PRM_ClosedChain.h.

bool PL_PRM_ClosedChain::GetClosedConfiguration ( Configuration & conf )

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

void PL_PRM_ClosedChain::GetRotAngles ( const Matrix4x4 & frame,

double & roll,

double & pitch,

double & yaw

) const [protected]

Reimplemented from PL_PRM_ClosedBase.
Definition at line 942 of file PL_PRM_Constrained.cpp.
References Rad2Deg(), and Sqr().
Referenced by Error(), and Error4().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::GetSatisfactoryConfiguration ( Configuration & conf,

Frame & pose

) const [protected]

Definition at line 397 of file PL_PRM_Constrained.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:

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

Reimplemented from PL_PRM_ClosedBase.
Definition at line 188 of file PL_PRM_Constrained.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_RRT_ClosedChain::AdjustConfiguration(), PL_RRT_ClosedChain::ConnectToGoal(), PL_RRT_ClosedChain::ConnectToGoal2(), Error(), Error1(), Error2(), Error3(), Error4(), Plan(), and UpdatePlanarConstraint().
Here is the call graph for this function:

BOOL PL_PRM_ClosedChain::GetUseGoalPose ( ) [inline]

Definition at line 47 of file PL_PRM_Constrained.h.
References FALSE, TRUE, and useGoalPose.

BOOL PL_PRM_ClosedChain::GetUseJacobian ( ) [inline]

Definition at line 45 of file PL_PRM_Constrained.h.
References FALSE, TRUE, and useJacobian.

BOOL PL_PRM_ClosedChain::GetUseOrientConstraint ( ) [inline]

Definition at line 48 of file PL_PRM_Constrained.h.
References FALSE, TRUE, and useOrientConstraint.

BOOL PL_PRM_ClosedChain::GetUsePlanarConstraint ( ) [inline]

Definition at line 46 of file PL_PRM_Constrained.h.
References FALSE, TRUE, and usePlanarConstraint.

bool PL_PRM_ClosedChain::IsClosed ( const Configuration & conf ) const [protected]

Reimplemented from PL_PRM_ClosedBase.
Definition at line 709 of file PL_PRM_Constrained.cpp.
References DEF_ERR_TOLERANCE, and Error().
Referenced by PL_RRT_ClosedChain::AdjustConfiguration(), and IsInterfering().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::IsInterfering ( const Configuration & c1,

const Configuration & c2

) [virtual]

Reimplemented from PL_PRM_ClosedRGD.
Definition at line 936 of file PL_PRM_ClosedChain.cpp.
References ALG_CLOSEDCHAIN_APD, ALG_CLOSEDCHAIN_JACOBIAN, ALG_CLOSEDCHAIN_LOCAL, ALG_CLOSEDCHAIN_RGD, PL_PRM_ClosedLocalJacobian::IsInterfering(), PL_PRM_ClosedJacobian::IsInterfering(), PL_PRM_ClosedAPDecomp::IsInterfering(), PL_PRM_ClosedRGD::IsInterfering(), and m_nAlgorithm.
Here is the call graph for this function:

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

Definition at line 774 of file PL_PRM_Constrained.cpp.
References IsClosed(), and PL_GraphBase::IsInterfering().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::IsInterfering ( const Configuration & c1,

const Configuration & c2

) const [protected, virtual]

Definition at line 714 of file PL_PRM_Constrained.cpp.
References DEF_DIST_TOLERANCE, PL_GraphBase::Distance(), edgeFrag, PlannerBase::HasTimeLimitExpired(), PL_GraphBase::IsInterfering(), MakeItClosed(), MAX_ITERATION, MAX_RETRY, and MAX_RETRY2.
Referenced by ConnectEdgesFull(), PL_RRT_ClosedChain::ConnectToGoal(), PL_RRT_ClosedChain::ConnectToGoal2(), PL_RRT_ClosedChain::Extend(), and GenerateRandomConfigForPose().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::MakeItClosed ( Configuration & conf ) [virtual]

Reimplemented from PL_PRM_ClosedRGD.
Definition at line 922 of file PL_PRM_ClosedChain.cpp.
References ALG_CLOSEDCHAIN_APD, ALG_CLOSEDCHAIN_JACOBIAN, ALG_CLOSEDCHAIN_LOCAL, ALG_CLOSEDCHAIN_RGD, m_nAlgorithm, PL_PRM_ClosedLocalJacobian::MakeItClosed(), PL_PRM_ClosedJacobian::MakeItClosed(), PL_PRM_ClosedAPDecomp::MakeItClosed(), and PL_PRM_ClosedRGD::MakeItClosed().
Here is the call graph for this function:

bool PL_PRM_ClosedChain::MakeItClosed ( Configuration & conf ) const [protected]

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

virtual bool PL_PRM_ClosedChain::Plan ( ) [virtual]

Reimplemented from PL_PRM_ClosedLocalJacobian.
Reimplemented in PL_RRT_ClosedChain, and PL_RRT_ClosedChain.

bool PL_PRM_ClosedChain::Plan ( ) [virtual]

Reimplemented from PL_PRM_ClosedLocalJacobian.
Reimplemented in PL_RRT_ClosedChain, and PL_RRT_ClosedChain.
Definition at line 62 of file PL_PRM_Constrained.cpp.
References 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(), UpdateOrientConstraint(), UpdatePlanarConstraint(), useGoalPose, useJacobian, useOrientConstraint, and usePlanarConstraint.
Here is the call graph for this function:

bool PL_PRM_ClosedChain::Plan_As_Usual ( ) [protected]

Definition at line 211 of file PL_PRM_Constrained.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_PRM_ClosedChain::SetAlgorithm ( ClosedChainAlgorithm alg ) [inline]

Definition at line 142 of file PL_PRM_ClosedChain.h.
References m_nAlgorithm.

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

Reimplemented from PL_PRM_ClosedLocalJacobian.
Reimplemented in PL_RRT_ClosedChain, and PL_RRT_ClosedChain.

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

Reimplemented from PL_PRM_ClosedLocalJacobian.
Reimplemented in PL_RRT_ClosedChain, and PL_RRT_ClosedChain.
Definition at line 49 of file PL_PRM_Constrained.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_PRM_ClosedChain::SetUseGoalPose ( BOOL use ) [inline]

Definition at line 42 of file PL_PRM_Constrained.h.
References TRUE, and useGoalPose.

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

Definition at line 40 of file PL_PRM_Constrained.h.
References TRUE, and useJacobian.

void PL_PRM_ClosedChain::SetUseOrientConstraint ( BOOL use ) [inline]

Definition at line 43 of file PL_PRM_Constrained.h.
References TRUE, and useOrientConstraint.

void PL_PRM_ClosedChain::SetUsePlanarConstraint ( BOOL use ) [inline]

Definition at line 41 of file PL_PRM_Constrained.h.
References TRUE, and usePlanarConstraint.

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

Reimplemented from PL_PRM_ClosedBase.
Definition at line 977 of file PL_PRM_Constrained.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_PRM_ClosedChain::UpdateOrientConstraint ( ) [protected]

Definition at line 789 of file PL_PRM_Constrained.cpp.
References Matrix4x4::Identity(), orientDesired, and Matrix4x4::Rotate().
Referenced by GetClosedConfiguration(), and Plan().
Here is the call graph for this function:

void PL_PRM_ClosedChain::UpdatePlanarConstraint ( ) [protected]

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