planners/inversekin/Passive.cpp

Go to the documentation of this file.

//
//Contents: Class CPassive
// 
//Relations:
//      CJoints --> CActive  ---o CRedundant
//            |---> CPassive -|
//
//      Class CActive/CPassive is derived from class CJoints
//      Class CRedundant constains entities of CActive and CPassive
//
//CPassive is used to do closed-form inverse kinematics. 
//The typical calling sequence is:
//      1.CPassive::CPassive(...)
//      2.CPassive::SetFrameManager(...)
//        CPassive::SetRobot(...)
//        CPassive::SetFirstJoint(...)
//        CPassive::SetLastJoint(...)
//      3.CPassive::SetFirstFrame(...)
//        CPassive::SetEndEffectFrame(...)
//      4.CPassive::SetDesireEndEffect(...)
//      5.CPassive::GetConfiguration(...)
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Mar-2003:  Z.Yao     First version implemented
//                          In this version, only two joints are
//                          allowed in a passive chain, and only
//                          position is considerred.
//                          
//Others:
//     a.[Mar-2003]

#include <math.h>
#include "math\math2.h"
#include "math\vector4.h"
#include "Passive.h"
#include "debug/debug.h"
//------------------------------------------------------------------
//
//Construction and Deconstruction function
//
//------------------------------------------------------------------
CPassive::CPassive():CJoints()
{
        solution = new Configuration[MAX_SOLUTION];
}

CPassive::CPassive(FrameManager* frameManager)
                :CJoints(frameManager)
{
        solution = new Configuration[MAX_SOLUTION];
}

CPassive::~CPassive()
{
        delete[] solution;
}

//------------------------------------------------------------------
//
//Set the robot in the environment
//As default, it set robot's end-effector as that of passive chain.
//
//------------------------------------------------------------------
void CPassive::SetRobot(R_OpenChain *robot)
{
        CJoints::SetRobot(robot);
        Frame *frame = frameManager->GetFrameRef(robot->GetToolFrame(0));
        if (frame != NULL)
                effectFrame.SetValues(*(frame));
        else
                effectFrame.SetValues(Frame::Identity());
}

//------------------------------------------------------------------
//
//Set the last joint in the passive joint chain
//
//------------------------------------------------------------------
void CPassive::SetLastJoint(unsigned int joint)
{
        CJoints::SetLastJoint(joint);
        for (int i=0; i<MAX_SOLUTION; i++)
        {
                solution[i].SetNumDOF(jointNum);
        }
        solutionNum = 0;
}

//------------------------------------------------------------------
//This function is used to set end-effector pose with respect to wrist.
//
//Input Parameter:
//      frame: end-effector frame r.w.t wrist
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CPassive::SetEndEffectFrame(Frame &frame)
{
        effectFrame.SetValues(frame);
        //this->effectFrameIndex=frame;
}

//------------------------------------------------------------------
//This function is used to set desired end-effector pose 
//with respect to universe frame.
//
//Input Parameter:
//      frame: end-effector frame r.w.t universe frame
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CPassive::SetDesireEndEffect(Frame &frame)
{
        desireEffect.SetValues(frame);
}

//------------------------------------------------------------------
//This function is used to set a base frame of passive chain.
//And the base frame is r.w.t universe frame.
//
//Input Parameter:
//      frame: base frame w.r.t universe frame
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      N.A
//------------------------------------------------------------------
void CPassive::SetFirstFrame(Frame &frame)
{
        firstFrame.SetValues(frame);
}

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

        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 passive joint variables randomly.
//
//Input Parameter:
//      N.A
//
//Ouput Parameter:
//      conf: A random configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CPassive::GetConfiguration(Configuration &conf)
{
        conf.SetLength(jointNum);
        if (jointNum==0)
                return true;

        if (!Inverse())
                return false;

        int random = solutionNum*rand() / (RAND_MAX+2);

        for (int i=0; i<jointNum; i++)
                conf[i] = (solution[random])[i];
        return true;
}

