Line 92: Line 92:
 
     }
 
     }
 
     break;
 
     break;
 +
}
 +
</syntaxhighlight>
 +
 +
- Make more commands by using BHand library api.
 +
<syntaxhighlight lang="cpp">
 +
switch (c)
 +
        {
 +
        case 'q':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_NONE);
 +
            bRun = false;
 +
            break;
 +
 +
        case 'h':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_HOME);
 +
            break;
 +
 +
        case 'r':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_READY);
 +
            break;
 +
 +
        case 'g':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_GRASP_3);
 +
            break;
 +
 +
        case 'k':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_GRASP_4);
 +
            break;
 +
 +
        case 'p':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_PINCH_IT);
 +
            break;
 +
 +
        case 'm':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_PINCH_MT);
 +
            break;
 +
 +
        case 'a':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_GRAVITY_COMP);
 +
            break;
 +
 +
        case 'e':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_ENVELOP);
 +
            break;
 +
 +
        case 'f':
 +
            if (pBHand) pBHand->SetMotionType(eMotionType_NONE);
 +
            break;
 +
        }
 +
 +
</syntaxhighlight>
 +
 +
 +
- Define functions
 +
<syntaxhighlight lang="cpp">
 +
// Load and create grasping algorithm
 +
bool CreateBHandAlgorithm()
 +
{
 +
    if (RIGHT_HAND)
 +
        pBHand = bhCreateRightHand();
 +
    else
 +
        pBHand = bhCreateLeftHand();
 +
 +
    if (!pBHand) return false;
 +
    pBHand->SetMotionType(eMotionType_NONE);
 +
    pBHand->SetTimeInterval(delT);
 +
    return true;
 +
}
 +
 +
// Destroy grasping algorithm
 +
void DestroyBHandAlgorithm()
 +
{
 +
    if (pBHand)
 +
    {
 +
#ifndef _DEBUG
 +
        delete pBHand;
 +
#endif
 +
        pBHand = NULL;
 +
    }
 +
}
 +
 +
// Print program information and keyboard instructions
 +
void PrintInstruction()
 +
{
 +
    printf("--------------------------------------------------\n");
 +
    printf("Keyboard Commands:\n");
 +
    printf("H: Home Position (PD control)\n");
 +
    printf("R: Ready Position (used before grasping)\n");
 +
    printf("G: Three-Finger Grasp\n");
 +
    printf("K: Four-Finger Grasp\n");
 +
    printf("P: Two-finger pinch (index-thumb)\n");
 +
    printf("M: Two-finger pinch (middle-thumb)\n");
 +
    printf("E: Envelop Grasp (all fingers)\n");
 +
    printf("A: Gravity Compensation\n\n");
 +
    printf("F: Servos OFF (any grasp cmd turns them back on)\n");
 +
    printf("Q: Quit this program\n");
 +
    printf("--------------------------------------------------\n\n");
 +
}
 +
</syntaxhighlight>
 +
 +
- Edit main()
 +
<syntaxhighlight lang="cpp">
 +
