basic/kinematics/Jacobian.cpp

Go to the documentation of this file.

//
//Contents: Class CJacobian.
// 
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Feb-2004:  G.Sheung  First version implemented
//     Jun-2004:  Z.Yao     Adapted, associate with a robot
//     Oct-2004:  Z.Yao     Adapted, for SSRMS/SPDM, where a part of robot need to do 
//                          the planning independently.
//     Nov-2004:  Z.Yao     Comments added
//  
//Notes:
//   1. The typical procedure to call CJacobian is as follows
//      a. Create a entity of class CJacobian, say $jacobian$, by associating it with a robot;
//      b. jacobian.SetConfiguration($conf$); 
//         After this point you can jacobian.GetJointFrame(...) or jacobian.GetJointWorldFrame(...)
//         to get the frame of each joint;
//      c. jacobian.SetInterestPoint(...).
//         After this point you can get the jacobian matrix, since it has been computed inside.
//      d. jacobian.GetJointVelocity(...), or jacobian.GetPhyVelocity(...)
//         Get joint-space velocity or physical-space velocity by the other velocity.
//
#include <malloc.h>
#include <assert.h>
#include <math.h>
#include "Jacobian.h"
#include "DH_Link.h"
#include "math\Matrixmxn.h"
#include "math\VectorN.h"
#include "math\Vector4.h"

#define PI 3.1415926535897932384
#define END_EFF_DOF   2

CJacobian::CJacobian(RobotBase& robot)
{
    m_pRobot = &robot;
    m_nDof = robot.DOF();
    m_configCurrent.SetNumDOF(m_nDof);
    m_bOrientation = false;
    m_bPosition = true;
    m_nJoint = m_nDof;
    m_pMatrix = new Matrixmxn(3, m_nDof);
    m_pJointMatrix = new Matrix4x4[m_nDof];
    m_pJointOrig = new Vector4[m_nDof];
    m_pJointZ = new Vector4[m_nDof];
}

CJacobian::~CJacobian()
{
    delete (m_pMatrix);
    delete [] m_pJointMatrix;
    delete [] m_pJointOrig;
    delete [] m_pJointZ;
}

//------------------------------------------------------------------
//SetInterestPoint
//
//This function is used to compute the Jacobian matrix.
//
//Input Parameter:
//      joint    : Target joint, for example, the joint to which the end-effect attach 
//      endFrame : (config., pose) of the direction
//      position : To consider position or not.
//      orient   : To consider orientation or not.
//      baseJoint: First joint of the planning part;
//
//Ouput Parameter:
//      N.A
//
//Return Value:
//      true/false.
//
//Notes:
//   1. At the end of this function call, m_pMatrix will be updated with the newly-computed jacobain matrix.
//   2. Taking SSRMS/SPDM as an exmple, a robot with three arms, consiting of 29 joints, 
//      in one of the tasks, it plans for one of the arms, 7-joint, joint 8-14,
//      then this function will be called like: 
//          SetInterestPoint(14, $endFrame$, true, true, 7);
//      where $endFrame$ it the end-effector tool frame w.r.t joint-14.
//      
//------------------------------------------------------------------
// Given a joint regarded as wrist, and relative position
bool CJacobian::SetInterestPoint(int joint, Frame& endFrame, bool position, bool orient, int baseJoint)
{
    int m_nDof = m_pRobot->DOF();
//      if (baseJoint>joint)
//      {
//              assert(false);
//              return false;
//      }

    if ((m_bPosition != position)||(m_bOrientation != orient) || (m_nJoint != joint) || (m_nBaseJoint!= baseJoint) )
    {
        m_frObserve = endFrame;
        m_nJoint = joint;
        m_bPosition = position;
        m_bOrientation = orient;
        m_nBaseJoint = baseJoint;
        delete m_pMatrix;
        if (orient && position)
        {
            m_pMatrix = new Matrixmxn(6, m_nDof);
        }
        else if (orient || position)
        {
            m_pMatrix = new Matrixmxn(3, m_nDof);
        }
        else
        {
                assert(false);
                return false;
        }
    }

    if (!Calculate())
    {
        return false;
    }

    return true;
}

// Determine physical velocity by joint velocity
bool CJacobian::GetPhyVelocity(VectorN& phyVel, VectorN& jointVel)
{
    assert(m_pMatrix->GetRows() == phyVel.Length());
    phyVel = (*m_pMatrix) * jointVel;
    return true;        
}

