planners/atace/PL_PRM_Constrained.cpp

Go to the documentation of this file.

#include "math/vector4.h"
#include "math/matrixmxn.h"
#include "synchronization/semaphore.h"
#include "robots/R_OpenChain.h"
#include "CollisionDetectors/CD_Swiftpp.h"
#include <assert.h>
#include "planners/ik_mpep/IK_Jacobian.h"
#include "PL_PRM_ClosedChain.h"
#include <gl/gl.h>
#include <opengl/gl_sphere.h>
#include <opengl/gl_group.h>

#define MAX_ITERATION        30
#define MAX_RETRY            30
#define MAX_RETRY2           30
#define DEF_NEIGHBOR         (2)
#define DEF_DIST_TOLERANCE   (3)
#define DEF_ERR_TOLERANCE    (1e-3)

double (PL_PRM_ClosedChain::*costFunc)(const Configuration &p1, const Frame &pose) const = &PL_PRM_ClosedChain::Error1;

PL_PRM_ClosedChain::PL_PRM_ClosedChain()
{
        edgePath.init(G);
        edgeFrag = new Fragment;
        //int dof = collisionDetector->DOF();
        //distTolerance = DEF_NEIGHBOR * dof;
        planner = new LocalPlannerClosed;
        pCollisionDetector = NULL;
        useJacobian = false;
        usePlanarConstraint = false;
        useOrientConstraint = false;
        useGoalPose = false;
}       

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

        edgeFrag->clear();
        delete edgeFrag;
}

void PL_PRM_ClosedChain::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        // Call Parent's Collision Detector and assign the collision detector
        PL_PRM::SetCollisionDetector( collisionDetector );

        if (pCollisionDetector != NULL)
                delete pCollisionDetector;

        pCollisionDetector = new CD_Swiftpp( *(collisionDetector->GetUniverse()) );
        planner->SetCollisionDetector(pCollisionDetector);
        pCollisionDetector->DeactivateFrames(1, pCollisionDetector->DOF());
}

bool PL_PRM_ClosedChain::Plan ()
{
        srand( (unsigned)time( NULL ) );

        StartTimer();
        if (usePlanarConstraint)
        {
                UpdatePlanarConstraint();
        }
        else if (useOrientConstraint)
        {
                UpdateOrientConstraint();
        }

        if (useJacobian)
        {
                planner->SetTimeLimitInSeconds(100);
                Configuration q1 = GetStartConfig();
                Configuration q2 = GetGoalConfig();
                planner->SetStartConfig(q1);
                planner->SetGoalConfig(q2);

                double dObstacleTol = 0.01;
                double dHomogain = 2.0;
                double dPathTol = 0.5;
                int nObstacleNum = 2;

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

                //if (planner->Plan())
                bool result = planner->Plan();

                PA_Points *plannerPath = (PA_Points *)planner->GetPath();
                path = *plannerPath;

                //To take a look at where planner fail to go on,
                //the path will be returned no matter the planing is success or not.
                return result;
        }
        else if (useGoalPose)
        {
                Configuration conf;
                Frame goalPose = GetToolFrame( GetGoalConfig() );
                bool Result = false;
                double oldTimeLimit = GetTimeLimitInSeconds();
                double timeLimit = 100;
                
                while (Result == false)
                {
                        Result = GenerateRandomConfigForPose(conf, goalPose);
                        if (Result)
                        {
                                timeLimit = GetTimeElapsedInSeconds() + 20;
                                oldTimeLimit = GetTimeLimitInSeconds();
                                if (timeLimit > oldTimeLimit)
                                        timeLimit = oldTimeLimit;
                                SetTimeLimitInSeconds( timeLimit );
                                PL_PRM::SetGoalConfig(conf);
                                Result = Plan_As_Usual();
                                if (GetTimeLimitInSeconds() != timeLimit)
                                        break;
                                else
                                        SetTimeLimitInSeconds( oldTimeLimit );
                        }
                        if (Result)
                                return true;
                        if ( HasTimeLimitExpired() )
                        {
                                return false;
                        }
                }
                return false;
                //return Plan_As_Usual();
                //return PL_PRM::Plan();
        }
        else
        {
                return Plan_As_Usual();
        }

/*
        Configuration q_last = q1;
        Configuration q_end = q2;
        path.AppendPoint(q1);
        int MAX_I = MAX_ITERATION * MAX_RETRY * MAX_RETRY2;
        int MAX_J = MAX_RETRY;
        int MAX_K = MAX_RETRY2;
        double distToLast = sqrt(Distance(q_last, q2));
        while ((i<MAX_I) && (j<MAX_J) && (k<MAX_K) && (distToLast>DEF_DIST_TOLERANCE))
        {
                i++;  j++;
                Configuration q_mid = PL_PRM::GenerateRandomConfig(q_last, DEF_NEIGHBOR*0.2);
                if (MakeItClosed(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (Distance(q_mid, q_end) < Distance(q_last, q_end))
                        {
                                k = 0;
                                path.AppendPoint(q_mid);
                                q_last = q_mid;
                                distToLast = sqrt(Distance(q_last, q2));
                        }
                }
        }

        if (sqrt(Distance(q_last, q_end))<DEF_DIST_TOLERANCE)
        {
                path.AppendPoint(q2);
                return true;
        }
        else
        {
                return false;
        }
*/
//      return PL_PRM::Plan();
}

