planners/inversekin/Joints.cpp

Go to the documentation of this file.

//
//Contents: Class CJoints
// 
//Relations:
//      CJoints --> CActive  ---o CRedundant
//            |---> CPassive -|
//
//      Class CActive/CPassive is derived from class CJoints
//      Class CRedundant constains entities of CActive and CPassive
//
//CJoints is pure virtual class, it represents a chain of joint. 
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Mar-2003:  Z.Yao     First version implemented
//                          
//Others:
//     a.[Mar-2003]

#include "kinematics/linkbase.h"
#include <math.h>
#include "math\math2.h"
#include "Joints.h"

//------------------------------------------------------------------
//
//Construction and Deconstruction function
//
//------------------------------------------------------------------
CJoints::CJoints()
{
        baseFrame = firstJoint = lastJoint = jointNum = 0;
}

CJoints::CJoints(FrameManager* frameManager)
                :frameManager(frameManager)
{
        baseFrame = firstJoint = lastJoint = jointNum = 0;
}

CJoints::~CJoints()
{}

//------------------------------------------------------------------
//This function is used to compute the distance between two
//joint variables.
//
//Input Parameter:
//      q1/q2: two joint variables
//      link: corresponding robot link
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      distance in double precise;
//------------------------------------------------------------------
double CJoints::Distance(LinkBase *link, double q1, double q2)
{
        double dist = 0 ;
        double jmax = link->JointMax() ;
        double jmin = link->JointMin() ;
        double dist1 = fabs( q2 - q1 ) ;
        if(link->JointWraps() )
        {
                double dist2 = fabs( jmax - q1 ) + fabs( q2 - jmin ) ;
                double dist3 = fabs( jmax - q2 ) + fabs( q1 - jmin ) ;
                dist = Min( dist1, dist2, dist3 ) ;             //IMPROVE: this is a rapid funciton
        }
        else
        {
                dist = dist1 ;
        }
        return dist;
}

//------------------------------------------------------------------
//
//Set the robot in the environment
//
//------------------------------------------------------------------
void CJoints::SetRobot(R_OpenChain *robot)
{
        this->robot =robot;
        std::list< LinkBase* > allLinks = robot->GetAllLinks() ;
        for( std::list< LinkBase* >::iterator i = allLinks.begin(); i != allLinks.end(); i++ )
        {
                LinkBase* link = ( *i )->Clone();
                links.push_back( link ) ;
        }
}

//------------------------------------------------------------------
//
//Set Frame Manager
//
//------------------------------------------------------------------
void CJoints::SetFrameManager(FrameManager* frameManager)
{
        this->frameManager = frameManager;
}

//------------------------------------------------------------------
//
//Set base frame of this joint chain
//
//------------------------------------------------------------------
void CJoints::SetBaseFrame(unsigned int frame)
{
        baseFrame = frame;
}

//------------------------------------------------------------------
//
//Set the first joint in the joint chain
//
//------------------------------------------------------------------
void CJoints::SetFirstJoint(unsigned int joint)
{
        firstJoint = joint;
}

//------------------------------------------------------------------
//
//Set the last joint in the joint chain
//
//------------------------------------------------------------------
void CJoints::SetLastJoint(unsigned int joint)
{
        lastJoint = joint;
        jointNum = lastJoint-firstJoint+1;
}

---