planners/ik_aca/IK_ACA.cpp

Go to the documentation of this file.

//=========================================================================
//
//  File:  IK_ACA.cpp
//
//  Created 31-Jul-01 by Shane Schneider
//
//              Inverse ACA planner for inverse kinematics.
//              Contains the inheritance from InvKinBase.



// IK_ACA
#include "synchronization/semaphore.h"
#include "robots/R_OpenChain.h"
#include "IK_ACA.h"

#include <assert.h>
#include <math.h>
#include <math/math2.h>
#include <math/vector4.h>
#include <math/matrix4x4.h>
#include <iosfwd>
#include <iostream>
#include <limits.h>

#include "Kinematics/DH_Link.h"


// fix gl path...
// #include <gl/glos.h>
#ifndef NOGL
#include "opengl/glos.h"
#include <gl/gl.h>
#endif

using std::endl;

//--------------------- Constants ---------------
// File Constants
static const char FILEEXT[] = ".ikaca";
static const char FILEHEADER[] = "INVERSE_ACA_PLANNER";
// Other Constants
static const Vector4 PX (1,0,0);
static const Vector4 PY (0,1,0);
static const Vector4 PZ (0,0,1);
static const double INFINITY = LONG_MAX; 
static const double DEFAULT_TOL = 0.02;
static const double DEFAULT_TOL1 = 0.005;
static const int DEFAULT_EMBRYOS_PER_NODE = 3;
static const double SAMPLING_TIME = 0.5;

#define _USE_ORIENTATION_
// Class IK_ACA 

// Default Constructor
IK_ACA::IK_ACA()
{
        // Assign the file header and extension for this type
        strcpy( fileext, FILEEXT );
        strcpy( fileheader, FILEHEADER );

        // Initialize the embryo count
        embryosPerLandmark = DEFAULT_EMBRYOS_PER_NODE;

        // Initialize the embryo node maps.
        embryo = new node_map<Configuration>[embryosPerLandmark];
        assert( embryo != NULL );

        for ( int i = 0; i < embryosPerLandmark; i++ )
        {
                embryo[i].init(G);
        }

        // Clears the searchPath
        searchPath.clear();

        // Initialize searchLandmark
        searchLandmark = nil;

        // Initialize the best params
        InitBestParams();
}


IK_ACA::~IK_ACA()
{
        // Default Destructor
        if ( embryo != NULL )
        {
                delete [] embryo;
                embryo = NULL;
        }
}

//=============================================================================
// DrawExplicit
//
// Purpose: Draws the planner data to the planner window when called
//=============================================================================
bool IK_ACA::DrawExplicit () const
{
        // use the GraphBase DrawExplicit and add planner specific drawing in DrawSpecific.
        return PL_GraphBase::DrawExplicit();
}


//=============================================================================
// Load
//
// Purpose: Loads partailly complete planner data from the disk
//=============================================================================
bool IK_ACA::Load (const char *filename)
{
        // Use GraphBase load routines.
        return PL_GraphBase::Load( filename );
}

//=============================================================================
// LoadContents
//
// Purpose: Loads the graph and other related base planner parameters to the the
//                  given ifstream.
//      
//                      Returns success of load.
//=============================================================================
bool IK_ACA::LoadContents( std::ifstream& infile )
{
        // Load the Graph and other base parameters
        bool success = PL_GraphBase::LoadContents( infile );
        if ( !success )
        {
                return false;
        }

        // Load the goal frame
        VectorN goalFrameVector;
        infile >> goalFrameVector;
        SetGoalFrame( goalFrameVector );

        
        // Load the ACA Specific Parameters here...
        
        // Prepare for Embryo Information
        infile >> embryosPerLandmark;
        if ( embryosPerLandmark < 1 )
        {
                embryosPerLandmark = 1;
        }
        if ( embryo != NULL )
        {
                delete [] embryo;
        }
        embryo = new node_map<Configuration>[embryosPerLandmark];
        assert( embryo != NULL );
        for ( int i = 0; i < embryosPerLandmark; i++ )
        {
                embryo[i].init(G);
        }


        // Load Embryo Information
        node n1;
        forall_nodes( n1, G )
        {
                int nodeID;
                infile >> nodeID;
                for( int i = 0; i < embryosPerLandmark; i++ )
                {
                        infile >> embryo[i][n1];
                }
        }

        InitNewSearch();

        return true;
}