int main()
 +
{
 +
    printf("myAllegrodHand: ");
 +
    if (RIGHT_HAND) printf("Right Hand, v%i.x\n\n", HAND_VERSION); else printf("Left Hand, v%i.x\n\n", HAND_VERSION);
 +
   
 +
    PrintInstruction();
 +
 +
    memset(&vars, 0, sizeof(vars));
 +
    memset(q, 0, sizeof(q));
 +
    memset(q_des, 0, sizeof(q_des));
 +
    memset(tau_des, 0, sizeof(tau_des));
 +
    memset(cur_des, 0, sizeof(cur_des));
 +
   
 +
    if (CreateBHandAlgorithm() && OpenCAN())
 +
        MainLoop();
 +
 +
    CloseCAN();
 +
    DestroyBHandAlgorithm();
 +
   
 +
    return 0;
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>

Revision as of 16:59, 3 July 2019

BHand Library

You will make....

Please download linux_BHand_tutorial.zip file and follow below steps. File:Linux BHand tutorial.zip
Also, you need to install BHand library. "libBHand": Grasping_Library_for_Linux

Edit main.cpp

- Include BHand library.

#include <BHand/BHand.h>

- Variable declaration for BHand library

// USER HAND CONFIGURATION
const bool	RIGHT_HAND = true;
const int	HAND_VERSION = 4;
 
// for BHand library
BHand* pBHand = NULL;
double q[MAX_DOF]; // joint position
double q_des[MAX_DOF]; // desired joint position used in joint pd control motion
double tau_des[MAX_DOF]; // desired joint torque
double cur_des[MAX_DOF]; // current joint torque  
const double tau_cov_const_v4 = 1200.0; // 1200.0 for SAH040xxxxx

-

 

- Additional function declarations

// functions declarations
char Getch();
void MainLoop();
bool OpenCAN();
void CloseCAN();
bool CreateBHandAlgorithm(); // create
void DestroyBHandAlgorithm(); // destroy
void ComputeTorque(); // torque compute

- Add ComputeTorque function to get desired torque. In this part, you have to make sure that

case ID_RTR_FINGER_POSE_1:
case ID_RTR_FINGER_POSE_2:
case ID_RTR_FINGER_POSE_3:
case ID_RTR_FINGER_POSE_4:
{
    int findex = (id & 0x00000007);
 
    vars.enc_actual[findex*4 + 0] = (short)(data[0] | (data[1] << 8));
    vars.enc_actual[findex*4 + 1] = (short)(data[2] | (data[3] << 8));
    vars.enc_actual[findex*4 + 2] = (short)(data[4] | (data[5] << 8));
    vars.enc_actual[findex*4 + 3] = (short)(data[6] | (data[7] << 8));
    data_return |= (0x01 << (findex)); // counting 4 fingers
 
    // If you have received all four finger data, set new desired torque. 
    // This can create a stable interval time of 3 ms to prevent overshoot.
    if (data_return == (0x01 | 0x02 | 0x04 | 0x08)) 
    {
        // convert encoder count to joint angle
        for (i=0; i<MAX_DOF; i++)
        {
            q[i] = (double)(vars.enc_actual[i])*(333.3/65536.0)*DEG2RAD;
        }
 
        ComputeTorque();
 
        // convert desired torque to desired current and PWM count
        for (int i=0; i<MAX_DOF; i++)
        {
            cur_des[i] = tau_des[i];
            // set limit
            if (cur_des[i] > 1.0) cur_des[i] = 1.0;
            else if (cur_des[i] < -1.0) cur_des[i] = -1.0;
        }
        for (int i=0; i<4;i++)
        {
            vars.pwm_demand[i*4+0] = (short)(cur_des[i*4+0]*tau_cov_const_v4);
            vars.pwm_demand[i*4+1] = (short)(cur_des[i*4+1]*tau_cov_const_v4);
            vars.pwm_demand[i*4+2] = (short)(cur_des[i*4+2]*tau_cov_const_v4);
            vars.pwm_demand[i*4+3] = (short)(cur_des[i*4+3]*tau_cov_const_v4);
 
            command_set_torque(CAN_Ch, i, &vars.pwm_demand[4*i]);
        }
        data_return = 0; 
    }
    break;
}

- Make more commands by using BHand library api.

switch (c)
        {
        case 'q':
            if (pBHand) pBHand->SetMotionType(eMotionType_NONE);
            bRun = false;
            break;
 
        case 'h':
            if (pBHand) pBHand->SetMotionType(eMotionType_HOME);
            break;
 
        case 'r':
            if (pBHand) pBHand->SetMotionType(eMotionType_READY);
            break;
 
        case 'g':
            if (pBHand) pBHand->SetMotionType(eMotionType_GRASP_3);
            break;
 
        case 'k':
            if (pBHand) pBHand->SetMotionType(eMotionType_GRASP_4);
            break;
 
        case 'p':
            if (pBHand) pBHand->SetMotionType(eMotionType_PINCH_IT);
            break;
 
        case 'm':
            if (pBHand) pBHand->SetMotionType(eMotionType_PINCH_MT);
            break;
 
        case 'a':
            if (pBHand) pBHand->SetMotionType(eMotionType_GRAVITY_COMP);
            break;
 
        case 'e':
            if (pBHand) pBHand->SetMotionType(eMotionType_ENVELOP);
            break;
 
        case 'f':
            if (pBHand) pBHand->SetMotionType(eMotionType_NONE);
            break;
        }


- Define functions

// Load and create grasping algorithm
bool CreateBHandAlgorithm()
{
    if (RIGHT_HAND)
        pBHand = bhCreateRightHand();
    else
        pBHand = bhCreateLeftHand();
 
    if (!pBHand) return false;
    pBHand->SetMotionType(eMotionType_NONE);
    pBHand->SetTimeInterval(delT);
    return true;
}
 
// Destroy grasping algorithm
void DestroyBHandAlgorithm()
{
    if (pBHand)
    {
#ifndef _DEBUG
        delete pBHand;
#endif
        pBHand = NULL;
    }
}
 
// Print program information and keyboard instructions
void PrintInstruction()
{
    printf("--------------------------------------------------\n");
    printf("Keyboard Commands:\n");
    printf("H: Home Position (PD control)\n");
    printf("R: Ready Position (used before grasping)\n");
    printf("G: Three-Finger Grasp\n");
    printf("K: Four-Finger Grasp\n");
    printf("P: Two-finger pinch (index-thumb)\n");
    printf("M: Two-finger pinch (middle-thumb)\n");
    printf("E: Envelop Grasp (all fingers)\n");
    printf("A: Gravity Compensation\n\n");
    printf("F: Servos OFF (any grasp cmd turns them back on)\n");
    printf("Q: Quit this program\n");
    printf("--------------------------------------------------\n\n");
}

- Edit main()

int main()
{
    printf("myAllegrodHand: ");
    if (RIGHT_HAND) printf("Right Hand, v%i.x\n\n", HAND_VERSION); else printf("Left Hand, v%i.x\n\n", HAND_VERSION);
 
    PrintInstruction();
 
    memset(&vars, 0, sizeof(vars));
    memset(q, 0, sizeof(q));
    memset(q_des, 0, sizeof(q_des));
    memset(tau_des, 0, sizeof(tau_des)); 
    memset(cur_des, 0, sizeof(cur_des)); 
 
    if (CreateBHandAlgorithm() && OpenCAN())
        MainLoop();
 
    CloseCAN();
    DestroyBHandAlgorithm();
 
    return 0;
}




Whos here now:   Members 0   Guests 0   Bots & Crawlers 1