planners/ik_mpep/IK_Jacobian.cpp

Go to the documentation of this file.

//=================================
// IK_Jacobian.cpp
// Implementation file for planner
//=================================

#include "synchronization/semaphore.h"
#include "Kinematics\DH_Link.h"
#include "Kinematics\Jacobian.h"
#include "assert.h"
#include <math/matrix4x4.h>
#include <math/Matrixmxn.h>
#include <math/vector4.h>
#include "CollisionDetectors\CD_Swiftpp.h"
#include "IK_Jacobian.h"

#if 1
        #define LogMatrix(a, b, c) 
        #define LogVector(a, b, c)
        #define LogMessage(a, b)
#endif

static const int FRAMEDOF = 6;
static const int SAMPLING_PERIOD = 1;

static const double INFINITY = LONG_MAX; 
static const double DEFAULT_OBS_TOL = 0.01;     
        // distance tolerance to obstacles
static const double DEFAULT_PATH_TOL = 0.5;
        // tolerance of deviation of end-effector from given path
static const double DEFAULT_ANG_TOL = 2;
        // tolerance of deviation of end-effector orienation
static const double DEFAULT_VEL = 1; 
        // default end effector velocity in any direction
static const double DEFAULT_OBS_GAIN = 0.1;
        // default obstacle point velocity ratio to inverse distance away from obstacle
static const double DEFAULT_REL_IMP = 1;  
        // relative importance of obstacle avoidance to following end-effector path
        // 1 = equal importance, < 1 = less important than following path
static const double MAX_OBS_VEL = 0.5;         // maximum obstacle point velocity
static const double MAX_JOINT_VEL = (M_PI/10.);       // maximum joint velocity (degrees)
static const double CALCULATION_FREQ = 100;     // per second
static const double UNITY_GAIN_DISTANCE = 0.5;    // arbitrary
static const double SOI_GAIN_DISTANCE   = 1.5;    // distance where the obstacle no longer matters
static const double DEFAULT_HOMO_GAIN = 0.25;

//Construction
IK_Jacobian::IK_Jacobian()
{
        if (collisionDetector != NULL)
        {
                m_nDof = collisionDetector->DOF();
        }
        else
        {
                m_nDof = FRAMEDOF;
        }

        //The following parameter could be set by users
    m_nObsPoint = 2;
    m_dObsTol = DEFAULT_OBS_TOL;
    m_dObsDistGain = DEFAULT_OBS_GAIN;
    m_dHomoGain = DEFAULT_HOMO_GAIN;
    m_dPathTol = DEFAULT_PATH_TOL;
    m_dAngTol = DEFAULT_ANG_TOL;

    path.Clear();
        m_vClosestPoints.clear(); 
}

//Deconstruction
IK_Jacobian::~IK_Jacobian()
{
        path.Clear();
        m_vClosestPoints.clear(); 
    //m_links.clear();  
}

//------------------------------------------------------------------
//SetCollisionDetector
//
//This function is derived from its base class, and update collision detector in planner.
//
//------------------------------------------------------------------
void IK_Jacobian::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
    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));
        }
}

