planners/inversekin/Redundant.cpp

Go to the documentation of this file.

//
//Contents: Class CRedundant
// 
//Relations:
//      CJoints --> CActive  ---o CRedundant
//            |---> CPassive -|
//
//      Class CActive/CPassive is derived from class CJoints
//      Class CRedundant constains entities of CActive and CPassive
//
//CRedundant is used to do configuration generation for redundant robots.
//It can generate a configuration randomly, or generate a configuration 
//to statisfy a end-effector certain pose. For the latter case, generally, 
// it partition robot into active joints and passive joint, while the 
//active joint variables are randomly generated and the passive joint 
//variables are computed by pose of active joints and required end-effector pose.
//
//The basic idea is similar to the following paper, for more information, 
//please refer to that.
//
//      L.Han and N.Amato. A kinematics-based probabilistic roadmap method
//      for closed kinematic chains. Workshop on Algorithmic Foundations of
//      Robotics, 2000.
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Mar-2003:  Z.Yao     First version implemented
//
//Other Notes:
//     a. [Mar-2003]: We actually assume only one active chain and 
//        one passive joint chain for every redundant robot. 
//        Each chain might contain a certain number of joints. 
//

#include "math\math2.h"
#include "Redundant.h"
#include "robots\r_openchain.h"
#include <windows.h>

//------------------------------------------------------------------
//
//Construction and Deconstruction function
//
//------------------------------------------------------------------
CRedundant::CRedundant()
{}

CRedundant::CRedundant(FrameManager *frameManager)
                :passive(frameManager),
                active(frameManager),
                frameManager(frameManager)
{}

CRedundant::~CRedundant()
{}

//------------------------------------------------------------------
//
//Set Frame Manager
//
//------------------------------------------------------------------
void CRedundant::SetFrameManager(FrameManager* frameManager)
{
        this->frameManager = frameManager;
        active.SetFrameManager(frameManager);
        passive.SetFrameManager(frameManager);
}

//------------------------------------------------------------------
//
//Set base frame of passive joints
//
//------------------------------------------------------------------
void CRedundant::SetPassiveBaseFrame(unsigned int frame)
{
        //Default: be the frame {0}
        passive.SetBaseFrame(frame);
}

//------------------------------------------------------------------
//
//Set the first joint in the passive joint chain
//
//------------------------------------------------------------------
void CRedundant::SetPassiveFirstJoint(unsigned int joint)
{
        passive.SetFirstJoint(joint);
}

//------------------------------------------------------------------
//
//Set the last joint in the passive joint chain
//
//------------------------------------------------------------------
void CRedundant::SetPassiveLastJoint(unsigned int joint)
{
        passive.SetLastJoint(joint);
        passiveJointNum = passive.jointNum;
        //passive.SetEndEffectFrame(joint+1);
}

//------------------------------------------------------------------
//
//Set the base frame of passive joints
//
//------------------------------------------------------------------
void CRedundant::SetActiveBaseFrame(unsigned int frame)
{
        active.SetBaseFrame(frame);
}

//------------------------------------------------------------------
//
//Set the first joint in the active joint chain
//
//------------------------------------------------------------------
void CRedundant::SetActiveFirstJoint(unsigned int joint)
{
        active.SetFirstJoint(joint);
}

//------------------------------------------------------------------
//
//Set the last joint in the active joint chain
//
//------------------------------------------------------------------
void CRedundant::SetActiveLastJoint(unsigned int joint)
{
        active.SetLastJoint(joint);
        activeJointNum = active.jointNum;
}

//------------------------------------------------------------------
//This function is use to set collision detector;
//
//Input Parameter:
//      collisionDetector: Collision detector
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CRedundant::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        this->collisionDetector = collisionDetector;
        if( dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0)) )
        {
                this->robot = (R_OpenChain *)collisionDetector->GetRobot(0);
                active.SetRobot(this->robot);
                passive.SetRobot(this->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 ) ;
                }
        }
        else
        {
                this->robot = NULL;
        }
}

