planners/ik_mpep/IK_Jacobian.h

Go to the documentation of this file.

//=======================================
// IK_Jacobian.h
// Planner for obstacle avoidance with 
// kinematically redundant robots
//=======================================

#ifndef IK_JACOBIAN_H
#define IK_JACOBIAN_H

#pragma warning( disable : 4250 )

#include <vector>
#include "planners/inversekin/IK_InvKinBase.h"
#include "Kinematics/Jacobian.h"
#include "Robots/R_OpenChain.h"
#include "math/math2.h"

typedef struct 
{
    // no need to record obstacle link number since it's always 0
    int     robFrame;   // the link of the robot where rob_pts lies
    double  distance;   // distance between obs_pts and rob_pts
    double  obs_pts[3]; // coordinate of the point on obstacle closest to robot
    double  rob_pts[3]; // coordinate of the point on robot closest to obstacle, w.r.t frame $robFrame$
} closeValues;

class IK_Jacobian: 
        virtual public IK_InvKinBase
{
public:
    // Default Constructor & Destructor
    IK_Jacobian();
    virtual ~IK_Jacobian();

    virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);
    virtual bool Plan ();

    //Set the given end-effector trajectory/path
        void SetTrajectory(std::vector<Frame> &traj);

    // Set/Get how close the robot can come to obstacles
    void      SetObstacleTolerance(double tol);
    double    GetObstacleTolerance()            {return m_dObsTol;}
    
    //Set/Get homogeneous gain
    void      SetHomogeneousGain(double gain)   {m_dHomoGain = gain;}
    double    GetHomogeneousGain()              {return m_dHomoGain;}
    
    //Set/Get the obstacle distance feedback gain 
    void      SetObstacleDistGain(double gain)  {m_dObsDistGain = gain;}
    double    GetObstacleDistGain()             {return m_dObsDistGain;}

    //Set/Get the path tolerance
    void      SetPathTolerence(double tol)      {m_dPathTol = tol;}
    double    GetPathTolerence()                {return m_dPathTol;}

    //Set/Get the number of obstacle points to be considered
    void      SetNumObstaclePt(int num)         {m_nObsPoint = (num>1)?num:1;}
    int       GetNumObstaclePt()                {return m_nObsPoint;}

    //Get/Set whether or not to consider orienation/position
        void      SetOrientation(bool bOrient)      {m_bOrientation = bOrient;}
        bool      GetOrientation(bool bOrient)      {return m_bOrientation;}
        void      SetPosition(bool bOrient)         {m_bPosition = bOrient;}
        bool      GetPosition(bool bOrient)         {return m_bPosition;}

protected:
    //planner parameter
    int             m_nObsPoint;          // Number of obstacle points to be considered
    bool            m_bPosition;          // Whether or not to consider position, usually true
    bool            m_bOrientation;       // Whether or not to consider orienation;
    double          m_dObsTol;            // Closest distance between robot and obstacles without claiming collision
    double          m_dHomoGain;          // Homogeneous gain
    double          m_dObsDistGain;       // Obstacle gain
    double          m_dAngTol;            // Maximum difference of yaw/roll/pitch acceptable for a given end-eff pose
    double          m_dPathTol;           // Maximum difference of position for a given end-eff pose

    int             m_nDof;               // DOF of the robot, for computation efficiency purpose
    int             m_nToolFrame;         // Obsolete, the index of tool frame;
    Frame           m_frEndEffector;      // The tool frame w.r.t 
    double          m_dTotalObsDist;      // The total distance of all obstacles considered
    R_OpenChain *   m_pRobot;             // to get DH parameters for Jacobian
    
    std::vector<closeValues>  m_vClosestPoints; // used to keep track of the values of the closest points
    std::vector<Frame>        poses;            // given end-effector trajectory, does NOT include start and goal configs

    //Empty, derived from base abstract class
    void        InitNewSearch();

    //Get tool frame, obsolete, it's from the base class: IK_InvKinBase
        //Matrix4x4 GetToolFrame( const Configuration& config ) const;
    
    // finds the closest m_nObsPoint number of points and their distances, 
    bool      GetClosestValues(const Configuration& config);
    
    //Check whether the jont variable execeed the limits.
    void      CheckJointLimits(Configuration& config);

    //Check whether the joint velocity execeed the limits.
    void      CheckJointVelocities(Configuration& config);

    //Compute the distance between two Frames, actully just consider position part.
        double Distance(Frame &frame1, Frame &frame2);
    
    //Computed joint velocity that brought by homogeneous term.
        VectorN ComputeHomogeneousTerm(CJacobian &jacobian, Matrixmxn &mJEnd, Matrixmxn &mJEndInv, VectorN &vEndEffVel);

    //Compute end-effector velocity according the given trajectory;
        VectorN ComputeEndEffVel(Frame &frStart, Frame &frEnd);

};

#endif

---