//------------------------------------------------------------------
//ComputeEndEffVel
//
//This function is used to compute the end-effector velocity along the given trajectory.
//
//Input Parameter:
//      frStart: start point. 
//      frEnd  : end point. 
//
//Ouput Parameter:
//      N.A.
//
//Return Value:
//      end-effector velocity. The dimension of the velocity depends on whether we 
//        position or orientation. If both are considered, the dimension=6, otherwise 3.
//      
//------------------------------------------------------------------
VectorN IK_Jacobian::ComputeEndEffVel(Frame &frStart, Frame &frEnd)
{
        VectorN returnMe;

        if (m_bOrientation && m_bPosition)
                returnMe.SetLength(6);
        else if (m_bOrientation || m_bPosition)
                returnMe.SetLength(3);
        else
        {
                assert(false);
                return returnMe;
        }

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

    //Compute angular velocity
    //Angular velocity = (angle rotated)*(Unit vector of rotation axis)
        if (m_bOrientation)
        {
                //compute equivalent rotation matrix, w.r.t fixed coordinate
                Frame rotated = frEnd * frStart.Inverse();
        
        //compute axis and angle rotated.
                double temp = rotated.values[0][0] + rotated.values[1][1] + rotated.values[2][2] - 1;
                assert(fabs(temp) <= (2+ROUNDING_ERROR));
                if (temp>2.0)
                        temp = 2.0;
                else if (temp<-2.0)
                        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;        
}

//------------------------------------------------------------------
//ComputeHomogeneousTerm
//
//This function is used to compute homogeneous solution.
//  \sum_{i=1}^{M}\alpha_i[J_o(I-J^{+}_eJ_e)]^{+}(\dot{x}_o-J_oJ^{+}_e\dot{x}_e)
//where $M$ is number of obstacles points to be considered,
//      $J_o$       : Jacobian matrix of $i^th$ obstacle point;
//      $J_e$       : End-effector Jacobian matrix;
//      $J^{+}_e$   : pseudo-inverse of $J_e$;
//      $\dot{x}_o$ : Velocity to escape from $i^th$ obstacle point;
//      $\dot{x}_e$ : End-effector velocity;
//
//Input Parameter:
//      jacobian  : Jacobian class, by which the Jacobian matrices of obstacle points will be computed. 
//      mJEnd     : End-effector Jacobian Matrix. ($J_e$ in above equation)
//      mJEndInv  : pseudo-inverse of End-effector Jacobian Matrix. ($J^{+}_e$ in above equation)
//      vEndEffVel: End-effector velocity. ($\dot{x}_e$ in above equation)
//
//Ouput Parameter:
//      N.A.
//
//Return Value:
//      joint velocity component of the homogeneous solution.
//      
//------------------------------------------------------------------
VectorN IK_Jacobian::ComputeHomogeneousTerm(CJacobian &jacobian, Matrixmxn &mJEnd, Matrixmxn &mJEndInv, VectorN &vEndEffVel)
{
    assert(m_vClosestPoints.size() == m_nObsPoint);

        VectorN vHomogeneousSolns;
        VectorN vSingleTerm(vHomogeneousSolns);

        vHomogeneousSolns.SetLength(m_nDof);
    for (int j = 0; j < m_nObsPoint; j++)
    {
                //Get frame of obstacle point with respect to previous link;
                int robFrame;
                Frame worldTObsFrame;     // frame of joing $robFrame$ w.r.t universe frame
                Frame frObsPtsInLink;     // frame of obstacle-point w.r.t joint $robFrame$
        Frame frObsPtsInLinkWorld;// frame of obstacle-point w.r.t universe frame
                VectorN vObsPtsInLink(0, 0, 0); // The position of closest point in joint

        //Determine the joint where current obstacle point lies.
        robFrame = m_vClosestPoints[j].robFrame;

        //Compute the universe frame of the obstacle-point in joint;
        //  entry: vObsPtsInLink[] w.r.t joint $robFrame$
        //  exit : vObsPtsInLink[] w.r.t universe frame
                worldTObsFrame = jacobian.GetJointWorldFrame(robFrame-1);       
        vObsPtsInLink[0] = m_vClosestPoints[j].rob_pts[0];  // point on robot arm closest to obstacle wrt link
        vObsPtsInLink[1] = m_vClosestPoints[j].rob_pts[1];
        vObsPtsInLink[2] = m_vClosestPoints[j].rob_pts[2]; 
                Vector4 vTranslate(vObsPtsInLink[0], vObsPtsInLink[1], vObsPtsInLink[2]);
                frObsPtsInLink.Translate(vTranslate);           
        frObsPtsInLinkWorld = worldTObsFrame * frObsPtsInLink;
                vTranslate = frObsPtsInLinkWorld.GetTranslationVector();
                vObsPtsInLink[0] = vTranslate[0];
                vObsPtsInLink[1] = vTranslate[1];
                vObsPtsInLink[2] = vTranslate[2];

                // The position of closest point in obstacle, it's wrt frame 0, universe frame
        VectorN vObsPtWorld(m_vClosestPoints[j].obs_pts[0],m_vClosestPoints[j].obs_pts[1], m_vClosestPoints[j].obs_pts[2]);
                //Set the normalized escape velocity
                VectorN vObsEscapeVel = vObsPtsInLink - vObsPtWorld;
                vObsEscapeVel /= vObsEscapeVel.Magnitude();

                //Compute Jacobian matrix with respect to obstacle point
        //Set jacobian to be interested in obstacle point;
                Matrixmxn mJObs, mJObsInv;              //Jacobian and its inverse of Obstacle point;
                if (robFrame == m_nDof)
                {
                        frObsPtsInLink = Matrix4x4::Identity();
                        frObsPtsInLink.Translate(jacobian.GetJointFrame(robFrame-1).GetTranslationVector());
                        jacobian.SetInterestPoint(robFrame-2, frObsPtsInLink, true, false);
                }
                else
                {
                        jacobian.SetInterestPoint(robFrame-1, frObsPtsInLink, true, false);
                }
                mJObs = *(jacobian.GetMatrix());
                mJObs.Inverse(mJObsInv);
                LogMatrix("ikJacobian.log", mJObs, "Jacobian Matrix for Obstale point: ");
                LogMatrix("ikJacobian.log", mJObsInv, "Inverse of Jobs: ");

                Matrixmxn mTemp(m_nDof, m_nDof);   //Identity
                Matrixmxn mTemp2(mTemp);           //Identity
                Matrixmxn mTemp3(mTemp);           //Identity           
                Matrixmxn mTemp4(mTemp);           //Identity

                mTemp -= mJEndInv * mJEnd;         //$I - J^{+}_e J_e$
                LogMatrix("ikJacobian.log", mTemp, "Temp: I-Je^+ Je");
                mTemp2 -= mJObsInv * mJObs;         //$I - J^{+}_o J_o$
                LogMatrix("ikJacobian.log", mTemp2, "Temp2: I-Jo^+ Jo");
                mTemp3 = mJObs * mTemp;             //$J_o (I - J^{+}_e J_e)$
                LogMatrix("ikJacobian.log", mTemp3, "Temp3: Jo (I-Je^+ Je)");
                mTemp3 = mTemp3.Inverse();          //$[J_o (I - J^{+}_e J_e)]^{+}$
                LogMatrix("ikJacobian.log", mTemp3, "Inverse of Temp3:");
                mTemp4 = mJObs * mJEndInv;          //J_o J^{+}_e
                LogMatrix("ikJacobian.log", mTemp3, "Temp4: Jo Je^+");

                double solnGain; //applied to every term of homogeneous solution
                double velcGain; //used to determine escape velocity, $x_o$
        if (m_vClosestPoints[j].distance < UNITY_GAIN_DISTANCE)
        { 
            solnGain = m_dHomoGain;
        }
        else if (m_vClosestPoints[j].distance > SOI_GAIN_DISTANCE)
        {
            solnGain = 0;
        }
        else
        {
            solnGain = m_dHomoGain*m_vClosestPoints[m_nObsPoint-1-j].distance/m_dTotalObsDist;
        }

        if ((m_dObsDistGain/(m_vClosestPoints[j].distance)) > MAX_OBS_VEL)
        {
            velcGain = MAX_OBS_VEL;
        }
        else
        {
            velcGain = m_dObsDistGain / (m_vClosestPoints[j].distance);
        }

                mTemp3 *= solnGain;    //$\alpha_i[J_o(I-J^{+}_eJ_e)]^{+}$
                VectorN vTemp = vObsEscapeVel * velcGain; //$\dot{x}_o$ is related to distance 
                vTemp -= mTemp4 * vEndEffVel;  //$\dot{x}_o-J_oJ^{+}_e\dot{x}_e$
                vSingleTerm = mTemp3 * vTemp;  //$\alpha_i[J_o(I-J^{+}_eJ_e)]^{+}(\dot{x}_o-J_oJ^{+}_e\dot{x}_e)$
                
                vHomogeneousSolns += vSingleTerm; //Sum them up

                LogVector("ikJacobian.log", vSingleTerm, "Single Term:");
    }   // got all homogeneous solutions

    return vHomogeneousSolns;
}

bool IK_Jacobian::Plan()
{
        // Start the timer
        StartTimer();

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

    Configuration   currentConfig, nextConfig, jointVelocity;
    currentConfig.SetNumDOF(m_nDof);
    nextConfig.SetNumDOF(m_nDof);
    jointVelocity.SetNumDOF(m_nDof);
    jointVelocity *= 0;     //Clear
    currentConfig = GetStartConfig();
    
    path.Clear();
    path.AppendPoint(currentConfig);

        //Initialize Jacobian, obstacle point and end-effector will share a Jacobian matrix.
        CJacobian jacobian(*m_pRobot);
        jacobian.SetConfiguration(currentConfig);

        bool abort = false;
    for (int iViaPts = 0; iViaPts < (poses.size()-1); iViaPts++)
    {
                //Compute endeffector Linear & Angular velocity;
                VectorN vEndEffVel;
                //Since the start configuration has been applied to jacobian, we get pose under startconfig.
                Frame frCurrentEndEff = jacobian.GetJointWorldFrame(m_nDof-1)*m_frEndEffector;

                double dist = Distance(frCurrentEndEff, poses[iViaPts]);
                if (dist > m_dPathTol)
                {
                        LogMessage("ikJacobian.log", "Aborting.... Motion too large!");
                        abort = true;
                        break;
                }
                if (m_bOrientation)
                {
                        double x1, y1, z1, yaw1, roll1, pitch1;
                        double x2, y2, z2, yaw2, roll2, pitch2;
                        GetRotAngles(frCurrentEndEff, x1, y1, z1, roll1, pitch1, yaw1);
                        GetRotAngles(poses[iViaPts], x2, y2, z2, roll2, pitch2, yaw2);
                        if ( (fabs(roll1-roll2)>m_dAngTol) || (fabs(pitch1-pitch2)>m_dAngTol) || (fabs(yaw1-yaw2)>m_dAngTol) )
                        {
                                LogMessage("ikJacobian.log", "Aborting.... Motion too large!");
                                abort = true;
                                break;
                        }
                }

        //nothing but logging
                Matrix4x4 logmat;
                Matrixmxn logmatmn;
                logmat = poses[iViaPts];
                logmatmn = logmat;
                LogMatrix("ikJacobian.log", logmatmn , "Expected pose in Trajectory:");
                logmat = frCurrentEndEff;
                logmatmn = logmat;
                LogMatrix("ikJacobian.log", logmatmn, "Actually Pose got:");
                logmat = jacobian.GetJointWorldFrame(m_nDof-1)*m_frEndEffector;
                logmatmn = logmat;
                LogMatrix("ikJacobian.log", logmatmn, "End-effecot Frame:");

        //Compute next end-effector velocity
                vEndEffVel = ComputeEndEffVel(frCurrentEndEff, poses[iViaPts+1]);
                LogVector("ikJacobian.log", vEndEffVel, "Endeffector Velocity");

        //Compute end-effector Jacobian matrix, and its inverse
                jacobian.SetInterestPoint(m_nDof-1, m_frEndEffector, m_bPosition, m_bOrientation);
                jacobian.GetJointVelocity(jointVelocity, vEndEffVel);
                Matrixmxn mJEnd, mJEndInv;
                mJEnd = *(jacobian.GetMatrix());
                mJEnd.Inverse(mJEndInv);
                LogMatrix("ikJacobian.log", mJEnd, "Jacobian matrix of end-effector:");
                LogMatrix("ikJacobian.log", mJEndInv, "Jacobian matrix Inverse:");

                //============= 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("ikJacobian.log", "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("ikJacobian.log", "Aborting.... Colliding, too close to obstacles!");
            abort = true;
            break;
        }            
        //** no obstacle scenario handled by infinite distance
        else
        {
            //Compute homogeneous solution
                        VectorN vHomogeneousSolns;
                        vHomogeneousSolns = ComputeHomogeneousTerm(jacobian, mJEnd, mJEndInv, vEndEffVel);
                        LogVector("ikJacobian.log", vHomogeneousSolns, "Homogeneous Term:");
                        
            //Compute general solution
                        jointVelocity += vHomogeneousSolns;
        } // obstacles exist
                //============= End of obstacles =====================

                //Convert to degree representation;
        CheckJointVelocities(jointVelocity);
                for (int i=0; i<m_nDof; i++)
                {
                        double dValue = jointVelocity[i];
                        jointVelocity[i] = Rad2Deg(dValue);
                }
                LogVector("ikJacobian.log", jointVelocity, "Joint Velocity");        
                
        // calculate where the new velocity takes us after time interval
                nextConfig = currentConfig + jointVelocity;
        CheckJointLimits(nextConfig);
                LogVector("ikJacobian.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);
                // GetEndEffVector(currentConfig, vCurPt);
            }
            jacobian.SetConfiguration(currentConfig);
        }
        else        // interfering
        {
            //MessageBox(NULL, "Obstacle in path could not be avoided","IK_Jacobian Planner Error", MB_OK);
                        LogMessage("ikJacobian.log", "Aborting.... Colliding when checking next configuration!");
            abort = true;
                        break;
        }            

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

    //semaphore.Unlock();
        if (abort)
        {
                LogMessage("ikJacobian.log", "### Planning Failure");
        }
        else
        {
                LogMessage("ikJacobian.log", "### Planning Success");
        }
    return (abort == false);
}