//=============================================================================
// Does Planning
//
// Purpose: performs the planning task
//=============================================================================
bool IK_ACA::Plan ()
{
        // quick checks
        if ( IsInterfering( GetStartConfig() ) )
        {
                // there is a collision with the start configuration...report failure
                return false;
        }

/*      if ( goalFrame.Compare(GetToolFrame( goalConfig ),DEFAULT_TOL) )
        {
                // solution has already been found
                return true;
        }*/

        // Start the timer
        StartTimer();

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

        // remove the goal node if it exists in the graph
        if ( ( goalNode != nil ) && ( goalNode != startNode ) )
        {
                G.del_node( goalNode );
        }
        goalNode = nil;

        // erase the graph path
        ClearGraphPath();

        // Start the search
        SuccessResultType success = FAIL;
        
        // add inverse ACA planning alogrithm here...
        while ( true )
        {
                // Find the global mininium starting from the last landmark;
                if ( searchLandmark != nil )
                {
                        semaphore.Unlock();
                        success = Search( searchLandmark );
                        semaphore.Lock();
                }

                if ( success != FAIL )
                {
                        break;
                }

                // Last seach was not successful.  Use another landmark.
                if (( searchLandmark != nil) && ( searchLandmark != G.last_node() ))
                {
                        // use next landmark in tree
                        searchLandmark = G.succ_node( searchLandmark );
                }
                else
                {
                        // add new landmark to tree
                        searchLandmark = Explore();
                }

                // Pause for drawing
                semaphore.Unlock();
                semaphore.Lock();
        }

        // Pause for drawing
        semaphore.Unlock();
        semaphore.Lock();
        
        if ( success == PASS )
        {
                // A successful path has been found.  Record it.
                ComputePath( searchLandmark, searchPath );

                // best path is the solved path, so clear old best path
                bestPath.clear();

                // Unlock the semaphore before leaving.
                semaphore.Unlock();
                return true;
        }
        else
        {
                // No successful path was found, record the best one.
                //ComputePath( bestLandmark, bestPath );

                // only show the best path on the graph, clear searchPath
                searchPath.clear();

                // Unlock the semaphore before leaving.
                semaphore.Unlock();
                return false;
        }
}

//=============================================================================
// Save
//
// Purpose: Saves partial plan data to disk
//=============================================================================
bool IK_ACA::Save (const char *filename) const
{
        // Use GraphBase save routines.
        return PL_GraphBase::Save( filename );
}

//=============================================================================
// SaveContents
//
// Purpose: Saves the graph and other related base planner parameters to the the
//                  given ofstream.
//      
//                      Returns success of save.
//=============================================================================
bool IK_ACA::SaveContents ( std::ofstream& outfile ) const
{
        // Save the Graph and parent parameters
        bool success = PL_GraphBase::SaveContents( outfile );
        if ( !success )
        {
                return false;
        }

        // Save the goal frame
        outfile << GetGoalFrameVector() << endl;

        // Save the ACA specific parameters here...

        // Save the embryo count.
        outfile << embryosPerLandmark << endl;

        // Save Embryo Information
        node n1;
        forall_nodes( n1, G )
        {
                outfile << n1->id();

                for( int i = 0; i < embryosPerLandmark; i++ )
                {
                        outfile << embryo[i][n1];
                }

                outfile << endl;
        }
        
        
        return true;
}




//=============================================================================
// DrawSpecific
//
// Purpose: Draw Inverse ACA planner specific info.  
//                      This function will draw all the connecting edges between the embyros
//                      and their parent landmarks. 
//
//=============================================================================
bool IK_ACA::DrawSpecific() const
{
        node landmark;
        double Xcor, Ycor, Zcor;

#ifndef NOGL
        // Draw the edges connecting the end of the embyros to their parent landmark (node).
        glLineWidth( 1.0f );
        glBegin(GL_LINES);
                // Draw all connecting edges
                glColor3f(0.4f,0.4f,0.4f);      // set colour to grey
                forall_nodes(landmark,G)
                {
                        for( int i = 0; i < embryosPerLandmark; i++ )
                        {
                                GetCoords( embryo[i][landmark], Xcor, Ycor, Zcor );     // get coordinates of source node;
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line.

                                GetCoords( landmark, Xcor, Ycor, Zcor );        // get coordinates of target node;
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line.
                        }
                }

                // draw the lines that indicate the progress of the search routine
                if ( !searchPath.empty() )
                {
                        list_item index = searchPath.last();
                        list_item first = searchPath.first();
                        glColor3f(0.0f,1.0f,0.0f);              // set colour to green
                        
                        while ( first != index )
                        {
                                GetCoords( searchPath.inf( index ), Xcor, Ycor, Zcor );
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for last line.

                                index = searchPath.pred( index );
                                
                                GetCoords( searchPath.inf( index ), Xcor, Ycor, Zcor );
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line.
                        }
                }

                // draw the lines that indicate the best path so far
                if ( !bestPath.empty() )
                {
                        list_item index = bestPath.last();
                        list_item first = bestPath.first();
                        glColor3f(0.5f,0.0f,0.625f);            // set colour to dark purple
                        
                        while ( first != index )
                        {
                                GetCoords( bestPath.inf( index ), Xcor, Ycor, Zcor );
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for last line.

                                index = bestPath.pred( index );
                                
                                GetCoords( bestPath.inf( index ), Xcor, Ycor, Zcor );
                                glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line.
                        }
                }

        glEnd();
#endif
        return true;
}


