planners/closedchain/PL_PRM_ClosedChain.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/inversekin/Redundant.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_ERR_TOLERANCE    (1e-3)       //For End-effector
#define DEF_RESOLUTION       (15)         //Distance between two closed configuration


PL_PRM_ClosedBase::PL_PRM_ClosedBase()
{
        edgePath.init(G);
        edgeFrag = new Fragment;
}       

PL_PRM_ClosedBase::~PL_PRM_ClosedBase()
{
        edgeFrag->clear();
        delete edgeFrag;
}

bool PL_PRM_ClosedBase::Plan ()
{
        return PL_PRM::Plan();
}

//****************************************************************************
//             Function to Get Tool Frame: End-effector Frame.
//****************************************************************************
Frame PL_PRM_ClosedBase::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;
}

Configuration PL_PRM_ClosedBase::GenerateRandomConfig ()
{
        Configuration conf;
        conf = PL_PRM::GenerateRandomConfig();
        MakeItClosed(conf);
        return conf;
}

Configuration PL_PRM_ClosedBase::GenerateRandomConfig ( const Configuration& seed, const double& std_dev )
{
        Configuration conf;
        conf = PL_PRM::GenerateRandomConfig(seed, std_dev);
        MakeItClosed(conf);
        return conf;
}

void PL_PRM_ClosedBase::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_ClosedBase::IsClosed(const Configuration &conf) const
{
        return (Error(conf) <= DEF_ERR_TOLERANCE);
}

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

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

SuccessResultType PL_PRM_ClosedBase::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;
}

//===========================================================================================
PL_PRM_ClosedLocalJacobian::PL_PRM_ClosedLocalJacobian()
{
        planner = new LocalPlannerClosed;
        pCollisionDetector = NULL;
}

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

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

        if (pCollisionDetector != NULL)
                delete pCollisionDetector;

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

bool PL_PRM_ClosedLocalJacobian::Plan ()
{
        PlannerBase::StartTimer() ;
        path.Clear() ;
        
        planner->SetTimeLimitInSeconds(GetTimeLimitInSeconds());
        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;
}

bool PL_PRM_ClosedLocalJacobian::MakeItClosed(Configuration &conf)
{
        return true;
}

bool PL_PRM_ClosedLocalJacobian::IsInterfering ( const Configuration& q1, const Configuration& q2 )
{
        return false;
}

//===========================================================================================
PL_PRM_ClosedJacobian::PL_PRM_ClosedJacobian()
{
        m_pRobot = NULL;
        jacobian = NULL;
}

PL_PRM_ClosedJacobian::~PL_PRM_ClosedJacobian()
{
        if (jacobian != NULL)
                delete jacobian;
}

void PL_PRM_ClosedJacobian::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        PL_PRM_ClosedBase::SetCollisionDetector(collisionDetector);

        collisionDetector->DeactivateFrames(1, collisionDetector->DOF());
        this->collisionDetector = collisionDetector;
        m_nDof = collisionDetector->DOF();
        m_pRobot = dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0));
        m_nToolFrame = m_pRobot->GetToolFrame(0);
        m_frEndEffector = Matrix4x4::Identity();
        if (m_nToolFrame != -1)
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                m_frEndEffector = *(frameManager->GetFrameRef(m_nToolFrame));
        }
        if (jacobian != NULL)
        {
                delete jacobian;
        }
        jacobian = new CJacobian(*m_pRobot);
}

bool PL_PRM_ClosedJacobian::MakeItClosed(Configuration &conf)
{
        while (true)
        {
                Frame currentFrame = GetToolFrame(conf);
                Vector4 offset = currentFrame.GetTranslationVector();
                offset[2] = 0;
                
                double dDistance = offset.Magnitude();
                if (dDistance < DEF_ERR_TOLERANCE)
                        break;

                Matrixmxn mJEnd, mJEndInv;
                jacobian->SetConfiguration(conf);
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);

                VectorN endEffOffset(-offset[0], -offset[1], -offset[2]); 
                Configuration jointOffset;
                jointOffset = mJEndInv * endEffOffset *  Rad2Deg(1);
                conf += jointOffset;
        }

        return true;
}