//------------------------------------------------------------------
//This function is used to get passive joint variables
//in a neighbour area of current configuration, and satisfying
//end-effector pose set by function SetDesiredEndEffector(...). 
//
//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 CPassive::GetConfiguration(Configuration &conf, Configuration &current, double dist)
{
        conf.SetLength(jointNum);
        if (jointNum==0)
                return true;

        bool bFind;
        int i, j;
        if (!Inverse())
        {
                return false;
        }

        //for (i=0; i<solutionNum; i++)
        //      for (j=0; j<jointNum; j++)
        //              solution[i][j] = Rad2Deg(solution[i][j]);
        //Now above conversion is done in Inverse()!!

        for (i=0; i<solutionNum; i++)
        {
                bFind = true;

                for (j=0; j<jointNum; j++)
                {
                        if (Distance(links[firstJoint+j],solution[i][j], current[j])>dist)
                        {
                                bFind = false;
                                break;
                        }
                }
                if (bFind)
                {
                        for (j=0; j<jointNum; j++)
                                conf[j] = (solution[i])[j];
                        break;
                }
        }
        return bFind;
}

//------------------------------------------------------------------
//This function is used to get passive joint variables
//that is closest to current configuration, and satisfying
//end-effector pose set by function SetDesiredEndEffector(...). 
//
//Input Parameter:
//      current: Given bias configuration
//
//Ouput Parameter:
//      conf: Closest satisfactory configuration
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CPassive::GetConfiguration(Configuration &conf, Configuration &current)
{
        conf.SetLength(jointNum);
        if (jointNum==0)
                return true;

        int i, j;
        if (!Inverse())
        {
                return false;
        }

        //for (i=0; i<solutionNum; i++)
        //      for (j=0; j<jointNum; j++)
        //              solution[i][j] = Rad2Deg(solution[i][j]);
        //Now above conversion is done in Inverse()!!

        int minDist = 0;
        int minSolution = 0;
        for (j=0; j<jointNum; j++)
        {
                double dist = Distance(links[firstJoint+j], solution[0][j], current[j]);
                minDist += dist*dist;
        }

        for (i=1; i<solutionNum; i++)
        {
                double currDist=0;
                for (j=0; j<jointNum; j++)
                {
                        double dist = Distance(links[firstJoint+j], solution[i][j], current[j]);
                        currDist += dist*dist;
                }

                if (currDist < minDist)
                {
                        minDist = currDist;
                        minSolution = i;
                }
        }

        for (j=0; j<jointNum; j++)
        {
                conf[j] = (solution[minSolution])[j];
        }

        return true;
}

//------------------------------------------------------------------
//This function is a private function to solve the case where 
//there is only one joint in passive chain.
//
//Input Parameter:
//      type: Joint type: Revolute or Prismatic
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CPassive::ResolveJoints(DH_Parameter type)
{
        return false;
}

