planners/inversekin/Active.cpp

Go to the documentation of this file.

//
//Contents: Class CActive
// 
//Relations:
//      CJoints --> CActive  ---o CRedundant
//            |---> CPassive -|
//
//      Class CActive/CPassive is derived from class CJoints
//      Class CRedundant constains entities of CActive and CPassive
//
//CActive is used for active joint chain. 
//The typical calling sequence is:
//      1.CActive::CActive(...)
//      2.CActive::SetFrameManager(...)
//        CActive::SetRobot(...)
//        CActive::SetFirstJoint(...)
//        CActive::SetLastJoint(...)
//      3.CActive::SetFirstFrame(...)
//        CActive::SetEndEffectFrame(...)
//      4.CActive::SetDesireEndEffect(...)
//      5.CActive::GetConfiguration(...)
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Mar-2003:  Z.Yao     First version implemented
//                          
//Others:
//     a.[Mar-2003]

#include "Active.h"
#include "kinematics/dh_link.h"
#include "math/math2.h"

#define MAX_ACTIVE_RETRY    100

//------------------------------------------------------------------
//
//Construction and Deconstruction function
//
//------------------------------------------------------------------
CActive::CActive():CJoints()
{}

CActive::CActive(FrameManager* frameManager)
                :CJoints(frameManager)
{}

CActive::~CActive()
{}

//------------------------------------------------------------------
//This function is used to get active joint variables randomly.
//
//Input Parameter:
//      N.A
//
//Ouput Parameter:
//      conf: A random configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CActive::GetConfiguration(Configuration &conf)
{
        conf.SetLength(jointNum);

        if (jointNum==0)
                return true;

#ifdef _DEBUG

        if ((lastJoint-firstJoint)!=(jointNum-1))
                return false;
#endif

        for( int i = 0; i < jointNum; i++ )
        {
                double max = links[firstJoint+i]->JointMax() ;
                double min = links[firstJoint+i]->JointMin() ;
                double random = 2*( double( rand() ) / RAND_MAX ) - 1; //this is a random number between [-1..1]
                double jointvalue = ( max - min ) * random;

                // Create a uniform distribution by having the random number go over the limits have wrapping the C-space;
                if ( jointvalue < min )
                {
                        jointvalue += (max - min);
                }
                else if ( jointvalue > max )
                {
                        jointvalue -= (max - min);
                }

                conf[ i ] = jointvalue;
        }

        return true;
}

//------------------------------------------------------------------
//This function is used to get active joint variables
//in a neighbour area of current configuration;
//
//Input Parameter:
//      current: Given bias configuration
//      dist: Deviation allowed for each joint variables
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CActive::GetConfiguration(Configuration &conf, Configuration &current, double dist)
{
        /*
          bool bFind;
          int i;
          int retry = 0;
          Configuration randConf;
         
          do 
          {
            bFind = true;
            GetConfiguration(randConf);
            for (i=0; i<jointNum; i++)
            {
              if (Distance(links[firstJoint+i], randConf[i], current[i])>dist)
              {  
                bFind = false;
                break;
              }
            }
            if (bFind)
            {
              for (i=0; i<jointNum; i++)
                conf[i] = randConf[i];
              break;
            }
            retry ++;
          } while ((!bFind) && (retry<MAX_ACTIVE_RETRY));
         
          return bFind;
        */

        conf.SetLength(jointNum);

        if (jointNum==0)
                return true;

#ifdef _DEBUG

        if ((lastJoint-firstJoint)!=(jointNum-1))
                return false;
#endif

        for( int i=0; i < jointNum; i++ )
        {
                double random = 2*( double( rand() ) / RAND_MAX ) - 1; //this is a random number between [-1..1]
                double jmax = links[firstJoint+i]->JointMax();
                double jmin = links[firstJoint+i]->JointMin();

                double jointvalue = current[i] + random*dist;
                // Use wrapping of the C-space (for all joints) to ensure normal distribution about seed.

                if ( jointvalue > jmax )
                {
                        //jointvalue -= ( jmax - jmin );
                        jointvalue = current[i] + random*(jmax - current[i]);
                }
                else if ( jointvalue < jmin )
                {
                        //jointvalue += ( jmax - jmin );
                        jointvalue = current[i] + random*(current[i]-jmin);
                }

                conf[i] = jointvalue;
        }

        return true;
}

---