//****************************************************************************
//             Function to Get Tool Frame: End-effector Frame.
//****************************************************************************
Frame PL_PRM_ClosedChain::GetToolFrame( const Configuration& config ) const
{
        Frame toolFrame = Matrix4x4();
        if ( collisionDetector != NULL )
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                collisionDetector->SetConfiguration( config );
                R_OpenChain * robot = (R_OpenChain *)collisionDetector->GetRobot(0);
                int toolFrameID = robot->GetToolFrame(0);
                if (toolFrameID == -1)
                {
                        toolFrameID = collisionDetector->JointFrameNum(collisionDetector->DOF()-1);
                        toolFrame = frameManager->GetWorldFrame( toolFrameID );
                }
                else
                {
                        toolFrame = *(frameManager->GetFrameRef(toolFrameID));
                        toolFrame = frameManager->GetTransformRelative(collisionDetector->DOF(), 0) * toolFrame;
                }
        }
        return toolFrame;
}

bool PL_PRM_ClosedChain::Plan_As_Usual ()
{
        PRM_StateMachineType currentState = lastPlanningState;

        // Nodes may have been added since last plan.  Allow planner to check roadmap again
        // IMPROVE:  Is this still needed?
        if ( lastPlanningState == PRM_FAILURE )
        {
                currentState = PRM_START;
        }

        // Start the timer
        //StartTimer();

        // Establish the semaphore
        Semaphore semaphore( guid );
        if( this->m_UseSemaphores == true )
        {
                semaphore.Lock();
        }
        

        // Some quick checks to determine if we really need to plan
        if ( (currentState != PRM_DONE ) && (currentState != PRM_FAILURE) ) 
        {
                if ( startNode == goalNode )
                {
                        // start and goal are the same.  Do not plan.
                        currentState = PRM_DONE;

                        // Assign a small path.
                        path.Clear();
                        path.AppendPoint( GetStartConfig() );
                }
                else if ( (PL_PRM::IsInterfering(startNode)) || (PL_PRM::IsInterfering(goalNode)) )
                {
                        // either the start or goal are already interfering.  Plan will
                        // automatically fail.
                        currentState = PRM_FAILURE;
                }
        }
        if( this->m_UseSemaphores == true )
        {
                semaphore.Unlock();
        }

        // Ensure the diagonal length is correct
        diagonal_squared = GetCspaceRange();
        
        // State Machine
        while (( currentState != PRM_DONE ) && ( currentState != PRM_FAILURE )) 
                // NOTE:  The PRM_FAILURE state should eventually be removed!
        {
                SuccessResultType success;

                if( this->m_UseSemaphores == true )
                {
                        semaphore.Lock();
                }
                // Determine the State function to call for this iteration
                switch ( currentState )
                {
                        case PRM_BUILD_INIT_ROADMAP :
                                success = BuildInitRoadMap();
                                break;
                        case PRM_FIND_PATH :
                                success = FindPath();
                                break;
                        case PRM_VERIFY_PATH :
                                success = VerifyPath();
                                break;
                        case PRM_TRANSLATE_PATH :
                                success = TranslatePath();
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                success = EnhanceRoadMap();
                                break;
                        default :
                                // No state functions are associated with this state.
                                // Do nothing except prepare for next iteration.
                                success = PASS;
                }  // end SWITCH


                // Check if timer has expired and one of the state functions had returned early.
                if ( success == TIMER_EXPIRED )
                {
                        // Escape the state machine.
                        break;
                }

                // Determine the State Function to call for the next iteration
                switch ( currentState )
                {
                        case PRM_START :
                                // Start by building the initial roadmap.
                                currentState = PRM_BUILD_INIT_ROADMAP;  
                                break;
                        case PRM_BUILD_INIT_ROADMAP :
                                // Search current road map for a short path.
                                currentState = PRM_FIND_PATH;                   
                                break;
                        case PRM_FIND_PATH :
                                if ( success == PASS )
                                {
                                        // A path has been found, verify it has no collision.
                                        currentState = PRM_VERIFY_PATH;
                                }
                                else
                                {
                                        // No path was found.  Enhance some of the trouble areas.
                                        currentState = PRM_ENHANCE_ROADMAP;
                                        //currentState = PRM_FAILURE;           // NOTE:  Once EnhanceRoadmap is working,
                                                                                                        //                the PRM_FAILURE state should be removed.
                                }
                                break;
                        case PRM_VERIFY_PATH :
                                if ( success == PASS )
                                {
                                        // Path represented by valid nodes is successful. Translate it so the
                                        // main program can implement it.
                                        currentState = PRM_TRANSLATE_PATH;
                                }
                                else
                                {
                                        // Path was not collision free.  Need to find a new path now that the troublesome
                                        // nodes/edges have been removed.
                                        currentState = PRM_FIND_PATH;
                                }
                                break;
                        case PRM_TRANSLATE_PATH :
                                // We are done.  Escape state machine and report success.
                                currentState = PRM_DONE;
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                // Try finding a path again with the new nodes and edges added.
                                currentState = PRM_FIND_PATH;
                                break;
                        default :
                                // an unexpected or invalid state has occurred.  Crash the program!
                                ASSERT( FALSE );
                } // end SWITCH.

                // Release semaphore temporary.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }
        }  // END State Machine


        // save the current state of the machine for the next time the planner is called.
        lastPlanningState = currentState;

        // Report the success of the planner.
        if ( currentState == PRM_DONE )
        {
                // planner has completed.  A path has been found and saved.
                        
                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                return true;
        }
        else
        {
                // planner has not completed and has terminated early
                graphPath.clear();

                // Assign a small path.
                path.Clear();
                path.AppendPoint( GetStartConfig() );

                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                return false;
        }
}  // END Plan

