planners/atace/LocalPlanner.cpp

Go to the documentation of this file.

//
//Contents: Class TrajPlanner; base class of local planners used in APACE.
//          Class RRT_TrajPlanner; probabilistic local planner.
//          Class Jacobian_TrajPlanner; Jacobian-based local planner.
// 
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Jun-2004:  Z.Yao     First version implemented
//     Nov-2004:  Z.Yao     Comments added
//  
//Notes:
//   1. The typical procedure to call the local planner is
//      a. Set start/goal: SetStartConfig()/SetGoalPose()/..
//      b. Set the given end-effector trajectory: SetTrajectory(..)
//      c. Set Parameters.
//      d. Begin planning: Plan()
//   2. Different collision detectors are created for different local planner
//      a. RRT_TrajPlanner:      V_Collide;
//      b. Jacobian_TrajPlanner: SWIFT++;
//   3. More Reference:
//      a. RRT_TrajPlanner:      PL_MPEP;
//      b. Jacobian_TrajPlanner: IK_Jacobian;
//   4. Since these local planners work under APACE framework, which is a
//      glboal framework. So, we can make our "local" planners local and easy.
//      For example, when using $RRT_TrajPlanner$, we set the connecting strategy
//      to the simplest one: GREEDY, with small number of retries. 
//

#include "collisiondetectors/CD_Swiftpp.h"
#include "LocalPlanner.h"
#include "Planners/ik_mpep/PL_MPEP.h"
#include "math/Vector4.h"
//#include "math\math2.h"
#include "debug/debug.h"

#define SAMPLE_RESOLUTION       0.4


//========================== Start of Trajectory Local Planner ====================
//
TrajPlanner::TrajPlanner()
{
        //LogMessage("LocalPlanner.log", "====================== Open Planner ========================");
}

TrajPlanner::~TrajPlanner()
{
        //LogMessage("LocalPlanner.log", "====================== Close Planner ========================");
}

bool TrajPlanner::DrawExplicit () const
{
        Sleep(100);
        return true;
}

//========================== Start of RRT Local Planner ====================
RRT_TrajPlanner::RRT_TrajPlanner()
{
        planner = new( PL_MPEP );
}

RRT_TrajPlanner::~RRT_TrajPlanner()
{
        delete planner;
}

//Inherited form ancestor
void RRT_TrajPlanner::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        planner->SetCollisionDetector(collisionDetector);
}

void RRT_TrajPlanner::SetStartConfig( const Configuration& config )
{
        planner->SetStartConfig(config);
}

void RRT_TrajPlanner::SetGoalConfig( const Configuration& config)
{
        planner->SetGoalConfig(config);
}

void RRT_TrajPlanner::SetTrajectory(std::vector<Frame> &poses)
{
        m_nNumPose = poses.size();
        planner->SetTrajectory(poses);
}

bool RRT_TrajPlanner::Plan ()
{
        UINT    passiveNum = 3;
        double  tolerance = 0.1;
        int             algorithm = ALG_GREEDY_PLANNER;//ALG_SMG_GREEDY_PLANNER; //ALG_GREEDY_PLANNER;//ALG_RRT_CONNECT_LIKE;
        bool    useStartConf = true;
        bool    useGoalConf = m_bUseGoalConfig;
        UINT    randomRetry = 50;
        UINT    extendNum = 50;
        UINT    iterationNum = 1;
        UINT    nodeInSMG = 5;//10;
        UINT    exploreNum = 10;//20;

        //if (m_bUseGoalConfig)
        //{
        //      algorithm = ALG_SMG_CONNECT_PLANNER; 
        //      extendNum = m_nNumPose * 10;
        //      planner->SetSMGNodeNum(nodeInSMG);
        //}

        if (!m_bUseOrientation)
                passiveNum = 2;

        planner->SetPassiveNum(passiveNum);
        planner->SetAlgorithm(algorithm);
        planner->SetTolerance(tolerance);
        planner->SetUsingStartConfig(useStartConf);
        planner->SetUsingGoalConfig(useGoalConf);
        planner->SetRandomRetry(randomRetry);
        planner->SetExtendNum(extendNum);
        planner->SetIterationNum(iterationNum);
        planner->SetExploreNum(exploreNum);

        if (planner->Plan())
        {
                path = *((PA_Points *)planner->GetPath());
                return true;
        }
        else
        {
                return false;
        }
}

//==================== Start of Jacobian-base Local Planner ==============
//
Jacobian_TrajPlanner::Jacobian_TrajPlanner()
{
        planner = new( IK_Jacobian );
        pCollisionDetector = NULL;
}

Jacobian_TrajPlanner::~Jacobian_TrajPlanner()
{
        if(pCollisionDetector)
        {
                delete pCollisionDetector;
        }
        delete planner;
}

void Jacobian_TrajPlanner::SetStartConfig( const Configuration& config )
{
        planner->SetStartConfig(config);
}

void Jacobian_TrajPlanner::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        if (pCollisionDetector != NULL)
                delete pCollisionDetector;

        pCollisionDetector = new CD_Swiftpp( *(collisionDetector->GetUniverse()) );
        planner->SetCollisionDetector(pCollisionDetector);
}

void Jacobian_TrajPlanner::SetTrajectory(std::vector<Frame> &poses)
{
        planner->SetTrajectory(poses);
}

bool Jacobian_TrajPlanner::Plan ()
{
        double dObstacleTol = 0.01;
        double dHomogain = 0.5;
        double dPathTol = 0.5;
        int nObstacleNum = 2;

    planner->SetObstacleTolerance(dObstacleTol);
    planner->SetNumObstaclePt(nObstacleNum);
        planner->SetHomogeneousGain(dHomogain);
        planner->SetOrientation(m_bUseOrientation);
        planner->SetPosition(true);
        planner->SetPathTolerence(dPathTol);

        if (planner->Plan())
        {
                path = *((PA_Points *)planner->GetPath());
                return true;
        }
        else
        {
                return false;
        }
}

---