BHand.h

Go to the documentation of this file.

/* Allegro, Copyright 2012 SimLab Co., Ltd. All rights reserved.
 *
 * This library is commercial and cannot be redistributed, and/or modified
 * WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
 */

#ifndef __BHAND_H__
#define __BHAND_H__

#include "BHandDef.h"

#define NOF 4 // number of fingers
#define NOJ 4 // number of joints in each finger
#define SIZEOF_VARRAY (NOF*NOJ*8)

enum eMotionType
{
        eMotionType_NONE,                               
        eMotionType_HOME,                               
        eMotionType_READY,                              
        eMotionType_PRE_SHAPE,                  
        eMotionType_GRASP_3,                    
        eMotionType_GRASP_4,                    
        eMotionType_PINCH_IT,                   
        eMotionType_PINCH_MT,                   
        eMotionType_OBJECT_MOVING,              
        eMotionType_ENVELOP,                    
        eMotionType_JOINT_PD,                   
        NUMBER_OF_MOTION_TYPE
};

enum eHandType
{
        eHandType_Left = 0,                             
        eHandType_Right                                 
};

class BHANDEXPORT BHand
{
public:
        BHand(eHandType ht);
        ~BHand(void);

        eHandType GetType();

        void SetTimeInterval(double dT);

        void SetMotionType(int motionType);

        void SetJointPosition(double* q);

        void UpdateControl(double time);

        void GetJointTorque(double* tau);

        void GetFKResult(double x[4], double y[4], double z[4]);

        void SetJointDesiredPosition(double* q);

        void SetGainsEx(double* kp, double* kd);

        void SetGraspingForce(double f[4]);

        void GetGraspingForce(double fx[4], double fy[4], double fz[4]);

private:
        BHand();
        void SetGains(int motionType);
        void SolveFK();
        void SolveFKLeft();
        void SolveFKRight();
        void CalculateJacobian();
        //void CalculateJacobianLeft();
        //void CalculateJacobianRight();
        void CalculateGravity();
        //void CalculateGravityLeft();
        //void CalculateGravityRight();

        void Motion_HomePosition();
        void Motion_Ready();
        void Motion_PreShape();
        void Motion_Grasp3();
        void Motion_Grasp4();
        void Motion_PinchIT();
        void Motion_PinchMT();
        void Motion_ObjectMoving();
        void Motion_Envelop();
        void Motion_JointPD();

private:
        double _dT;                                                     
        double _curT;
        
        eHandType _handType;                            
        eMotionType _motionType;                        
        
        double _q[NOF][NOJ];                            
        double _q_filtered[NOF][NOJ];           
        double _q_pre[NOF][NOJ];                        
        double _q_filtered_pre[NOF][NOJ];       
        double _qdot[NOF][NOJ];                         
        double _qdot_filtered[NOF][NOJ];        
        double _qdot_pre[NOF][NOJ];                     
        double _tau_des[NOF][NOJ];                      
        double _q_des[NOF][NOF];                        

        double _kp[NOF][NOJ];                           
        double _kd[NOF][NOJ];                           
        double _kp_task[NOF][NOJ];                      
        double _kd_task[NOF][NOJ];                      

        double _x[NOF];                                         
        double _y[NOF];                                         
        double _z[NOF];                                         
        double _x_filtered[NOF];                        
        double _y_filtered[NOF];                        
        double _z_filtered[NOF];                        
        double _x_pre[NOF];                                     
        double _y_pre[NOF];                                     
        double _z_pre[NOF];                                     
        double _x_filtered_pre[NOF];            
        double _y_filtered_pre[NOF];            
        double _z_filtered_pre[NOF];            
        
        double _xdot[NOF];                                      
        double _ydot[NOF];                                      
        double _zdot[NOF];                                      
        double _xdot_filtered[NOF];                     
        double _ydot_filtered[NOF];                     
        double _zdot_filtered[NOF];                     
        double _xdot_pre[NOF];                          
        double _ydot_pre[NOF];                          
        double _zdot_pre[NOF];                          

        double _J[NOF][3][NOJ];                         

        double _G[NOF][NOJ];                            

        double _f_des[NOF];                                     

        double _x_des[NOF];                                     
        double _y_des[NOF];                                     
        double _z_des[NOF];                                     

        double S1_C[NOJ], S2_C[NOJ], S3_C[NOJ], S4_C[NOJ], S23_C[NOJ], S234_C[NOJ], S34_C[NOJ];
        double C1_C[NOJ], C2_C[NOJ], C3_C[NOJ], C4_C[NOJ], C23_C[NOJ], C234_C[NOJ], C34_C[NOJ];

        double _mass[NOF][NOJ];                         
};

BHAND_EXTERN_C_BEGIN

BHANDEXPORT BHand* bhCreateLeftHand();

BHANDEXPORT BHand* bhCreateRightHand();

BHAND_EXTERN_C_END

#endif // __BHAND_H__