bool PL_PRM_ClosedChain::GetSatisfactoryConfiguration(Configuration &conf, Frame &pose) const
{
        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY;
        double err_q = (this->*costFunc)(q, pose);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if ((this->*costFunc)(q_next, pose) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = (this->*costFunc)(q, pose);
                }
                if ( HasTimeLimitExpired() )
                {
                        return false;
                }
        }

        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_PRM_ClosedChain::GetClosedConfiguration(Configuration &conf)
{
        UpdatePlanarConstraint();
        UpdateOrientConstraint();

        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY;
        double err_q = Error(q);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if (Error(q_next) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = Error(q);
                }
        }

        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_PRM_ClosedChain::GenerateRandomConfigForPose (Configuration &conf, Frame &endEffPose)
{
        conf = PL_PRM::GenerateRandomConfig();
        costFunc = &PL_PRM_ClosedChain::Error1;

        bool Result = GetSatisfactoryConfiguration(conf, endEffPose);
        if (Result)
        {
                Result = !IsInterfering(conf);
        }
        return Result;
}

Configuration PL_PRM_ClosedChain::GenerateRandomConfig () const
{
        Configuration conf;
        Frame endEffPose;
        conf = PL_PRM::GenerateRandomConfig();
        if (usePlanarConstraint)
        {
                endEffPose = planeDesired;
                costFunc = &PL_PRM_ClosedChain::Error3;
        }
        else if (useOrientConstraint)
        {
                endEffPose = orientDesired;
                costFunc = &PL_PRM_ClosedChain::Error4;
        }
        else
        {
                costFunc = &PL_PRM_ClosedChain::Error2;
        }

        GetSatisfactoryConfiguration(conf, endEffPose);
        //MakeItClosed(conf);
        return conf;
}

Configuration PL_PRM_ClosedChain::GenerateRandomConfig ( const Configuration& seed, const double& std_dev ) const
{
        Configuration conf;
        Frame endEffPose;
        conf = PL_PRM::GenerateRandomConfig(seed, std_dev);
        if (usePlanarConstraint)
        {
                endEffPose = planeDesired;
                costFunc = &PL_PRM_ClosedChain::Error3;
        }
        else if (useOrientConstraint)
        {
                endEffPose = orientDesired;
                costFunc = &PL_PRM_ClosedChain::Error4;
        }
        else
        {
                costFunc = &PL_PRM_ClosedChain::Error2;
        }

        GetSatisfactoryConfiguration(conf, endEffPose);
        return conf;
}