void IK_Jacobian::InitNewSearch()
{
}
    
void IK_Jacobian::SetObstacleTolerance(double tol)
{
    m_dObsTol = tol;
    ((CD_Swiftpp*) collisionDetector)->SetObstacleTolerance(tol);
}


//------------------------------------------------------------------
//GetClosestValues
//
//This function is used to finds the closest $m_nObsPoint number$ of points and their distances.
//
//Input Parameter:
//      config: Given a configuration.
//
//Ouput Parameter:
//      N.A.
//
//Return Value:
//      true if collision detected; false otherwise.
//
//Note:
//      Refer to SWIFT++ for more parameter information.
//      
//------------------------------------------------------------------
bool IK_Jacobian::GetClosestValues(const Configuration& config)
{    
    double  *query_d, *query_npts;
    int     iLink1, iLink2, query_np, *query_oids, *query_num_contacts, 
            index = -1;

    m_vClosestPoints.clear();
    closeValues init;
    init.distance = INFINITY;
    m_vClosestPoints.push_back(init);

    bool collision = ((CD_Swiftpp*) collisionDetector)->QueryContactDetermination( config, 
        true, INFINITY, query_np, &query_oids, &query_num_contacts, &query_d, &query_npts);

    if (collision != false) 
        {
                return true;
        }

    int j, k, min;    // temp variables for indexing

    //** start here: figure out what to do with no obstacles because query_np may not be 0 without obstacles
    //** also check why query is reported for b/t robot links

    for (int i = 0; i < query_np; i++)
    {
        index += query_num_contacts[i];
        if (m_vClosestPoints.size() >= m_nObsPoint)
        {
            if (query_d[index] > m_vClosestPoints[m_nObsPoint-1].distance)  
                        {
                                continue;
                        }
            // if the shortest distances is not short enough to replace any current distances
        }

        // make sure links are valid:
        iLink1 = ((CD_Swiftpp*) collisionDetector)->GetRobotLinkBaseFrame(query_oids[2*i]);
        iLink2 = ((CD_Swiftpp*) collisionDetector)->GetRobotLinkBaseFrame(query_oids[2*i+1]);
        if ((iLink1 == -1) || (iLink2 == -1))  
                {
                        return false;
                }
        if (((iLink1 == 0)^(iLink2 == 0)) == false)  //between obstacles or between robot joints
                {
                        continue;
                }

        // testing begins here:
        min = min(query_num_contacts[i], m_nObsPoint);
        k = 0;
        for (j = 0; j < min; j++)
        {
            while (k < m_nObsPoint)
            {
                if(m_vClosestPoints[k].distance > query_d[index-j]) // need insertion
                {
                    closeValues newEntry;
                    newEntry.distance = query_d[index-j];
                    if (iLink1 == 0)
                    {
                        newEntry.obs_pts[0] = query_npts[6*index];
                        newEntry.obs_pts[1] = query_npts[6*index+1];
                        newEntry.obs_pts[2] = query_npts[6*index+2];
                        newEntry.robFrame = iLink2;
                        newEntry.rob_pts[0] = query_npts[6*index+3];
                        newEntry.rob_pts[1] = query_npts[6*index+4];
                        newEntry.rob_pts[2] = query_npts[6*index+5];
                    }
                    else // iLink2 == 0
                    {
                        newEntry.obs_pts[0] = query_npts[6*index+3];
                        newEntry.obs_pts[1] = query_npts[6*index+4];
                        newEntry.obs_pts[2] = query_npts[6*index+5];
                        newEntry.robFrame = iLink1;
                        newEntry.rob_pts[0] = query_npts[6*index];
                        newEntry.rob_pts[1] = query_npts[6*index+1];
                        newEntry.rob_pts[2] = query_npts[6*index+2];
                    }
                    m_vClosestPoints.insert(m_vClosestPoints.begin()+k, newEntry);                    
                    k++;
                    break;
                }
                else if(&m_vClosestPoints[k] == m_vClosestPoints.end())  // at the end
                {
                    closeValues newEntry;
                    newEntry.distance = query_d[index-j];
                    if (iLink1 == 0)
                    {
                        newEntry.obs_pts[0] = query_npts[6*index];
                        newEntry.obs_pts[1] = query_npts[6*index+1];
                        newEntry.obs_pts[2] = query_npts[6*index+2];
                        newEntry.robFrame = iLink2;
                        newEntry.rob_pts[0] = query_npts[6*index+3];
                        newEntry.rob_pts[1] = query_npts[6*index+4];
                        newEntry.rob_pts[2] = query_npts[6*index+5];
                    }
                    else // iLink2 == 0
                    {
                        newEntry.obs_pts[0] = query_npts[6*index+3];
                        newEntry.obs_pts[1] = query_npts[6*index+4];
                        newEntry.obs_pts[2] = query_npts[6*index+5];
                        newEntry.robFrame = iLink1;
                        newEntry.rob_pts[0] = query_npts[6*index];
                        newEntry.rob_pts[1] = query_npts[6*index+1];
                        newEntry.rob_pts[2] = query_npts[6*index+2];
                    }
                    m_vClosestPoints.push_back(newEntry);
                    k++;
                    break;
                }
                k++;
            }
            if (k >= m_nObsPoint) {break;}
        }
    }
    if ((m_vClosestPoints[0].distance == INFINITY)||(query_np==0))    // no obstacles
    {   // setting everything to infinity essentially makes it useless
        closeValues defaults;
        defaults.distance = INFINITY;
        defaults.robFrame = 1;
        defaults.obs_pts[0] = INFINITY;
        defaults.obs_pts[1] = INFINITY;
        defaults.obs_pts[2] = INFINITY;
        defaults.rob_pts[0] = 1;
        defaults.rob_pts[1] = 0;
        defaults.rob_pts[2] = 0;

        m_vClosestPoints.clear();
        for (i = 0; i < m_nObsPoint; i++)
        {
            m_vClosestPoints.push_back(defaults);
        }
             
    }
    else if (query_np > 0)  
    {
        m_vClosestPoints.erase(m_vClosestPoints.begin() + m_nObsPoint, m_vClosestPoints.end());
        m_dTotalObsDist = 0;
        for (i = 0; i < m_vClosestPoints.size(); i++)
        {
            m_dTotalObsDist += m_vClosestPoints[i].distance;
        }
    }
    else                        // negative should never happen, so return as a collision
    {
        return true;
    }

    return false;
}