//=============================================================================
// SetStartConfig
//
// Purpose: Sets the start config and assigns it as first node in the current graph.
//
//=============================================================================
void IK_ACA::SetStartConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( startNode != nil ) && ( GetStartConfig() == config ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // A new configuration has been requested.  Need to clear the current graph and search parameters
        ClearGraph();
        searchPath.clear();
        
        // Call the graph base SetStartConfig to ensure all required initialization is established
        // and the start config will be added as the first landmark.
        PL_GraphBase::SetStartConfig( config );

        // Verify the startNode was initiated.
    assert( startNode != nil );

        // Create an embryo for the start landmark.
        CreateEmbryos( startNode );

        // Initialize a new search
        this->InitNewSearch();
        

}

//=============================================================================
// SetGoalConfig
//
// Purpose: Resolves ambiguity by ensuring only the InvKinBase version of
//                      SetGoalConfig is called.
//=============================================================================
void IK_ACA::SetGoalConfig( const Configuration& config )
{
        // Check if new goal config (frame) was requested.
        if ( config != GetGoalConfig() )
        {
                // Assign new goal config (frame)
                IK_InvKinBase::SetGoalConfig( config );
        }

}

//=============================================================================
// AssignGoalConfig
//
// Purpose: After planner is completed, this function is called to save the 
//                      the computed configuration as the goal configuration and node
//                      in the graph.  
//=============================================================================
void IK_ACA::AssignGoalConfig( const Configuration& config )
{
        // Call parent's version of AssignGoalConfig
        IK_InvKinBase::AssignGoalConfig( config );

        // Add the goal node to the graph.
        PL_GraphBase::SetGoalConfig( config );

        // Assign all embryos of goal to itself.
        for ( int i = 0; i < embryosPerLandmark; i++ )
        {
                embryo[i][goalNode] = config;
        }

}



//=============================================================================
// SetCollisionDetector
//
// Purpose: Resolves ambiguity by ensuring the GraphBase version of
//                      SetCollisionDetector is called.
//=============================================================================
/*void IK_ACA::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        // Call the parent's SetCollisionDetector to ensure proper assignment.
        PL_GraphBase::SetCollisionDetector( collisionDetector );

}
*/


//=============================================================================
// CopySettings
//
// Purpose: copies the settings from a different planner
//=============================================================================
void IK_ACA::CopySettings( PlannerBase* original )
{
        // Call parent copy settings
        IK_InvKinBase::CopySettings( original );
        PL_GraphBase::CopySettings( original );

        IK_ACA* originalACA = dynamic_cast< IK_ACA* >( original );
        if( originalACA == NULL )
        {
                return;
        }

        //copy ACA specific parameters here...
        SetEmbryosPerLandmark( originalACA->embryosPerLandmark );
        
        // Initialize a new search
        InitNewSearch();


}

//=============================================================================
// CreateEmbryos
//
// Purpose: creates required number of new valid embyro for given landmark.
//=============================================================================
void IK_ACA::CreateEmbryos (const node& landmark )
{
        for (int i = 0; i < embryosPerLandmark; i++ )
        {
                CreateEmbryo( landmark, i );
        }

}


//=============================================================================
// CreateEmbryo
//
// Purpose: creates a new valid embyro and assigns it to the given landmark.
//=============================================================================
void IK_ACA::CreateEmbryo(const node& landmark, const int& embryoID)
{
        // propose a new configuration.
        //Configuration newconfig = GenerateRandomConfig();
        Configuration landmarkConfig = G.inf( landmark );
        Configuration newconfig = GenerateRandomConfig( landmarkConfig ); 
        Configuration start = GetStartConfig();
        //newconfig[0] = start[0];
        //newconfig[1] = start[1];
        //newconfig[5] = start[5];

        //LogMessage("ik-aca.log", "==============================");
        //LogVector("ik-aca.log", landmarkConfig, "Configuration get in explore");
        //LogVector("ik-aca.log", newconfig, "Configuration get in explore");

        // verify new config has a straight-line path from landmark.
        if ( IsInterfering( landmarkConfig, newconfig ) )
        {
                // Collision occurred.  Use the mid point between landmark and collision as the
                // new config for the embyro.
                newconfig = PL_GraphBase::collisionDetector->GetLastIntersection();
                newconfig = GetMidPoint( landmarkConfig, newconfig );
                //LogMessage("ik-aca.log", "Get mid point....");

        }

        // Assign new config as the embyro config associated with the landmark.
        embryo[embryoID][landmark] = newconfig;
        //LogVector("ik-aca.log", newconfig, "Configuration get in explore");

}