void PL_PRM_ClosedChain::ConnectEdgesFull( const node& newnode, const double& radius_squared )
{
        // verify connectIDp is valid.
        assert( connectIDp != NULL );

        // Save calculation time by determining configuration of the node once intstead of in each iteration
        Configuration nodeconfig = G.inf(newnode);

        node n1;

        // create a list of neighbour nodes to connect to within the given radius.
        node_pq<double> neighbours(G);
        forall_nodes( n1, G)
        {
                if ( newnode != n1)
                {
                        double edgelength_squared = Distance( nodeconfig, G.inf(n1) );  // returns the squared distance
                        // Check if the two nodes are within radius
                        if (  edgelength_squared <= radius_squared )  // can compare squares since a^2<b^2 if |a|<|b|
                        {
                                // node is within radius, add it as a neighbour with its distance as its priority.
                                neighbours.insert( n1, edgelength_squared );
                        }
                }
        }

        // New node is currently not connected to anyone, assign it the NIL_ID.
        (*connectIDp)[newnode] = NIL_ID;

        // Create a running list of graph connected components new node is attached to.
        list<int> connectIDs;

        // While the list of neighbour is not empty, connect the node to the closests nodes, not already graph
        // connected to.
        while ( !neighbours.empty() )
        {
                // get closest neighbour and remove it from the list
                node neighbour = neighbours.find_min();
                double dist_sq = neighbours.inf( neighbour );
                neighbours.del( neighbour );

                // connect the neighbour to the new node if it is not already graph connected.
                // A node is graph connected if its current connectID is in the running list of
                // connected graph components or has the same connectID as the new node.
                if ( ((*connectIDp)[neighbour] == NIL_ID) ||
                         ( ((*connectIDp)[neighbour] != (*connectIDp)[newnode]) &&
                           ( !NodeInConnectionList(neighbour, connectIDs) ) )
                   )
                {
                        edgeFrag->clear();
                        // nodes are not aleady connected.  Connect them now if there is a collision free path.
                        if ( !IsInterfering( nodeconfig, G.inf(neighbour) ) )
                        {
                                double edgelength = sqrt( dist_sq );
                                edge newedge = G.new_edge( newnode, neighbour, edgelength );
                                edgeChecked[newedge] = TRUE;
                                edgePath[newedge] = *edgeFrag;

                                //LogMessage("closedchain.log", "Configurations checked in fragment");
                                //for (int testj=0; testj<edgePath[newedge].size(); testj++)
                                //{
                                //      Fragment& E= edgePath[newedge];
                                //      list_item itt = E.get_item(testj);
                                //      Configuration conf = (E[itt]);
                                //      LogVector("closedchain.log", conf, "Conf: ");
                                //}

                                // update the connectionIDs to show these two nodes are now graph connected.
                                if ( (*connectIDp)[neighbour] != NIL_ID )
                                // neighbour node has been previously connected to a graph component.
                                {
                                        if ( (*connectIDp)[newnode] == NIL_ID )
                                        {
                                                // node has not been previously connected, assign it the connect ID of its new neighbour.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else if ( (*connectIDp)[neighbour] < (*connectIDp)[newnode] )
                                        {
                                                // the neighbour node has the smaller ID.  Select this as the new ID for the two
                                                // newly connected graph components (one from the new neighbour, one from the new node).

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[newnode] );

                                                // assign the lower connection ID to the new node.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else
                                        {
                                                // the new node has the smaller ID.  Select this as the new ID for the two components.

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[neighbour] );

                                                // assign the lower connection ID to the neighbour.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                }  // end if neighbour != NIL_ID
                                else
                                // neighbour has not been previously connected.
                                {
                                        if ( (*connectIDp)[newnode] != NIL_ID )
                                        {
                                                // new node has been previously connected, assign the neighbour its ID.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                        else
                                        {
                                                // both nodes have not been previously connected.  Create a new graph connected
                                                // component by assigning these two nodes the baseConnectID.
                                                (*connectIDp)[newnode] = baseConnectID;
                                                (*connectIDp)[neighbour] = baseConnectID;

                                                // Update the base connect ID to ensure a new unique ID is available for next new node.
                                                baseConnectID++;
                                        }
                                }  // end update connectIDs.

                        } // end if interfering
                        else
                        {
                                edgeFrag->clear();
                                // nodes have a collision path.  Use the midpoint between these two nodes as a seed for enhancing.
                                // Add the midpoint of this potential edge as a seed, if requested
                                if ( useMidPts )
                                {
                                        Configuration midpt = GetMidPoint( nodeconfig, G.inf(neighbour) );
                                        config_seeds.append( midpt );
                                }
                        }  // end if interfering else.

                }       // end if not already graph connected.

        } // end while !neighbours.empty().

        // Search the entire graph, if required, updating connectIDs to reflect how new node has graph connected
        // different components of the graph.
        if ( ((*connectIDp)[newnode] != NIL_ID) && ( !connectIDs.empty() ) )
        {
                // new node is connected to more than one graph connected component.  Update the connect IDs of
                // affected nodes.
                int newID = (*connectIDp)[newnode];

                forall_nodes( n1, G )
                {
                        if ( ((*connectIDp)[n1] != newID ) && (NodeInConnectionList( n1, connectIDs )) )
                        {
                                (*connectIDp)[n1] = newID;
                        }
                }
        }  // end update connectIDs of entire graph

        return;
}

bool PL_PRM_ClosedChain::MakeItClosed(Configuration &conf) const
{
        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY;
        double err_q = Error(q);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if (Error(q_next) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = Error(q);
                }
                if ( HasTimeLimitExpired() )
                {
                        return false;
                }
        }

        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_PRM_ClosedChain::IsClosed(const Configuration &conf) const
{
        return (Error(conf) <= DEF_ERR_TOLERANCE);
}

bool PL_PRM_ClosedChain::IsInterfering ( const Configuration& q1, const Configuration& q2 ) const
{
        int i, j, k;
        i = 0;  j = 0;  k = 0;
        Configuration q_last = q1;
        //Configuration q_end = q2;
        edgeFrag->append(q1);
        int MAX_I = MAX_ITERATION * MAX_RETRY * MAX_RETRY2;
        int MAX_J = MAX_RETRY;
        int MAX_K = MAX_RETRY2;
        double distToLast = sqrt(Distance(q_last, q2));
        bool result = false;
        while ((i<MAX_I) && (j<MAX_J) && (k<MAX_K) && (distToLast>DEF_DIST_TOLERANCE))
        {
                i++;  j++;
                //Configuration q_mid = PL_PRM::GenerateRandomConfig(q_last, DEF_NEIGHBOR*0.2);
                Configuration q_mid = q_last*(1-(DEF_DIST_TOLERANCE/distToLast)) + q2*(DEF_DIST_TOLERANCE/distToLast);
                if (MakeItClosed(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (PL_PRM::IsInterfering(q_mid))
                        {
                                result = true;
                                break;
                        }
                        if (Distance(q_mid, q2) < distToLast*distToLast)
                        {
                                k = 0;
                                edgeFrag->append(q_mid);
                                q_last = q_mid;
                                distToLast = sqrt(Distance(q_last, q2));
                        }
                }
                if ( HasTimeLimitExpired() )
                {
                        result = true;
                        break;
                }
        }

        //LogMessage("closedchain.log", "Configurations added into fragment");
        //for (int testj=0; testj<edgeFrag->size(); testj++)
        //{
        //      list_item itt = edgeFrag->get_item(testj);
        //      Configuration conf = edgeFrag->contents(itt);
        //      LogVector("closedchain.log", conf, "Conf: ");
        //}

        if ((!result) && (distToLast<=DEF_DIST_TOLERANCE))
        {
                edgeFrag->append(q2);
                return false;
        }
        else
        {
                edgeFrag->clear();
                return true;
        }
}

bool PL_PRM_ClosedChain::IsInterfering ( const Configuration& c1 ) const
{
        if (!IsClosed(c1)) 
                return true;
        if (PL_PRM::IsInterfering(c1))
                return true;
        return false;
}


void PL_PRM_ClosedChain::UpdatePlanarConstraint() 
{
        planeDesired = GetToolFrame( GetStartConfig() );
}

void PL_PRM_ClosedChain::UpdateOrientConstraint()
{
        //orientDesired = GetToolFrame( GetStartConfig() );
        orientDesired = Matrix4x4::Identity();
        orientDesired.Rotate(Vector4(1, 0, 0), -90);
        //orientDesired.Rotate(Vector4(0, 1, 0), 0);
        //orientDesired.Rotate(Vector4(0, 0, 1), 0);
}

//Distance between a configuration to a pose
double PL_PRM_ClosedChain::Error1(const Configuration &p1, const Frame &pose) const
{
        double distance;

        Frame toolFrame = GetToolFrame( p1 );
        if (!useOrientConstraint)
        {
                Vector4 offset = pose.GetTranslationVector() - toolFrame.GetTranslationVector();
                distance = offset.Magnitude();
        }
        else
        {
                /*
                double distance_sq;
                Vector4 offset = pose.GetTranslationVector() - toolFrame.GetTranslationVector();
                distance_sq = offset.MagSquared();
                Vector4 rot1, rot2;
                GetRotAngles(toolFrame, rot1[0], rot1[1], rot1[2]);
                GetRotAngles(pose, rot2[0], rot2[1], rot2[2]);
                rot1 -= rot2;
                distance_sq += Deg2Rad(1)*Deg2Rad(1)*rot1.MagSquared();
                distance = sqrt(distance_sq);
                */
                Vector4 PX(1, 0, 0);
                Vector4 PY(0, 1, 0);
                Vector4 PZ(0, 0, 1);
                // Generate vectors from base frame to each of the unit vector of the given frames.
                Vector4 Pax = toolFrame * PX;
                Vector4 Pay = toolFrame * PY;
                Vector4 Paz = toolFrame * PZ;

                Vector4 Pbx = pose * PX;
                Vector4 Pby = pose * PY;
                Vector4 Pbz = pose * PZ;

                // Generate difference vectors that connect the corresponding unit vectors of the given frames
                Vector4 dx = Pax - Pbx;
                Vector4 dy = Pay - Pby;
                Vector4 dz = Paz - Pbz;

                // Calculate the value of the distance metric (square the magitude of each difference vectors and sum.
                double distance_sq = dx.MagSquared() + dy.MagSquared() + dz.MagSquared();
                distance = sqrt(distance_sq);
                //*/
        }

        return distance;
}

//Distance between a configuration to a closure constraint
double PL_PRM_ClosedChain::Error2(const Configuration &p1, const Frame &pose) const
{
        Frame toolFrame = GetToolFrame( p1 );
        Vector4 offset = toolFrame.GetTranslationVector();
        offset[2] = 0;
        return offset.Magnitude();
}

//Distance between a configuration to a plane
double PL_PRM_ClosedChain::Error3(const Configuration &p1, const Frame &pose) const
{
        Frame toolFrame = GetToolFrame( p1 );
        //return fabs(toolFrame(1, 3)-planeDesired);
#if 0 //plannar constraint
        return fabs(toolFrame(1, 3)-pose(1, 3));

#ifdef ABCD //sphere constraint without constraint
        double cx, cy, cz, radius;
        cx=15;  cy=0;  cz=5;  //center
        radius = 12;
        Vector4 center(cx, cy, cz);
        Vector4 pos1 = toolFrame.GetTranslationVector();
        pos1 -= center;
        return sqrt((pos1.Magnitude() - radius)*(pos1.Magnitude() - radius));
#endif

#else //ifdef ABCD
        double cx, cy, cz, radius;
        cx=15;  cy=0;  cz=5;  //center
        radius = 12;
        Vector4 center(cx, cy, cz);
        //Get desired Z-axis
        Vector4 pos1 = pose.GetTranslationVector();
        Vector4 desiredZ = center;
        desiredZ -= pos1;
        desiredZ.Normalize();
        desiredZ *= -(radius-1);
        desiredZ += center;
        //Get current Z-axis
        Vector4 pos2 = toolFrame.GetTranslationVector();
        Vector4 currentZ;
        currentZ[0] = toolFrame(0, 2);
        currentZ[1] = toolFrame(1, 2);
        currentZ[2] = toolFrame(2, 2);
        currentZ += pos2;

        desiredZ -= currentZ;
        return desiredZ.Magnitude();
#endif
}

//Distance between a configuration to a plane
double PL_PRM_ClosedChain::Error4(const Configuration &p1, const Frame &pose) const
{
        Frame toolFrame = GetToolFrame( p1 );
        double yaw1, pitch1, roll1;
        double yaw2, pitch2, roll2;
        GetRotAngles(toolFrame, roll1, pitch1, yaw1);
        GetRotAngles(pose, roll2, pitch2, yaw2);
        return ((roll1-roll2)*(roll1-roll2) + (yaw1-yaw2)*(yaw1-yaw2)
                /*+ (pitch1-pitch2)*(pitch1-pitch2)*/);
}

double PL_PRM_ClosedChain::Error(const Configuration &p1) const
{
        Frame toolFrame = GetToolFrame( p1 );

        if (usePlanarConstraint)
        {
                //Before adding the sphere.
                //return fabs(toolFrame(1, 3)-planeDesired(1, 3));
                return Error3(p1, toolFrame);
        }
        else if (useOrientConstraint)
        {
                double yaw1, pitch1, roll1;
                double yaw2, pitch2, roll2;
                GetRotAngles(toolFrame, roll1, pitch1, yaw1);
                GetRotAngles(orientDesired, roll2, pitch2, yaw2);
                //return ((roll1-roll2)*(roll1-roll2) + (yaw1-yaw2)*(yaw1-yaw2));
                return ((roll1-roll2)*(roll1-roll2) + (yaw1-yaw2)*(yaw1-yaw2) 
                        /*+ (pitch1-pitch2)*(pitch1-pitch2)*/);
        }
        else
        {
                //here we're only working on 2D case, and assume the base is the origin
                Vector4 offset = toolFrame.GetTranslationVector();
                offset[2] = 0;
                return offset.Magnitude();
        }
}

void PL_PRM_ClosedChain::GetRotAngles( const Matrix4x4& frame, double& roll, double& pitch, double& yaw ) const
{
        // Declare the fixed angle variables.
        double alpha, beta, gamma;
        
        // Calculate beta and cache the cosine.
        beta = atan2( -frame(2,0) , sqrt( Sqr(frame(0,0)) + Sqr(frame(1,0)) ) );
        double cB = cos ( beta );

        // Calculate the other two rotations
        if ( cB == 0.0 )
        { 
                alpha = 0;
                if ( beta > 0 ) // if beta == PI/2
                {
                        gamma = atan2( frame(0,1) , frame(1,1) );
                }
                else
                {
                        gamma = -atan2( frame(0,1), frame(1,1) );
                }
        }
        else
        {
                alpha = atan2( frame(1,0) / cB , frame(0,0) / cB );
                gamma = atan2( frame(2,1) / cB , frame(2,2) / cB );
        }

        // Convert angles to degrees and assign to appropriate parameters
        // NOTE:  These angles must correspond to same angles used in SetGoalFrame
        yaw   = Rad2Deg( alpha );
        pitch = Rad2Deg( beta );
        roll  = Rad2Deg( gamma );
}

SuccessResultType PL_PRM_ClosedChain::TranslatePath()
{
        // Clear any current path.
        path.Clear();

        // Check if a graph path is available.
        if ( graphPath.empty() )
        {
                // Path is empty.  Set start as the beginning of the path, and return.
                path.AppendPoint( GetStartConfig() );

                // no path exists.
                return FAIL;
        }

        // If still in the function, there is a graphPath.
        // Go through all nodes stored in graphPath and assign the configuration they correspond to the
        // actual path.
        node n1;
        node n2;
        forall( n1, graphPath )
        {
                Configuration conf = G.inf(n1);
                path.AppendPoint( conf );
                n2 = graphPath.succ(n1);
                if (n2 == NULL)
                        continue;

                edge e;
                forall_inout_edges(e, n1)
                //edge e = G.first_in_edge(n1);
                //while(e)
                {
                        if (G.opposite(n1, e) == n2)
                        {
                                int nStep = 0;
                                int dir = -1;
                                Fragment& frag = edgePath[e];
                                int nFirst = frag.size();
                                if (conf == frag[frag[0]])
                                {
                                        nFirst = 0;
                                        dir = 1;
                                }
                                for (int i=1; i<(frag.size()-1); i++)
                                {
                                        nStep += dir;
                                        list_item it = frag[nFirst+nStep];
                                        conf = frag[it];
                                        //->get_item(i);//((*frag)[nFirst+dir]);
                                        path.AppendPoint( conf );
                                }
                                break;
                        }
                }
        }

        // Path has been translated and is ready to be animated.
        return PASS;
}

double LocalPlannerClosed::Distance(Configuration &conf1, Configuration &conf2)
{
        // Get the differences between each of the joints
        VectorN jointDiff;
        jointDiff = collisionDetector->JointDisplacement( conf1, conf2 );

        // Square and sum each of the joint differences
        double distance = 0;
        for( int i = 0; i < jointDiff.Length(); i++ )
        {
                distance += Sqr(jointDiff[i] );
        }

        // Return the squared euclidean distance
        return sqrt(distance);
}

bool LocalPlannerClosed::Plan()
{
    // check to see that SWIFTpp is the collision detector used
    const CD_Swiftpp* pSwiftpp = dynamic_cast< const CD_Swiftpp* >( collisionDetector );
    if (pSwiftpp == NULL)
    {
        return false;
    }

    // quick checks
        if ( collisionDetector->IsInterfering( GetStartConfig() ) )
        {
                // there is a collision with the start configuration...report failure
        //MessageBox(NULL, "Collision detected while planning.","IK_Jacobian Planner Error", MB_OK);
                return false;
        }

        // Start the timer
        StartTimer();

        // Establish the semaphore
        //Semaphore semaphore( guid );
        //semaphore.Lock();

    Configuration   currentConfig, goalConfig, nextConfig, jointVelocity;
    currentConfig.SetNumDOF(m_nDof);
    nextConfig.SetNumDOF(m_nDof);
    jointVelocity.SetNumDOF(m_nDof);
    for (int i = 0; i < m_nDof; i++)
    {
        jointVelocity[i] = 0;
    }
    currentConfig = GetStartConfig();
        goalConfig = GetGoalConfig();
    
    path.Clear();
    path.AppendPoint(currentConfig);

        //Initialize Jacobian, obstacle point and end-effector will share a Jacobian matrix.
        CJacobian jacobian(*m_pRobot);
        jacobian.SetConfiguration(currentConfig);
        //Frame frEndEffector = jacobian.GetJointFrame(m_nDof+1);

        bool abort = false;

        VectorN vEndEffVel(0, 0, 0);
    //for (int iViaPts = 0; iViaPts < (poses.size()-1); iViaPts++);
        double dist = Distance(currentConfig, goalConfig);
        while (dist > DEF_DIST_TOLERANCE )
    {
                jacobian.SetInterestPoint(m_nDof-1, m_frEndEffector, m_bPosition, m_bOrientation);
                //jacobian.GetJointVelocity(jointVelocity, vEndEffVel);

                Matrixmxn mJEnd, mJEndInv;
                mJEnd = *(jacobian.GetMatrix());
                mJEnd.Inverse(mJEndInv);

                Matrixmxn mTemp(m_nDof, m_nDof); 
                mTemp -= mJEndInv * mJEnd;

                //jointVelocity = currentConfig*(1-(DEF_DIST_TOLERANCE/dist)) + goalConfig*(DEF_DIST_TOLERANCE/dist);
                jointVelocity = (goalConfig-currentConfig);
                if (dist > DEF_DIST_TOLERANCE)
                        jointVelocity *= (DEF_DIST_TOLERANCE/dist);
                for (int i=0; i<m_nDof; i++)
                {
                        double dValue = jointVelocity[i];
                        jointVelocity[i] = Deg2Rad(dValue);
                }
                jointVelocity = mTemp * jointVelocity;

                //============= Begin of obstacles =====================
        // Getting the homogeneous solutions for the obstacles (Jo's)
        // check for the closest points between robot/obstacles
        if (GetClosestValues(currentConfig) != false)
                {
                        //LogMessage("Aborting.... Colliding when getting collision points!");
                        abort = true;
            break;
                }

        double obsDirMag = 0;
        // if closest distance is critical, abort
        if (m_vClosestPoints[0].distance <= m_dObsTol)
        {                
            //MessageBox(NULL, "Collision detected while planning", "IK_Jacobian Planner Error", MB_OK);
                        //LogMessage("Aborting.... Colliding, too close to obstacles!");
            abort = true;
            break;
        }            
        //** no obstacle scenario handled by infinite distance
        else
        {
                        VectorN vHomogeneousSolns;
                        vHomogeneousSolns = ComputeHomogeneousTerm(jacobian, mJEnd, mJEndInv, vEndEffVel);

                        //LogVector(vHomogeneousSolns, "Homogeneous Term:");
                        
                        jointVelocity += vHomogeneousSolns;
        } // obstacles exist

                //============= End of obstacles =====================

                //Convert to degree representation;
        CheckJointVelocities(jointVelocity);
                for (i=0; i<m_nDof; i++)
                {
                        double dValue = jointVelocity[i];
                        jointVelocity[i] = Rad2Deg(dValue);
                }

                //LogVector(jointVelocity, "Joint Velocity");
        
                // calculate where the new velocity takes us after time interval
                nextConfig = currentConfig + jointVelocity;
        CheckJointLimits(nextConfig);

                
 //               currentConfig = nextConfig;
//                              path.AppendPoint(currentConfig);
//              LogVector("ClosedLocal.log", nextConfig, "Next Configuration");                       



        // got next configuration, now need to see if it's valid               
        //if (collisionDetector->IsInterferingLinear(currentConfig, nextConfig ) == false)
                if (collisionDetector->IsInterfering(nextConfig ) == false)
        {        
            // there's no point adding it if it's the same
            if (currentConfig != nextConfig)        
            {
                currentConfig = nextConfig;
                path.AppendPoint(currentConfig);
                                dist = Distance(currentConfig, goalConfig);
               // GetEndEffVector(currentConfig, vCurPt);
            }
            jacobian.SetConfiguration(currentConfig);
        }
        else        // interfering
        {
            //MessageBox(NULL, "Obstacle in path could not be avoided","IK_Jacobian Planner Error", MB_OK);
                        //LogMessage("Aborting.... Colliding when checking next configuration!");
            abort = true;
                        break;
        }            
            //semaphore.Unlock();
            //semaphore.Lock();

        if (HasTimeLimitExpired() != false)
        {            
            //MessageBox(NULL, "Time's up!", "Plan() fail - Time's Up!", MB_OK);
                        //LogMessage("Aborting.... Time up!");
            abort = true;
                        break;
        }
    }

    //semaphore.Unlock();
        if (abort)
        {
                //LogMessage("### Planning Failure");
        }
        else
        {
        if (collisionDetector->IsInterferingLinear(currentConfig, goalConfig ) == false)
                {
            path.AppendPoint(goalConfig);
                }
                //LogMessage("### Planning Success");
        }
    return (abort == false);
}

//===========================================================================================

---