//------------------------------------------------------------------
//This function is a private function to solve the case where 
//there are two joints in passive chain.
//
//Input Parameter:
//      type1/type2: Joint type: Revolute or Prismatic
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CPassive::ResolveJoints(DH_Parameter type1, DH_Parameter type2)
{
        double a1, alpha1, d1, theta1;
        double a2, alpha2, d2, theta2;

        DH_Link *link1 = (DH_Link *) links[firstJoint];
        DH_Link *link2 = (DH_Link *) links[firstJoint+1];

        a1 = link1->GetA();
        a2 = link2->GetA();

        d1 = link1->GetD();
        d2 = link2->GetD();

        alpha1 = link1->GetAlpha();
        alpha2 = link2->GetAlpha();

        theta1 = link1->GetTheta();
        theta2 = link2->GetTheta();

        double x, y, z;
        double Xe, Ye, Ze;
        double Xo, Yo, Zo;

        //Xe, Ye, Ze, End Effect Position with respect to frame {0}
        Xe = desireEffect(0, 3);
        Ye = desireEffect(1, 3);
        Ze = desireEffect(2, 3);

        //x, y, z, End Effect Position with respect to last frame
        x = effectFrame(0, 3);
        y = effectFrame(1, 3);
        z = effectFrame(2, 3);

        Vector4 firstPos(a1, -sin(alpha1)*d1, cos(alpha1)*d1);
        Vector4 firstPos0= firstFrame * firstPos;
        Xo = firstPos0[0];
        Yo = firstPos0[1];
        Zo = firstPos0[2];
        //Xo = firstFrame(0, 3);
        //Yo = firstFrmae(1, 3);
        //Zo = firstFrame(2, 3);

        double a, b, c, d;

        double solu[2];
        int num;

        solutionNum = 0;

        //------------Both are Revolute------------
        if ((type1==DH_THETA)&& (type2==DH_THETA))
        {
                a = (Xe-Xo)*(Xe-Xo)+(Ye-Yo)*(Ye-Yo)-a1*a1-a2*a2;
                b = -2*a1*a2;

                double cos2, sin2;
                cos2 = a/b;
                if (fabs(cos2)>1)
                {
                        if (IsZero(fabs(cos2)-1))
                        {
                                cos2 = cos2/fabs(cos2);
                                sin2 = 0;
                        }
                        else
                        {
                                return false;
                        }
                }
                else
                {
                        sin2 = sqrt(1-cos2*cos2);
                }

                sin2 = sqrt(1-cos2*cos2);

                solution[0][1] = M_PI-atan2(sin2, cos2);
                solution[1][1] = atan2(sin2, cos2)-M_PI;

                solutionNum = 2;

                Vector4 end2First(Xe, Ye, Ze);
                Vector4 end2First0 = firstFrame.Inverse() * end2First;
                Xo = end2First0[0];
                Yo = end2First0[1];
                Zo = end2First0[2];

                for (int i=0; i<2; i++)
                {
                        cos2 = cos(solution[i][1]);
                        sin2 = sin(solution[i][1]);

                        a = cos(alpha1)*Yo+sin(alpha1)*Zo;
                        b = Xo-a1;
                        c = sin2*x - cos2*y;
                        d = a2 + cos2*x - sin2*y;

                        Solution(a, b, c, d, solu[0]);
                        solution[i][0] = solu[0];
                }

                /*
                ----------------------------------------
                +cos(theta1)*()
                -sin(theta1)*(Xo-a1)
                +sin(theta2)*(-x)
                +cos(theta2)*(y)
                =0
                ----------------------------------------   
                +sin(theta1)*(cos(alpha1)*Yo+sin(alpha1)*Zo)
                +cos(theta1)*(Xo-a1)
                +cos(theta2)*(-x)
                +sin(theta2)*(y)
                =a2
                ----------------------------------------
                */
                /*
                    //Get theta2
                    a = y*sin(alpha2);
                    b = x*sin(alpha2);
                    c = -d1-sin(alpha1)*Ye+cos(alpha1)*Ze-cos(alpha2)*z-cos(alpha2)*d2;
                 
                    Solution(a, b, c, solu, num);
                    if (num==1)
                    {
                      solutionNum = 1;
                      solution[0][1] = solu[0];
                    }
                    else if (num==2)
                    {
                      solutionNum = 2;
                      solution[0][1] = solu[0];
                      solution[1][1] = solu[1];
                    }
                    else
                    {
                      return false;
                    }
                 
                    //Get theta1
                    for (int i=0; i<solutionNum; i++)
                    {
                      theta2 = solution[i][1];
                 
                                  a=(Xe-a1);
                                  b=(cos(alpha1)*Ye+sin(alpha1)*Ze);
                                  c=-(-sin(theta2)*cos(alpha2)*x
                          -cos(theta2)*cos(alpha2)*y
                          +sin(alpha2)*z
                          +sin(alpha2)*d2
                         );
                                  d=-(-cos(theta2)*x+sin(theta2)*y-a2);
                 
                      Solution(a, b, c, d, solu[0]);
                      solution[i][0] = solu[0];
                    }
                */
        }

        //------------Both are Prismatic------------
        else if((type1==DH_D)&& (type2==DH_D))
        {
                a = cos(theta1)*(Xe-a1)
                    +sin(theta1)*(cos(alpha1)*Ye+sin(alpha1)*Ze)
                    -cos(theta2)*x+sin(theta2)*y-a2;

                if (!IsZero(a))
                        return false;

                //Get D2
                a=sin(alpha2);
                b=-(-sin(theta1)*(Xe-a1)
                    +cos(theta1)*(cos(alpha1)*Ye+sin(alpha1)*Ze)
                    -sin(theta2)*cos(alpha2)*x
                    -cos(theta2)*cos(alpha2)*y+sin(alpha2)*z
                   );
                //if (a==0)
                if (!IsZero(a))
                        return false;
                solution[0][1]=d2=b/a;

                //Get D1
                solution[0][0]=(-sin(theta2)*sin(alpha2)*x
                                -cos(theta2)*sin(alpha2)*y
                                -sin(alpha1)*Ye
                                +cos(alpha1)*Ze
                                -cos(alpha2)*z
                                -cos(alpha2)*d2
                               );

                solutionNum = 1;
        }

        //------------First Revolute, second Prismatic------------
        else if((type1==DH_THETA)&& (type2==DH_D))
        {
                //Get Theta1
                a=(Xe-a1);
                b=(cos(alpha1)*Ye+sin(alpha1)*Ze);
                c=-(-cos(theta2)*x+sin(theta2)*y-a2);

                Solution(a, b, c, solu, num);
                if (num==1)
                {
                        solutionNum = 1;
                        solution[0][0] = solu[0];
                }
                else if (num==2)
                {
                        solutionNum = 2;
                        solution[0][0] = solu[0];
                        solution[1][0] = solu[1];
                }
                else
                {
                        return false;
                }

                //Get D2
                for (int i=0; i<solutionNum; i++)
                {
                        theta1 = solution[i][0];

                        a=sin(alpha2);
                        b=-(-sin(theta1)*(Xe-a1)
                            +cos(theta1)*(cos(alpha1)*Ye+sin(alpha1)*Ze)
                            -sin(theta2)*cos(alpha2)*x
                            -cos(theta2)*cos(alpha2)*y
                            +sin(alpha2)*z
                           );

                        //if (a==0)
                        if (!IsZero(a))
                        {
                                solutionNum = 0;
                                return false;
                        }
                        else
                        {
                                solution[i][1] = b/a;
                        }
                }

        }

        //------------First Prismatic, second Revolute------------
        else
        {
                //Get Theta2
                a=cos(alpha2)*y;
                b=cos(alpha2)*x;
                c=-sin(theta1)*(Xe-a1)
                  +cos(theta1)*(cos(alpha1)*Ye+sin(alpha1)*Ze)
                  +sin(alpha2)*z+sin(alpha2)*d2;

                Solution(a, b, c, solu, num);
                if (num==1)
                {
                        solutionNum = 1;
                        solution[0][1] = solu[0];
                }
                else if (num==2)
                {
                        solutionNum = 2;
                        solution[0][1] = solu[0];
                        solution[1][1] = solu[1];
                }
                else
                {
                        return false;
                }

                //Get D1
                for (int i=0; i<solutionNum; i++)
                {
                        theta2 = solution[i][1];

                        solution[i][0]=(-sin(theta2)*sin(alpha2)*x
                                        -cos(theta2)*sin(alpha2)*y
                                        -sin(alpha1)*Ye+cos(alpha1)*Ze
                                        -cos(alpha2)*z
                                        -cos(alpha2)*d2
                                       );
                }
        }

        return true;
}