//=============================================================================
// Explore
//
// Purpose: Explores C-space by updating the road map.  
//                      Takes the furtherst embyro in the road map and converts it
//                      into a landmark, and creates two new embyros.  
//              
//                      This function returns the new landmark so it can be used for the next
//                      iteration of search.
//
//=============================================================================
node IK_ACA::Explore()
{
        double landmark_dist = 0;

        node n1, n2, parentNode;
        double distance, min_dist;
        int embryoID;

        // find the embryo that is the farthest
        forall_nodes(n1, G)
        {
                for( int i = 0; i < embryosPerLandmark; i++)
                {
                        min_dist = INFINITY;
                        forall_nodes(n2, G)
                        {
                                distance = Distance( embryo[i][n1] , G.inf(n2) );
                                if ( distance < min_dist )
                                {
                                        min_dist = distance;
                                }
                        }
        
                        if ( min_dist > landmark_dist )
                        {
                                // record current embryo's parent, its ID, and its landmark distance.
                                parentNode = n1;
                                embryoID = i;
                                landmark_dist = min_dist;
                        }
                }
        }

        // Convert embryo into a new node and add it to the end of the node list.
        n1 = G.new_node( G.last_node(), embryo[embryoID][parentNode], LEDA::after);

        // Record straight-line path from old embryo to its parent and represent it as an edge in roadmap.
        distance = sqrt( Distance( n1, parentNode ) );
        G.new_edge( n1, parentNode, distance );


        // Create new embryos for old embryo and new landmark.
        CreateEmbryo( parentNode, embryoID );     // old embryo's parent who needs a replacement embryo.
        CreateEmbryos( n1 );    // new landmark in road map.

        // return the new landmark
        return n1;
        
}


//=============================================================================
// Search
//
// Purpose: Performs the search algorithm for ACA.  Will start from a 
//                  given configuration and apply the cost minimizing function
//                      until a local min configuration is found
//
// NOTE:        This function uses a semaphore, so all semaphores must be unlocked
//                      prioir to this function being called.
//=============================================================================
SuccessResultType IK_ACA::Search( const node& landmark )
{
        // Establish the semaphore
        Semaphore semaphore( guid );
        if( this->m_UseSemaphores == true )
        {
                semaphore.Lock();
        }

        // Clear the current search path and start fresh with the land mark.
        searchPath.clear();
        searchPath.append( G.inf(landmark) );

        while ( !HasTimeLimitExpired() )
        {
                list<Configuration> minPath;
                Configuration q0 = searchPath.back();
                Configuration q1 = MinimizeDistance( q0 , minPath );

                // get the latest tool frame
                Matrix4x4 toolFrame = GetToolFrame( q1 );

                // check if solution has been found
                if ( q1.Compare( q0 , DEFAULT_TOL1 ) )
                {
                        // local minimum has been found.  Need to start again with new landmark

                        // check if local min is the best so far
                        double dist_sq = FrameDistance( goalFrame, toolFrame );
                        if ( dist_sq < bestDistance_sq )
                        {
                                // this config is closer than previous best, record it.
                                bestDistance_sq = dist_sq;
                                bestLandmark = landmark;
                                bestPath = searchPath;
                        }

                        // report failure so we can start again.
                        semaphore.Unlock();
                        return FAIL;
                }

                // add new config to search path
                if ( !IsInterfering( q0, q1 ) )
                {
                        // direct straight-line path exists from previous iteration, add new config directly.
                        searchPath.append( q1 );
                }
                else
                {
                        // path has collision, use the collision free path generated from minimizing configs
                        searchPath.conc( minPath , LEDA::after );
                }
                
                // check if tool frame now matches goal.
//#ifdef _USE_POSITION_ONLY_
                double distToGoal = FrameDistance(goalFrame, toolFrame);
                if ( distToGoal < DEFAULT_TOL )
//#else
//              if ( toolFrame.Compare( goalFrame , DEFAULT_TOL ) )
//#endif
                {
                        // goal and tool frames match.  Report success.
                        semaphore.Unlock();
                        return PASS;
                }

                semaphore.Unlock();
                semaphore.Lock();

        }

        // If escaped from While LOOP, then timer has expired.

        semaphore.Unlock();
        return TIMER_EXPIRED;

}