//------------------------------------------------------------------
//This function is use to get a end-effector frame by a given
//configuration
//
//Input Parameter:
//      conf: given configuration
//
//Ouput Parameter:
//      frame: corresponding pose
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CRedundant::GetDesireFrameByConfiguration(const Configuration &conf, Frame &frame) const
{
        collisionDetector->SetConfiguration(conf);

        Frame *endFrame = frameManager->GetFrameRef(robot->GetToolFrame(0));

        DH_Link *link = (DH_Link*)links[activeJointNum+passiveJointNum-1];

        frame *= frameManager->GetTransformRelative(link->FrameNum(), 0);
        //frame *= frameManager->GetTransformRelative(robot->GetToolFrame(0), 0);

        if (endFrame!=NULL)
                frame *= *(endFrame);
}

//------------------------------------------------------------------
//This function is use to combine active joint variables and
//passive joint variables into a complete configuration.
//
//Input Parameter:
//      activeConf: Active joint variables
//      passiveConf: Passive joint variables
//
//Ouput Parameter:
//      conf: complete configuration
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CRedundant::CombineConfiguration(Configuration &conf, Configuration &activeConf, Configuration &passiveConf)
{
        int i;

        conf.SetLength(activeJointNum+passiveJointNum);

        //Here, we just suppose that the passive chain is at the end of robot chain
        for (i=0; i<activeJointNum; i++)
        {
                conf.SetJointVar(i, activeConf[i]);
        }

        for (i=activeJointNum; i<(activeJointNum+passiveJointNum); i++)
        {
                conf.SetJointVar(i, passiveConf[i-activeJointNum]);
        }
}

//------------------------------------------------------------------
//This function is use to update active joint variables to robot
//so that the base frame of the passive joint chain can be 
//computed and updated.
//
//Input Parameter:
//      active: Active joint variables
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CRedundant::UpdateActiveConfiguration(Configuration &active)
{
        Frame frame;
        Frame currentFrame;

        /*
          DH_Link *link = (DH_Link*)links[activeJointNum-1];
         
          for (int i=0; i<active.DOF(); i++)
          {
            links[i]->SetJointVariable(active[i]);
            links[i]->UpdateFrames();
          }
          
          frame *= frameManager->GetTransformRelative(link->FrameNum(), 0);
         
        */
        for (int i=0; i<active.DOF(); i++)
        {
                links[i]->SetJointVariable(active[i]);
                links[i]->UpdateFrames();
                ((DH_Link *)links[i])->GetFrame(currentFrame);
                frame *= currentFrame;
                //currentFrame
        }

        passive.SetFirstFrame(frame);
}