void IK_Jacobian::CheckJointLimits(Configuration& config)
{
        for (int i = 0; i<m_nDof; i++)
        {
                if ( config[i] < collisionDetector->JointMin(i) )
                {
                        config[i] = collisionDetector->JointMin(i);
                }
                else if ( config[i] > collisionDetector->JointMax(i) )
                {
                        config[i] = collisionDetector->JointMax(i);
                }
        }
}

void IK_Jacobian::CheckJointVelocities(Configuration& config)
{
    for (int i = 0; i < m_nDof; i++)
    {
        if (config[i] < -MAX_JOINT_VEL)
        {
            config[i] = -MAX_JOINT_VEL;
        }
        else if (config[i] > MAX_JOINT_VEL)
        {
            config[i] = MAX_JOINT_VEL;
        }
        
    }
}

/*
Matrix4x4 IK_Jacobian::GetToolFrame( const Configuration& config ) const
{

        if ( (collisionDetector == NULL) || (m_nToolFrame == -1) )
        {
                return IK_InvKinBase::GetToolFrame(config);
        }

        Frame toolFrame = Matrix4x4();

        if ( collisionDetector != NULL )
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                collisionDetector->SetConfiguration( config );
                toolFrame = *(frameManager->GetFrameRef(m_nToolFrame));
                toolFrame = frameManager->GetTransformRelative(collisionDetector->DOF(), 0) * toolFrame;
        }
        return toolFrame;
}
*/

void IK_Jacobian::SetTrajectory(std::vector<Frame> &traj)
{
        poses.clear();
        for (int i=0; i<traj.size(); i++)
        {
                poses.push_back(traj[i]);

                char szMsg[100];
                Matrixmxn matLog = traj[i];
                sprintf(szMsg, "%d(th) pose:\n", i);
                LogMatrix("ikJacobian.log", matLog, szMsg);
        }

}


double IK_Jacobian::Distance(Frame &frame1, Frame &frame2)
{
        Vector4 vec1 = frame1.GetTranslationVector();
        Vector4 vec2 = frame2.GetTranslationVector();
        vec1 -= vec2;
        return vec1.Magnitude();
}

---