SuccessResultType IK_ACA::Search1( const node& landmark )
{
        // Establish the semaphore
        Semaphore semaphore( guid );
        if( this->m_UseSemaphores == true )
        {
                semaphore.Lock();
        }

        // Clear the current search path and start fresh with the land mark.
        searchPath.clear();

        searchPath.append( G.inf(landmark) );

        //get configuration of Searchlandmark
        Configuration gh=searchPath.back();

        Matrix4x4 toolFrame = GetToolFrame( gh );               
        // check if tool frame now matches goal.
        double distToGoal = FrameDistance(goalFrame, toolFrame);
        if ( distToGoal < DEFAULT_TOL )
        //if ( toolFrame.Compare( goalFrame , DEFAULT_TOL ) )
        {
                // goal and tool frames match.  Report success.
                semaphore.Unlock();
                return PASS;
        }

    while ( !HasTimeLimitExpired() )
        {
                Configuration q0 = searchPath.back();
                Configuration q1 = GetNextConfig( q0 );

                for (int i = 0; i<q0.DOF(); i++)
                {
                        if ( q1[i] < collisionDetector->JointMin(i) )
                                q1[i] = collisionDetector->JointMin(i);
                        else if ( q1[i] > collisionDetector->JointMax(i) )
                                q1[i] = collisionDetector->JointMax(i);
                }

                // check if solution has been found
                if ( q1.Compare( q0 , DEFAULT_TOL1*DEFAULT_TOL1 ) )
                {
                        // check if local min is the best so far
                        double dist_sq = FrameDistance( goalFrame, toolFrame );
                        if ( dist_sq < bestDistance_sq )
                        {
                                // this config is closer than previous best, record it.
                                bestDistance_sq = dist_sq;
                                bestLandmark = landmark;
                                bestPath = searchPath;
                        }

                        // report failure so we can start again.
                        semaphore.Unlock();
                        return FAIL;
                }
                
                // get the latest tool frame
                toolFrame = GetToolFrame( q1 );         
                // check if tool frame now matches goal.
                distToGoal = FrameDistance(goalFrame, toolFrame);
                if ( distToGoal < DEFAULT_TOL )
                //if ( toolFrame.Compare( goalFrame , DEFAULT_TOL ) )
                {
                        // goal and tool frames match.  Report success.
            searchPath.append( q1 );
                        semaphore.Unlock();
                        return PASS;
                }               

                if (IsInterfering(q1))
                {
                        semaphore.Unlock();
                        return FAIL;
                }

                // add new config to search path
                searchPath.append( q1 );

                semaphore.Unlock();
                semaphore.Lock();
        }

        // If escaped from While LOOP, then timer has expired.

        semaphore.Unlock();
        return TIMER_EXPIRED;
}

VectorN IK_ACA::ComputeEndEffDiff(Matrix4x4 &frStart, Matrix4x4 &frEnd)
{
        VectorN returnMe;
        returnMe.SetLength(6);

        int index = 0;
        returnMe[index++] = frEnd(0, 3) - frStart(0, 3);
        returnMe[index++] = frEnd(1, 3) - frStart(1, 3);
        returnMe[index++] = frEnd(2, 3) - frStart(2, 3);

        //Frame rotated = frStart.Inverse() * frEnd;
        Matrix4x4 rotated = frEnd * frStart.Inverse();
        double temp = rotated.values[0][0] + rotated.values[1][1] + rotated.values[2][2] - 1;
        assert(fabs(temp) <= (2+1e-5));
        if (temp>2.)
                temp = 2.0;
        else if (temp<-2.)
                temp = -2.0;
        double angle = acos(temp/2);
        Vector4 axis;
        axis[0] = rotated.values[2][1] - rotated.values[1][2];
        axis[1] = rotated.values[0][2] - rotated.values[2][0];
        axis[2] = rotated.values[1][0] - rotated.values[0][1];

        if (sin(angle) != 0)
                axis /= (2*sin(angle));

        axis *= angle;

        returnMe[index++] = axis[0];
        returnMe[index++] = axis[1];
        returnMe[index++] = axis[2];

        return returnMe;        
}

Configuration IK_ACA::GetNextConfig( Configuration& q0 )
{
        Matrix4x4 current_tool_frame=GetToolFrame(q0);
        VectorN Start_Goal_Diff=ComputeEndEffDiff(current_tool_frame, goalFrame);
        if (Start_Goal_Diff.Magnitude() > SAMPLING_TIME)
        {
                Start_Goal_Diff.Normalize();
                Start_Goal_Diff*=SAMPLING_TIME;
        }
        Frame identity;
        jacobian->SetConfiguration(q0);
        jacobian->SetInterestPoint(q0.DOF()-1, identity, true, true);
        VectorN jointDiff;
        jacobian->GetJointVelocity(jointDiff, Start_Goal_Diff);
        //LogVector("ik-aca.log", jointDiff, "jointDiff:");
        Configuration q1=q0+jointDiff*Rad2Deg(1);
        //LogVector("ik-aca.log", q1, "NextConfiguration:");
        return q1;
}

//=============================================================================
// SetCollisionDetector
//
// Purpose: Assigns the collision detector.
//
//=============================================================================
void IK_ACA::SetCollisionDetector (CollisionDetectorBase* collisionDetector)
{
        //assign the goal frame
        CD_BasicStyle* basicStyle = dynamic_cast< CD_BasicStyle* >(  collisionDetector );
        assert( basicStyle != NULL );
        if( basicStyle != NULL )
        {
                R_OpenChain *   robot; 
                robot = dynamic_cast<R_OpenChain*>(basicStyle->GetRobot(0));
                jacobian = new CJacobian(*robot);
                //FrameManager* fm = basicStyle->GetFrameManager();
                //int newFrame = fm->AddFrame();
                PL_GraphBase::SetCollisionDetector( basicStyle );
        }
}