//------------------------------------------------------------------
//This function is use to get a totally random configuration
//
//Input Parameter:
//      N.A
//
//Ouput Parameter:
//      conf: A random configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetRandomConfiguration(Configuration &conf)
{
        Configuration activeConf, passiveConf;

        activeConf.SetLength(activeJointNum);
        if (!active.GetConfiguration(activeConf))
                return false;

        passiveConf.SetLength(passiveJointNum);
        if (!passive.GetRandomConfiguration(passiveConf))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

//------------------------------------------------------------------
//This function is use to get a totally random configuration
//whose end-effector pose is same as given configuration
//
//Input Parameter:
//      desire: Given configuration
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetRandomConfiguration(Configuration &conf, Configuration &desire)
{
        Frame frame;

        GetDesireFrameByConfiguration(desire, frame);

        return GetRandomConfiguration(conf, frame);
}

//------------------------------------------------------------------
//This function is used to get a random configuration
//whose end-effector pose is same as given configuration, 
//in a neighbour area of bias.
//
//Input Parameter:
//      bias: Given bias configuration
//      desire: Given configuration for desired pose
//      dist: Deviation allowed for each joint variables
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetRandomConfiguration(Configuration &conf, Configuration &bias, Configuration &desire, double dist)
{
        Frame frame;

        GetDesireFrameByConfiguration(desire, frame);

        return GetRandomConfiguration(conf, bias, frame, dist);
}

//------------------------------------------------------------------
//This function is used to get a random configuration
//whose end-effector pose satisfies given frame. 
//
//Input Parameter:
//      frame: Given frame, might include orientation/position
//             but only position is considerred in currently
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetRandomConfiguration(Configuration &conf, Frame &frame)
{
        Configuration activeConf, passiveConf;

        passive.SetDesireEndEffect(frame);

        activeConf.SetLength(activeJointNum);
        if (!active.GetConfiguration(activeConf))
                return false;

        UpdateActiveConfiguration(activeConf);

        passiveConf.SetLength(passiveJointNum);
        if (!passive.GetConfiguration(passiveConf))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

//------------------------------------------------------------------
//This function is used to get a random configuration
//whose end-effector pose satisfies given frame. 
//in a neighbour area of bias.
//
//Input Parameter:
//      bias: Given bias configuration
//      frame: Given frame, might include orientation/position
//             but only position is considerred in currently
//      dist: Deviation allowed for each joint variables
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetRandomConfiguration(Configuration &conf, Configuration &bias, Frame &frame, double dist)
{
        int i;

        Configuration activeConf, passiveConf;
        Configuration activeBias, passiveBias;

        passive.SetDesireEndEffect(frame);

        activeConf.SetLength(activeJointNum);
        activeBias.SetLength(activeJointNum);
        for (i=0; i<activeJointNum; i++)
                activeBias[i] = bias[i];
        if (!active.GetConfiguration(activeConf, activeBias, Rad2Deg(dist)))
                return false;

        UpdateActiveConfiguration(activeConf);

        passiveConf.SetLength(passiveJointNum);
        passiveBias.SetLength(passiveJointNum);
        for (i=0; i<passiveJointNum; i++)
                passiveBias[i] = bias[activeJointNum+i];
        if (!passive.GetConfiguration(passiveConf, passiveBias, Rad2Deg(dist)))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

//------------------------------------------------------------------
//This function is used to get a configuration by active joint variables,
//computing the passive joint variables to satisfy given frame. 
//
//Input Parameter:
//      active: Active joint variables
//      frame: Given frame, might include orientation/position
//             but only position is considerred in currently
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetConfigurationByActive(Configuration &conf, Configuration &active, Frame &frame)
{
        Configuration activeConf, passiveConf;

        activeConf.SetLength(activeJointNum);

        passive.SetDesireEndEffect(frame);

        for (int i=0; i<activeJointNum; i++)
        {
                activeConf[i] = active[i];
        }
        UpdateActiveConfiguration(activeConf);

        passiveConf.SetLength(passiveJointNum);
        if (!passive.GetConfiguration(passiveConf))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

//------------------------------------------------------------------
//This function is used to get a closest configuration to the
//bias by active joint variables, computing the passive joint 
//variables to satisfy given frame. 
//
//Input Parameter:
//      active: Active joint variables
//      frame: Given frame, might include orientation/position
//             but only position is considerred in currently
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetConfigurationByActive(Configuration &conf, Configuration &active, Configuration &bias, Frame &frame)
{
        Configuration activeConf, passiveConf, passiveBias;

        activeConf.SetLength(activeJointNum);

        passive.SetDesireEndEffect(frame);

        for (int i=0; i<activeJointNum; i++)
        {
                activeConf[i] = active[i];
        }
        UpdateActiveConfiguration(activeConf);

        passiveConf.SetLength(passiveJointNum);
        passiveBias.SetLength(passiveJointNum);
        for (i=0; i<passiveJointNum; i++)
                passiveBias[i] = bias[activeJointNum+i];
        if (!passive.GetConfiguration(passiveConf, passiveBias))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

//------------------------------------------------------------------
//This function is used to get a configuration in a neighbour area 
//of bias by active joint variables, computing the passive joint 
//variables to satisfy given frame. 
//
//Input Parameter:
//      active: Active joint variables
//      frame: Given frame, might include orientation/position
//             but only position is considerred in currently
//      dist: Deviation allowed for each joint variables
//
//Ouput Parameter:
//      conf: Satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CRedundant::GetConfigurationByActive(Configuration &conf, Configuration &active, Configuration &bias, Frame &frame, double dist)
{
        Configuration activeConf, passiveConf, passiveBias;

        activeConf.SetLength(activeJointNum);

        passive.SetDesireEndEffect(frame);

        for (int i=0; i<activeJointNum; i++)
        {
                activeConf[i] = active[i];
        }
        UpdateActiveConfiguration(activeConf);

        passiveConf.SetLength(passiveJointNum);
        passiveBias.SetLength(passiveJointNum);
        for (i=0; i<passiveJointNum; i++)
                passiveBias[i] = bias[activeJointNum+i];
        if (!passive.GetConfiguration(passiveConf, passiveBias, Rad2Deg(dist)))
                return false;

        CombineConfiguration(conf, activeConf, passiveConf);
        return true;
}

---