// Determine joint velocity by physical velocity
bool CJacobian::GetJointVelocity(VectorN& jointVel, VectorN& phyVel)
{
    Matrixmxn inv;
    m_pMatrix->Inverse(inv);
    jointVel = inv*phyVel;
    return true;
}

bool CJacobian::SetConfiguration(Configuration& conf)
{
    m_configCurrent = conf;
    if (UpdateJoints())
        return true;
    else
        return false;
}
    
Matrixmxn* CJacobian::GetMatrix()
{
    return m_pMatrix;
}

bool CJacobian::UpdateJoints()
{
    std::list< LinkBase*> robotLinks = m_pRobot->GetAllLinks();
    std::list< LinkBase* >::iterator link = robotLinks.begin();

    Frame frame;
    for (int i = 0; i < m_nDof; i++, link++)
    {
        ((DH_Link *)(*link))->SetJointVariable(m_configCurrent[i]);
        frame *= ((DH_Link *)(*link))->GetFrame();
        m_pJointMatrix[i] = frame;

        m_pJointOrig[i] = frame.GetTranslationVector();
        m_pJointZ[i][0] = frame(0, 2);
        m_pJointZ[i][1] = frame(1, 2);
        m_pJointZ[i][2] = frame(2, 2);
    }
    return true;
}

bool CJacobian::Calculate()
{
    assert(m_nDof >= m_nJoint);

    Vector4 pEndZ;
    Vector4 pEndOrig;

    Frame frame = m_pJointMatrix[m_nJoint];
    frame *= m_frObserve;

    pEndOrig = frame.GetTranslationVector();
    pEndZ[0] = frame(0, 2);
    pEndZ[1] = frame(1, 2);
    pEndZ[2] = frame(2, 2);
 
        
    for (int i = 0; i < m_nBaseJoint; i++)
    {
        int nRow=3;
        if ((m_bOrientation) && (m_bPosition))
            nRow = 6;
        for (int j = 0; j< nRow; j++)
        {
            m_pMatrix->SetValues(j, i, (double)0);
        }                       
    }

    // calculate Jacobian
    std::list< LinkBase*> robotLinks = m_pRobot->GetAllLinks();
    std::list< LinkBase* >::iterator link = robotLinks.begin();
    for (i = m_nBaseJoint; i <= m_nJoint; i++)
    {
        int jointType = ((DH_Link*)(*link))->GetJointType();
        assert((jointType == DH_THETA) || (jointType == DH_D));

        Vector4 pIPTool; // (p-p[i-1])
        Vector4 linearColumn;
        Vector4 angularColumn;

        //Compute jacobian column for linear velocity
        if (m_bPosition)
        {
            linearColumn = m_pJointZ[i]; //Prismatic joint, by default
            if (jointType == DH_THETA) //Revolute joint
            {
                pIPTool = pEndOrig - m_pJointOrig[i];  // (Pobs - Piorg)
                linearColumn = linearColumn.Cross(pIPTool);
            }

            for (int j = 0; j < 3; j++)
            {
                m_pMatrix->SetValues(j, i, linearColumn[j]);
            }
        }

        //Compute jacobian column for angular velocity
        if (m_bOrientation)
        {
            angularColumn *= 0; //clear variable, Prismatic joint, by default
            if (jointType == DH_THETA) //Revolute joint
            {
                angularColumn = m_pJointZ[i];
            }
            int offset = (m_bPosition)?3:0;
            for (int j = 0 ; j < 3; j++)
            {
                m_pMatrix->SetValues(offset+j, i, angularColumn[j]);
            }
        }
        link++;
    }   // Jacobian matrix calculated

    for (i = m_nJoint+1; i < m_nDof; i++)
    {
        int nRow=3;
        if ((m_bOrientation) && (m_bPosition))
            nRow = 6;
        for (int j = 0; j< nRow; j++)
        {
            m_pMatrix->SetValues(j, i, (double)0);
        }                       
    }

    return true;
}

Frame CJacobian::GetJointFrame(int nJoint) const
{
    Frame lastJoint;
    if (nJoint > 0)
    {
        lastJoint = m_pJointMatrix[nJoint-1].Inverse();
    }
    lastJoint *= m_pJointMatrix[nJoint];
    return lastJoint;
}

Frame CJacobian::GetJointWorldFrame(int nJoint) const
{
    return m_pJointMatrix[nJoint];
}

---