bool PL_PRM_ClosedJacobian::IsInterfering ( const Configuration& q1, const Configuration& q2 )
{
        if (PL_PRM::IsInterfering(q1) || PL_PRM::IsInterfering(q2))
                return true;

        Configuration fromConf = q1;
        Configuration dirVel = q2 - q1;
        double distChanged1 = 360*q1.DOF(); //distance to goal;
        Configuration nextConf = fromConf;
        double dDistanceToEnd = dirVel.Magnitude();
        Configuration lastConf = q1;

        bool bBadConfig = false;
        Matrixmxn mJEnd, mJEndInv;
        Matrixmxn mTemp(m_nDof, m_nDof); 
        Matrixmxn matrixIdentity(m_nDof, m_nDof);

        edgeFrag->append(fromConf);
        jacobian->SetConfiguration(fromConf);
        while((bBadConfig == false) && (dDistanceToEnd > DEF_RESOLUTION))
        {
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                dirVel *= (DEF_RESOLUTION / dirVel.Magnitude());
                nextConf = nextConf + dirVel;
                MakeItClosed(nextConf);
                //LogConfiguration("JacobianError.log", nextConf);
                if (PL_PRM::IsInterfering(nextConf, lastConf))
                {
                        bBadConfig = true;
                }
                else
                {
                        //distChanged2 = dirVel.Magnitude();
                        dirVel = q2 - nextConf;
                        dDistanceToEnd = dirVel.Magnitude();
                }

                if (bBadConfig)
                {
                        edgeFrag->clear();
                        break;
                }
                else
                {
                        edgeFrag->append(nextConf);
                        jacobian->SetConfiguration(nextConf);
                }
                lastConf = nextConf;
        }

        if (!bBadConfig)
        {
                edgeFrag->append(q2);
        }

        return bBadConfig;
}

//===========================================================================================
PL_PRM_ClosedRGD::PL_PRM_ClosedRGD()
{
}

PL_PRM_ClosedRGD::~PL_PRM_ClosedRGD()
{
}

bool PL_PRM_ClosedRGD::MakeItClosed(Configuration &conf)
{
        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_ClosedRGD::IsInterfering ( const Configuration& q1, const Configuration& q2 )
{
        if (PL_PRM::IsInterfering(q1) || PL_PRM::IsInterfering(q2))
                return true;

        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 bBadConfig = false;
        while ((i<MAX_I) && (j<MAX_J) && (k<MAX_K) && (distToLast>DEF_RESOLUTION))
        {
                i++;  j++;
                //Configuration q_mid = PL_PRM::GenerateRandomConfig(q_last, DEF_NEIGHBOR*0.2);
                Configuration q_mid = q_last*(1-(DEF_RESOLUTION/distToLast)) + q2*(DEF_RESOLUTION/distToLast);
                if (MakeItClosed(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (PL_PRM::IsInterfering(q_mid, q_last))
                        {
                                bBadConfig = true;
                                break;
                        }
                        if (Distance(q_mid, q2) < distToLast*distToLast) //return value of Distance is squared!!
                        {
                                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 (distToLast > DEF_RESOLUTION)
                bBadConfig = true;
        if (!bBadConfig)
        {
                if (PL_PRM::IsInterfering(q_last, q2)==false)
                        edgeFrag->append(q2);
                else
                        bBadConfig = true;
        }
        if (bBadConfig)
        {
                edgeFrag->clear();
        }
        return bBadConfig;
}

//===========================================================================================
PL_PRM_ClosedAPDecomp::PL_PRM_ClosedAPDecomp()
{
}

PL_PRM_ClosedAPDecomp::~PL_PRM_ClosedAPDecomp()
{
}

void PL_PRM_ClosedAPDecomp::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        PL_PRM_ClosedBase::SetCollisionDetector(collisionDetector);
        
        //null is a valid collision detector to set this to
        if( this->collisionDetector != NULL )
        {
                FrameManager *frameManager=this->collisionDetector->GetFrameManager();
                redundant.SetFrameManager(frameManager);
                redundant.SetCollisionDetector(this->collisionDetector);

                int nDOF = collisionDetector->DOF();

                redundant.SetActiveBaseFrame(0);
                redundant.SetActiveFirstJoint(0);
                redundant.SetActiveLastJoint(nDOF-3);

                redundant.SetPassiveBaseFrame(0);
                redundant.SetPassiveFirstJoint(nDOF-2);
                redundant.SetPassiveLastJoint(nDOF-1);
        }
}

bool PL_PRM_ClosedAPDecomp::MakeItClosed(Configuration &conf)
{
        Configuration activeConf=conf;
        Configuration biasConf=conf;
        Frame frEndEffector = GetToolFrame(conf);
        frEndEffector(0,3)= 0;
        frEndEffector(1,3)= 0;
        redundant.GetConfigurationByActive(conf, activeConf, biasConf, frEndEffector);
        //redundant.GetConfigurationByActive(conf, activeConf, frEndEffector);
        return IsClosed(conf);
}

bool PL_PRM_ClosedAPDecomp::IsInterfering ( const Configuration& c1, const Configuration& c2 )
{
        if (PL_PRM::IsInterfering(c1) || PL_PRM::IsInterfering(c2))
                return true;

        Configuration q_last = c1;
        double dDistance = sqrt(Distance(c1, c2));
        int nStepNum = (dDistance/DEF_RESOLUTION)+1;
        if (nStepNum == 0)
                return true;

        edgeFrag->append(c1);
        bool bBadConfig = false;
        for (int i=1; i<=nStepNum; i++)
        {
                Configuration q_mid = c1*(1-((double)i/(double)nStepNum)) + c2*((double)i/(double)nStepNum);
                //LogConfiguration("APDError.log", q_mid);
                if (MakeItClosed(q_mid))
                {
                        if (PL_PRM::IsInterfering(q_mid, q_last))
                        {
                                bBadConfig = true;
                                break;
                        }
                        edgeFrag->append(q_mid);
                        q_last = q_mid;
                }
                else
                {
                        bBadConfig = true;
                        break;
                }
        }

        //if (!result) 
        //{
        //      edgeFrag->append(c2);
        //      return false;
        //}
        if (bBadConfig) //colliding!!!
        {
                edgeFrag->clear();
        }
        return bBadConfig;
}

//===========================================================================================
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_RESOLUTION )
    {
                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_RESOLUTION/dist)) + goalConfig*(DEF_RESOLUTION/dist);
                jointVelocity = (goalConfig-currentConfig);
                if (dist > DEF_RESOLUTION)
                        jointVelocity *= (DEF_RESOLUTION/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);

        // 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);
}

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

PL_PRM_ClosedChain::PL_PRM_ClosedChain()
{
        m_nAlgorithm = ALG_CLOSEDCHAIN_RGD;
}

PL_PRM_ClosedChain::~PL_PRM_ClosedChain()
{
}

bool PL_PRM_ClosedChain::Plan ()
{
        if (m_nAlgorithm == ALG_CLOSEDCHAIN_LOCAL)
                return PL_PRM_ClosedLocalJacobian::Plan();
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_JACOBIAN)
                return PL_PRM_ClosedJacobian::Plan();
        else
                return PL_PRM_ClosedBase::Plan();
}