//=============================================================================
// FindJointAdjust
//
// Purpose: Applies the cost minimizing function to the given joint
//                      to find adjustment required to minimize the distance between
//                      goal and tool frames.
//
// Note:  This function does not perform any collision or joint limit checking.
//
//=============================================================================
double IK_ACA::FindJointAdjust( const unsigned int& jointNum, const Matrix4x4& toolFrame ) const
{

        // get pointer to frame manager
        FrameManager* frameManager = collisionDetector->GetFrameManager();

        // get link frame
        Matrix4x4 linkFrame = frameManager->GetWorldFrame( collisionDetector->JointFrameNum( jointNum ) );

        // create vectors from link frame to tips of unit axes vectors of goal frame
        Matrix4x4 goalWRTlinkFrame = FrameManager::GetTransformRelative( goalFrame, linkFrame );
#ifdef _USE_ORIENTATION_
        Vector4 Pgx = goalWRTlinkFrame * PX;
        Vector4 Pgy = goalWRTlinkFrame * PY;
        Vector4 Pgz = goalWRTlinkFrame * PZ;
#else
        Vector4 goalVec;
        goalVec[0] = goalWRTlinkFrame(0, 3);
        goalVec[1] = goalWRTlinkFrame(1, 3);
        goalVec[2] = goalWRTlinkFrame(2, 3);
#endif

        // create vectors from link frame to tips of unit axes vectors of tool frame
        Matrix4x4 toolWRTlinkFrame = FrameManager::GetTransformRelative( toolFrame, linkFrame );
#ifdef _USE_ORIENTATION_
        Vector4 Ptx = toolWRTlinkFrame * PX;
        Vector4 Pty = toolWRTlinkFrame * PY;
        Vector4 Ptz = toolWRTlinkFrame * PZ;
#else
        Vector4 toolVec;
        toolVec[0] = toolWRTlinkFrame(0, 3);
        toolVec[1] = toolWRTlinkFrame(1, 3);
        toolVec[2] = toolWRTlinkFrame(2, 3);
#endif

        // variable to store joint adjustment.
        double adjustment;
        
        // Find joint adjustment to minimize the distance metric
        DH_Parameter jointType = collisionDetector->JointType( jointNum );
        switch ( jointType )
        {
                case DH_THETA :
                        {
                                // Using a rotation about Z, find minimum.  See thesis for details of this equation.
                                // MAPLE Output:  
                                //    y[gz]*y[tz]+x[gz]*x[tz]+x[gy]*x[ty]+y[gy]*y[ty]+x[tx]*x[gx]+y[tx]*y[gx] = a
                                //    y[gz]*x[tz]-x[gz]*y[tz]-x[gy]*y[ty]+y[gy]*x[ty]+x[tx]*y[gx]-y[tx]*x[gx] = b
#ifdef _USE_ORIENTATION_
                                double a = Pgz[1]*Ptz[1] + Pgz[0]*Ptz[0] + Pgy[0]*Pty[0] + Pgy[1]*Pty[1] + Pgx[0]*Ptx[0] + Pgx[1]*Ptx[1];
                                double b = Pgz[1]*Ptz[0] - Pgz[0]*Ptz[1] - Pgy[0]*Pty[1] + Pgy[1]*Pty[0] + Ptx[0]*Pgx[1] - Ptx[1]*Pgx[0];
#else
                                double a = goalVec[0]*toolVec[0] + goalVec[1]*toolVec[1];
                                double b = -goalVec[0]*toolVec[1] + toolVec[0]*goalVec[1];
#endif                          
                                double theta1 = atan2( b , a );
                                double theta2;
                                if ( theta1 > 0 )
                                {
                                        theta2 = theta1 - M_PI;
                                }
                                else
                                {
                                        theta2 = theta1 + M_PI;
                                }
        
                                // differential equation gave two answers, find the one that minimizes the cost function
                                double val1 = -a*cos(theta1) - b*sin(theta1);
                                double val2 = -a*cos(theta2) - b*sin(theta2);
                                if ( val1 < val2 )
                                {
                                        adjustment = Rad2Deg( theta1 );
                                }
                                else
                                {
                                        adjustment = Rad2Deg( theta2 );
                                }
                        }
                        break;
                case DH_ALPHA :
                        {
                                // Using a rotation about X, find minimum.  See thesis for details of this equation.
                                // MAPLE Output:  
                                //    z[gy]*y[ty]-y[gy]*z[ty]-z[tx]*y[gx]+y[tx]*z[gx]-y[gz]*z[tz]+z[gz]*y[tz] = b
                                //    y[gz]*y[tz]+z[gz]*z[tz]+y[gy]*y[ty]+z[gy]*z[ty]+y[tx]*y[gx]+z[tx]*z[gx] = a
#ifdef _USE_ORIENTATION_
                                double b = Pgy[2]*Pty[1] - Pgy[1]*Pty[2] - Ptx[2]*Pgx[1] + Ptx[1]*Pgx[2] - Pgz[1]*Ptz[2] + Pgz[2]*Ptz[1];
                                double a = Pgz[1]*Ptz[1] + Pgz[2]*Ptz[2] + Pgy[1]*Pty[1] + Pgy[2]*Pty[2] + Pgx[1]*Ptx[1] + Pgx[2]*Ptx[2];
#else
                                assert(false); //haven't done yet;
                                double b = 1;
                                double a = 1;
#endif
                                        
                                double alpha1 = atan2( b , a );
                                double alpha2;
                                if ( alpha1 > 0 )
                                {
                                        alpha2 = alpha1 - M_PI;
                                }
                                else
                                {
                                        alpha2 = alpha1 + M_PI;
                                }
        
                                // differential equation gave two answers, find the one that minimizes the cost function
                                double val1 = -a*cos(alpha1) - b*sin(alpha1);
                                double val2 = -a*cos(alpha2) - b*sin(alpha2);
                                if ( val1 < val2 )
                                {
                                        adjustment = Rad2Deg( alpha1 );
                                }
                                else
                                {
                                        adjustment = Rad2Deg( alpha2 );
                                }
                        }
                        break;
                case DH_D :
                        {
                                // Using prismatic along Z, find minimum.  See thesis for details of this equation.
                                // MAPLE Output:
                                //        1/3*z[gz]+1/3*z[gy]-1/3*z[tx]+1/3*z[gx]-1/3*z[ty]-1/3*z[tz]
#ifdef _USE_ORIENTATION_
                                adjustment = ( Pgz[2] + Pgy[2] - Ptx[2] + Pgx[2] - Pty[2] - Ptz[2] ) / 3;
#else
                                adjustment = 0;
#endif
                        }
                        break;
                case DH_A :
                        {
                                // Using prismatic along Z, find minimum.  See thesis for details of this equation.
                                // MAPLE Output:
                                //        1/3*x[gz]+1/3*x[gy]-1/3*x[tx]+1/3*x[gx]-1/3*x[ty]-1/3*x[tz]
#ifdef _USE_ORIENTATION_
                                adjustment = ( Pgz[0] + Pgy[0] - Ptx[0] + Pgx[0] - Pty[0] - Ptz[0] ) / 3;
#else
                                adjustment = 0;
#endif
                        }
                        break;
                default :
                        adjustment = 0;
        }

        // return the joint adjustment to minimize distance between goal and tool frame.
        return adjustment;

}