//------------------------------------------------------------------
//This function is a private function to solve the case where 
//there are three joints in passive chain.
//
//Input Parameter:
//      type1/type2/type3: Joint type: Revolute or Prismatic
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool CPassive::ResolveJoints(DH_Parameter type1, DH_Parameter type2, DH_Parameter type3)
{
        double a1, alpha1, d1, theta1;
        double a2, alpha2, d2, theta2;

        DH_Link *link1 = (DH_Link *) links[firstJoint];
        DH_Link *link2 = (DH_Link *) links[firstJoint+1];
        DH_Link *link3 = (DH_Link *) links[firstJoint+2];

        a1 = link1->GetA();
        a2 = link2->GetA();

        d1 = link1->GetD();
        d2 = link2->GetD();

        alpha1 = link1->GetAlpha();
        alpha2 = link2->GetAlpha();

        theta1 = link1->GetTheta();
        theta2 = link2->GetTheta();

        double x, y, z;
        double Xw, Yw, Zw;
        double Xo, Yo, Zo;

        Frame desireWrist = desireEffect * effectFrame.Inverse();
        Xw = desireWrist(0, 3);
        Yw = desireWrist(1, 3);
        Zw = desireWrist(2, 3);

        link3->SetJointVariable(0);
        Frame wristFrame = link3->GetFrame();
        x = wristFrame(0, 3);
        y = wristFrame(1, 3);
        z = wristFrame(2, 3);

        Vector4 firstPos(a1, -sin(alpha1)*d1, cos(alpha1)*d1);
        Vector4 firstPos0= firstFrame * firstPos;
        Xo = firstPos0[0];
        Yo = firstPos0[1];
        Zo = firstPos0[2];
        //Xo = firstFrame(0, 3);
        //Yo = firstFrmae(1, 3);
        //Zo = firstFrame(2, 3);

#if 0
        Log("passive.log", \
                "desireEffect:\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                desireEffect(0, 0), desireEffect(0, 1), desireEffect(0, 2), desireEffect(0, 3), \
                desireEffect(1, 0), desireEffect(1, 1), desireEffect(1, 2), desireEffect(1, 3), \
                desireEffect(2, 0), desireEffect(2, 1), desireEffect(2, 2), desireEffect(2, 3) \
                );
        Log("passive.log", \
                "desireWrist:\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                desireWrist(0, 0), desireWrist(0, 1), desireWrist(0, 2), desireWrist(0, 3), \
                desireWrist(1, 0), desireWrist(1, 1), desireWrist(1, 2), desireWrist(1, 3), \
                desireWrist(2, 0), desireWrist(2, 1), desireWrist(2, 2), desireWrist(2, 3) \
                );
        Log("passive.log", \
                "wristFrame:\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                wristFrame(0, 0), wristFrame(0, 1), wristFrame(0, 2), wristFrame(0, 3), 
                wristFrame(1, 0), wristFrame(1, 1), wristFrame(1, 2), wristFrame(1, 3), 
                wristFrame(2, 0), wristFrame(2, 1), wristFrame(2, 2), wristFrame(2, 3)
                );
        Log("passive.log", \
                "firstFrame:\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                firstFrame(0, 0), firstFrame(0, 1), firstFrame(0, 2), firstFrame(0, 3), 
                firstFrame(1, 0), firstFrame(1, 1), firstFrame(1, 2), firstFrame(1, 3), 
                firstFrame(2, 0), firstFrame(2, 1), firstFrame(2, 2), firstFrame(2, 3)
                );
#endif

        double a, b, c, d;
        double solu[2];

        solutionNum = 0;

        if ((type1==DH_THETA)&& (type2==DH_THETA) && (type3==DH_THETA))
        {
                a = (Xw-Xo)*(Xw-Xo)+(Yw-Yo)*(Yw-Yo)-a1*a1-a2*a2;
                b = -2*a1*a2;

                double cos2, sin2;
                cos2 = a/b;
                if (fabs(cos2)>1)
                {
                        if (IsZero(fabs(cos2)-1))
                        {
                                cos2 = cos2/fabs(cos2);
                                sin2 = 0;
                        }
                        else
                        {
                                return false;
                        }
                }
                else
                {
                        sin2 = sqrt(1-cos2*cos2);
                }

                solution[0][1] = M_PI-atan2(sin2, cos2);
                solution[1][1] = atan2(sin2, cos2)-M_PI;

                solutionNum = 2;

                Vector4 end2First(Xw, Yw, Zw);
                Vector4 end2First0 = firstFrame.Inverse() * end2First;
                Xo = end2First0[0];
                Yo = end2First0[1];
                Zo = end2First0[2];

                for (int i=0; i<2; i++)
                {
                        cos2 = cos(solution[i][1]);
                        sin2 = sin(solution[i][1]);

                        a = cos(alpha1)*Yo+sin(alpha1)*Zo;
                        b = Xo-a1;
                        c = sin2*x - cos2*y;
                        d = a2 + cos2*x - sin2*y;

                        Solution(a, b, c, d, solu[0]);
                        solution[i][0] = solu[0];

                        //Add wrist joint value
                        link1->SetJointVariable(Rad2Deg(solution[i][0]));
                        link2->SetJointVariable(Rad2Deg(solution[i][1]));
                        link3->SetJointVariable(0);
                        Frame currWristFrame = firstFrame*link1->GetFrame()*link2->GetFrame()*link3->GetFrame();
                        Frame link3Frame = currWristFrame.Inverse() * desireWrist;

#if 0
                        Log("passive.log", \
                                "currWristFrame:\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                                currWristFrame(0, 0), currWristFrame(0, 1), currWristFrame(0, 2), currWristFrame(0, 3), 
                                currWristFrame(1, 0), currWristFrame(1, 1), currWristFrame(1, 2), currWristFrame(1, 3), 
                                currWristFrame(2, 0), currWristFrame(2, 1), currWristFrame(2, 2), currWristFrame(2, 3)
                                );
                        Log("passive.log", \
                                "link3Frame:\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n \
                                \t[%3.4f, %3.4f, %3.4f, %3.4f]\n", \
                                link3Frame(0, 0), link3Frame(0, 1), link3Frame(0, 2), link3Frame(0, 3), 
                                link3Frame(1, 0), link3Frame(1, 1), link3Frame(1, 2), link3Frame(1, 3), 
                                link3Frame(2, 0), link3Frame(2, 1), link3Frame(2, 2), link3Frame(2, 3)
                                );
#endif
                        
                        Solution(link3Frame, solu[0]);
                        solution[i][2] = solu[0];
#if 0
                        Log("passive.log", "The solution we get is %f\n==========", solu[0]);
#endif
                }

        }
        else
        {
                //Not support other case yet!
                assert(false);
        }
        return true;
}

//------------------------------------------------------------------
//This function is a private function to initiate inverse kinematics
//computation. The calling stack would be:
//   Inverse(...)           -- Dispatcher
//      ResolveJoints(...)      -- Handle kinematics
//         Solution(...)    -- Solve arithematic equation
//
//Input Parameter:
//      N.A
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//
//Note:
//      The solution is stored in member variable: solutions[]
//------------------------------------------------------------------
bool CPassive::Inverse()
{

        if ((jointNum>3) || (jointNum<=0))
        {
                return false;
        }

        DH_Parameter type1, type2, type3;

        DH_Link *link1, *link2, *link3;

        link1 = (DH_Link *) links[firstJoint];
        type1 = link1->GetJointType();
        if ((type1 == DH_ALPHA) || (type1 == DH_A))
        {
                return false;
        }

        if (jointNum > 1)
        {
                link2 = (DH_Link *)links[firstJoint+1];
                type2 = link2->GetJointType();

                if ((type2 == DH_ALPHA) || (type2 == DH_A))
                {
                        return false;
                }
        }

        if (jointNum > 2)
        {
                link3 = (DH_Link *)links[firstJoint+2];
                type3 = link3->GetJointType();

                if ((type2 == DH_ALPHA) || (type2 == DH_A))
                {
                        return false;
                }
        }

        bool bResolved;
        if (jointNum==1)
        {
                bResolved = ResolveJoints(type1);
        }
        else if (jointNum==2)
        {
                bResolved = ResolveJoints(type1, type2);
        }
        else if (jointNum==3)
        {
                bResolved = ResolveJoints(type1, type2, type3);
        }

        if (bResolved)
        {
                for (int i=0; i<solutionNum; i++)
                for (int j=0; j<jointNum; j++)
                        solution[i][j] = Rad2Deg(solution[i][j]);
        }

        return bResolved;
}

//------------------------------------------------------------------
//This function is a private function to solve the equation:
//  a*cos(theta)+b*sin(theta)=c, where
//      a, b, c, is known, asking for "theta"
//
//Input Parameter:
//      a,b,c:
//
//Ouput Parameter:
//      num: number of solution
//      solution[]: solutions for theta
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool __inline CPassive::Solution(double a, double b, double c, double solution[2], int &num)
{
        if ((a==0) && (b==0))
        {
                num = 0;
                return false;
        }

        if ((a*a+b*b)<(c*c))
        {
                return false;
        }
        else if ((a*a+b*b)==(c*c))
        {
                num = 1;
                if (c>0)
                        solution[0] = atan2(b,a);
                else
                        solution[0] = atan2(b,a)+M_PI;
        }
        else
        {
                num = 2;
                solution[0] = atan2(b,a)+atan2(sqrt(a*a+b*b-c*c), c);
                solution[1] = atan2(b,a)-atan2(sqrt(a*a+b*b-c*c), c);
        }
        return true;
}

//------------------------------------------------------------------
//This function is a private function to solve the equations:
//  a*cos(theta)-b*sin(theta)=c  && 
//  a*sin(theta)+b*cos(theta)=d, where
//      a, b, c, is known, asking for "theta"
//
//Input Parameter:
//      a,b,c:
//
//Ouput Parameter:
//      solution: solution for theta, there will be only one
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool __inline CPassive::Solution(double a, double b, double c, double d, double &solution)
{
        if ((a==0) && (b==0))
        {
                return false;
        }

        solution = atan2(a*d-b*c, a*c+b*d);
        return true;
}

//------------------------------------------------------------------
//This function is a private function to solve the rotation:
//  given a frame, computer the equivalent rotation axis
//  and the angle rotated
//
//Input Parameter:
//      rotated: Frame(orientation) after rotated
//
//Ouput Parameter:
//      solution: solution for theta, there will be only one
//
//Return Value:
//      true, if success; 
//      false, otherwise;
//------------------------------------------------------------------
bool __inline CPassive::Solution(Frame &rotated, double &solution)
{
        double temp;
        temp = rotated.values[0][0] + rotated.values[1][1] + rotated.values[2][2] - 1;

        assert( fabs(temp) <= (2+ROUNDING_ERROR) );

        solution = acos(temp/2);
        if ((rotated.values[1][0] - rotated.values[0][1]) < 0)
                solution = -solution;
        return true;
}

---