bool PL_PRM_ClosedChain::MakeItClosed(Configuration &conf)
{
        if (m_nAlgorithm == ALG_CLOSEDCHAIN_RGD)
                return PL_PRM_ClosedRGD::MakeItClosed(conf);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_APD)
                return PL_PRM_ClosedAPDecomp::MakeItClosed(conf);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_JACOBIAN)
                return PL_PRM_ClosedJacobian::MakeItClosed(conf);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_LOCAL)
                return PL_PRM_ClosedLocalJacobian::MakeItClosed(conf);
        else
                return true;
}

bool PL_PRM_ClosedChain::IsInterfering ( const Configuration& c1, const Configuration& c2 )
{
        if (m_nAlgorithm == ALG_CLOSEDCHAIN_RGD)
                return PL_PRM_ClosedRGD::IsInterfering(c1, c2);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_APD)
                return PL_PRM_ClosedAPDecomp::IsInterfering(c1, c2);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_JACOBIAN)
                return PL_PRM_ClosedJacobian::IsInterfering(c1, c2);
        else if (m_nAlgorithm == ALG_CLOSEDCHAIN_LOCAL)
                return PL_PRM_ClosedLocalJacobian::IsInterfering(c1, c2);
        else
                return true;
}

void PL_PRM_ClosedChain::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        PL_PRM_ClosedBase::SetCollisionDetector( collisionDetector );

        if( this->collisionDetector == NULL )
                return;

        //Create a SWIFTpp collistion detector for Jacobian-based local planner
        if (pCollisionDetector != NULL)
                delete pCollisionDetector;
        pCollisionDetector = new CD_Swiftpp( *(collisionDetector->GetUniverse()) );
        planner->SetCollisionDetector(pCollisionDetector);
        pCollisionDetector->DeactivateFrames(1, pCollisionDetector->DOF());

        //Create jocobian for Jacobian computation
        collisionDetector->DeactivateFrames(1, collisionDetector->DOF());
        this->collisionDetector = collisionDetector;
        m_nDof = collisionDetector->DOF();
        m_pRobot = dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0));
        m_nToolFrame = m_pRobot->GetToolFrame(0);
        m_frEndEffector = Matrix4x4::Identity();
        if (m_nToolFrame != -1)
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                m_frEndEffector = *(frameManager->GetFrameRef(m_nToolFrame));
        }
        if (jacobian != NULL)
        {
                delete jacobian;
        }
        jacobian = new CJacobian(*m_pRobot);

        //Setups for Active Passive Link decomposition method.
        FrameManager *frameManager=this->collisionDetector->GetFrameManager();
        redundant.SetFrameManager(frameManager);
        redundant.SetCollisionDetector(this->collisionDetector);
        int nDOF = collisionDetector->DOF();

        redundant.SetActiveBaseFrame(0);
        redundant.SetActiveFirstJoint(0);
        redundant.SetActiveLastJoint(nDOF-3);

        redundant.SetPassiveBaseFrame(0);
        redundant.SetPassiveFirstJoint(nDOF-2);
        redundant.SetPassiveLastJoint(nDOF-1);

}

---