//=============================================================================
// MinimizeDistance
//
// Purpose: Applies the cost minimizing function to the given configuration
//                      to find a valid configuration that minimizes the distance metric
//
//=============================================================================
Configuration IK_ACA::MinimizeDistance( const Configuration& config , list<Configuration>& minPath )
{

        Configuration minConfig = config;
        minPath.clear();

        // Iteratively apply the cost minimizing function to each joint sequentially.
        int dof = collisionDetector->DOF();
        for ( int i = 0; i < dof; i++ )
        {
                // find the current tool frame for given config
                Matrix4x4 toolFrame = GetToolFrame( minConfig );

                // adjust the joint to minimize distance for that joint 
                double jointAdjust = minConfig[i] + FindJointAdjust( i, toolFrame );

                // ensure joint does not exceeed the limits
                double jmax = collisionDetector->JointMax( i );
                double jmin = collisionDetector->JointMin( i );
                if ( collisionDetector->JointWraps( i ) )
                {
                        if ( jointAdjust > jmax )
                        {
                                jointAdjust -= ( jmax - jmin );
                        }
                        else if ( jointAdjust < jmin )
                        {
                                jointAdjust += ( jmax - jmin );
                        }
                }
                else
                {
                        if ( jointAdjust > jmax )
                        {
                                jointAdjust = jmax;
                        }
                        if ( jointAdjust < jmin )
                        {
                                jointAdjust = jmin;
                        }
                }

                // create new configuration with joint adjustment
                Configuration newConfig = minConfig;
                newConfig[ i ] = jointAdjust;

                // Perform collision checking to ensure path is clear
                if ( IsInterfering( minConfig, newConfig ) )
                {
                        // Set the new minConfig for next iteration based on the last valid point
                        minConfig = collisionDetector->GetLastValid();  
                }
                else
                {
                        // set the new minConfig for next iteration based on collision free adjustment
                        minConfig[ i ] = jointAdjust;
                }

                minPath.append( minConfig );

        }

        return minConfig;
                
}


//=============================================================================
// FrameDistance
//
// Purpose: Computes the squared distance between two frames based on the distance metric
//                  defined by Ahuactizin and Gupta (Aug 1999).
//
// NOTE:    This is a computationally expensive function because of all the matrix
//                      and vector multiplication
//=============================================================================
double IK_ACA::FrameDistance ( const Matrix4x4& frameA, const Matrix4x4& frameB ) const
{
#ifdef _USE_ORIENTATION_
        // Generate vectors from base frame to each of the unit vector of the given frames.
        Vector4 Pax = frameA * PX;
        Vector4 Pay = frameA * PY;
        Vector4 Paz = frameA * PZ;

        Vector4 Pbx = frameB * PX;
        Vector4 Pby = frameB * PY;
        Vector4 Pbz = frameB * 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();
#else
        Vector4 offset1, offset2;
        offset1[0] = frameA(0, 3);
        offset1[1] = frameA(1, 3);
        offset1[2] = frameA(2, 3);

        offset2[0] = frameB(0, 3);
        offset2[1] = frameB(1, 3);
        offset2[2] = frameB(2, 3);

        offset1 -= offset2;

        double distance_sq = offset1.MagSquared();
#endif

        //return the squared distance.
        return distance_sq;

}
          
//=============================================================================
// GetEmbryosPerLandmark
//
// Purpose: Returns the number of embryos associated with each landmark
//=============================================================================
int  IK_ACA::GetEmbryosPerLandmark() const
{
        return embryosPerLandmark;
}

//=============================================================================
// SetEmbryosPerLandmark
//
// Purpose: Adjusts the number of embryos associated with each landmark and
//                      adds or deletes embryos as required.
//
// Note:    This function will create new node_maps and copy the contents of the
//                      existing ones before deleting them.
//
// Note:        THIS FUNCTION SHOULD BE THE ONLY WAY THE EMBRYO COUNT IS UPDATED!
//=============================================================================
void IK_ACA::SetEmbryosPerLandmark( const int& num_of_embryos )
{
        if ( num_of_embryos == embryosPerLandmark )
        {
                // no change...return early.
                return;
        }

        // set pointer to existing node maps
        node_map<Configuration>* old_embryo = embryo;
        assert( old_embryo != NULL );

        // update embryo counters
        int old_embryosPerLandmark = embryosPerLandmark;
        if ( num_of_embryos < 1 )
        { 
                embryosPerLandmark = 1;
        }
        else
        {
                embryosPerLandmark = num_of_embryos;
        }


        // create new node map structures for embryos and initialize
        embryo = new node_map<Configuration>[embryosPerLandmark];
        assert( embryo != NULL );
        for ( int i = 0; i < embryosPerLandmark; i++ )
        {
                embryo[i].init(G);
        }

        // copy existing embyros, adding new ones when required
        node n1;
        forall_nodes( n1, G )
        {
                for( int i = 0; i < embryosPerLandmark; i++ )
                {
                        if ( i < old_embryosPerLandmark )
                        {
                                // copy embryo from existing node map
                                embryo[i][n1] = old_embryo[i][n1];
                        }
                        else
                        {
                                // a previous embryo does not exist, add a new one.
                                CreateEmbryo( n1, i );
                        }
                }
        }

        // delete the old embryos
        delete [] old_embryo;

}

//=============================================================================
// InitNewSearch
//
// Purpose: Initiates parameters to indicate if a new search has been requested.
//=============================================================================
void IK_ACA::InitNewSearch()
{
        // new search, start with the first node in the tree, the start node.
        searchLandmark = startNode;

        // Since object of planner is to find a goal config, tempoary assign the start config as the goal config
        // so it is at least defined.
        SetGoalConfig( GetStartConfig() );

        // Reset the best parameters
        InitBestParams();

}

//=============================================================================
// InitBestParams
//
// Purpose: Initiates parameters used to indicate the best config and path so far
//=============================================================================
void IK_ACA::InitBestParams()
{
        bestLandmark = startNode;
        bestPath.clear();

        if ( collisionDetector != NULL )
        {
                bestDistance_sq = FrameDistance( goalFrame, GetToolFrame( GetGoalConfig() ) );
        }
        else
        {
                bestDistance_sq = INFINITY;
        }
}

//=============================================================================
// ComputePath
//
// Purpose: Computes the final path from the tree and search path to the actual 
//                      or best goal config
//=============================================================================
SuccessResultType IK_ACA::ComputePath ( const node& landmark, const list<Configuration>& myPath )
{
        
        // Trace path from last landmark used to start config.
        ClearGraphPath();
        node n0 = landmark;
        graphPath.push( n0 );
        while ( n0 != startNode )
        {
                n0 = G.target( G.first_adj_edge( n0 ) );
                graphPath.push( n0 );
        }
                
        // Translate the graph path to the system path
        SuccessResultType success = TranslatePath();

        // Append the myPath to the system path
        list_item last = myPath.last();
        list_item index = myPath.first();
        while ( index != last )
        {
                index = myPath.succ( index );
                path.AppendPoint( myPath.inf(index) );
        }

        // Assign the solved (best) goal configuration
        AssignGoalConfig( myPath.inf( index ) );

        return success;

}

---