Skip to content

F/T Sensor and Gripper

System Configuration

IndySDK provides a shared memeory for users to handle a variety of input-output data. This example demonstrates how to use F/T sensor and gripper through a shared memory. As shown in the following figure, the entire system considered in the example is configured with 6-axis F/T sensor and 24V electric gripper attached at the end-effector (or end-tool), and moreover, the button-type switch is installed at the control box so as to send on/off command to the gripper


Schematic diagram of system configurations for using F/T sensor, gripper and digital input

This example investigates the followings:

  • How to acquire F/T sensor data through shared memory
  • How to operate grippers with digital output signals

Example Code

Generate a joint control component. Then, to handle shared memory, add the relevant headers and declare the ShmemManager-type pointer like follows:

1
2
#include <NRMKFramework/Indy/Sharedmemory/SharedData.h>
NRMKFramework::ShmemManager * indyShm = NULL;

Define structure variables to get data structures registered in the shared memory; ExtraIOData, RobotControlSharedData, and EndToolDOSharedData denote respectively the information on input-output data of control box(CB), robot state and control data, and digital output data for end-tool of the robot (for more details, refer to IndyFramework section)

1
2
3
NRMKIndy::SharedData::ExtraIOData extioData;
NRMKIndy::SharedData::RobotControlSharedData ctrData;
NRMKIndy::SharedData::EndToolDOSharedData endToolData;

In constructor and destructor, add the code that allocates and releases the memory like follows:

1
2
3
4
5
//========== Contructor
indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);

//========== Destructor
if (indyShm != NULL) delete indyShm;

Acquire F/T sensor data using getExtraIOData and getControlData libraries allowing to access the shared memory like follows. ExtraIoData helps to get the raw data, otherwise, ControlData allows to get the refined data by transformation.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
if (indyShm == NULL)
    printf("Shared memory is missed\n");

NRMKIndy::SharedData::getExtraIOData(*indyShm, extioData);
NRMKIndy::SharedData::getControlData(*indyShm, ctrData);

for (int i=0; i<JOINT_DOF; i++)
{
  Fext[i]       = ctrData.Fext[i];
  FextRaw[i]    = extioData.ftRobotCanRaw[i];
  FextRawTr[i]  = extioData.ftRobotCanTr[i];
}

static unsigned int print_count = 0;
if (print_count % 4000 == 0)
{
  printf("================= Fext ========= FextRaw ======= FextRawTr \n");
  for (int i=0; i<JOINT_DOF; i++)
  {
    printf("Fext[%d]: \t %7.3f \t %7.3f \t %7.3f\n", i,
            Fext[i], FextRaw[i], FextRawTr[i]);
  }
  print_count = 0;
}
print_count++;

Next, the gripper is supposed to be controlled by on/off switch connected to the CB. To this end, SmartDIO and setEndToolDO functions are introduced. They are respectively in charge of getting the states of on/off switch and sending the digital output signals to the gripper.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
static unsigned int gripper_on = 0;
bool b_red = 0, b_yellow = 0;

NRMKIndy::SharedData::getSmartDIO(*indyShm, smartData);

b_red = smartData.smartDI[8];
b_yellow = smartData.smartDI[9];
if (b_red)
    gripper_on = 0;
if (b_yellow)
    gripper_on = 1;

if (gripper_on)
{
  endToolData.endtoolDO[0] = 1;
  NRMKIndy::SharedData::setEndToolDO(*indyShm, endToolData);
}else{
  endToolData.endtoolDO[0] = 0;
  NRMKIndy::SharedData::setEndToolDO(*indyShm, endToolData);
}

As a scenario, this example assumes that users want to replace the internal position controller with the F/T sensor based force controller and make the robot pick the object with the gripper in the direct teaching mode. In summary, it should be possible to

  • Get the F/T sensor data in position control mode (computeControlTorq)
  • Control the gripper with the switch in direct teaching mode (computeGravityTorq)

Consequently, these are respectively implemented as follows:

F/T Sensor Value Acquisition

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
int JointControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec const & qDesired,
JointVec const & qdotDesired, JointVec const & qddotDesired,
JointVec & torque)
{
    if (indyShm == NULL)
      printf("Shared memory is missed\n");

    //data gathering
    NRMKIndy::SharedData::getExtraIOData(*indyShm, extioData);
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);

    for (int i=0; i<JOINT_DOF; i++)
  {
    Fext[i]       = ctrData.Fext[i];
    FextRaw[i]    = extioData.ftRobotCanRaw[i];
    FextRawTr[i]  = extioData.ftRobotCanTr[i];
  }

    static unsigned int print_count = 0;
    if (print_count % 4000 == 0)
    {
        printf("================= Fext ========= FextRaw
            ======= FextRawTr \n");

        for (int i=0; i<JOINT_DOF; i++)
            printf("Fext[%d]: \t %7.3f \t %7.3f \t %7.3f\n", i,
              Fext[i], FextRaw[i], FextRawTr[i]);

        print_count = 0;
    }
    print_count++;

  // Position controller
  if (reset_ctrl)
  {
    robot.resetHinfController();
    reset_ctrl = false;
  }

  robot.setHinfControlGain( _kp, _kv, _ki);
  robot.HinfController(gravDir, qDesired, qdotDesired, qddotDesired, torque);

  return 0;
}

Electric Gripper Operation

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
int JointControlComponent::computeGravityTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec & torque)
{
  // Actuate endtool gripper
    static unsigned int gripper_on = 0;
    bool b_red = 0, b_yellow = 0;

    NRMKIndy::SharedData::getSmartDIO(*indyShm, smartData);

    b_red = smartData.smartDI[8];
    b_yellow = smartData.smartDI[9];
    if (b_red)
        gripper_on = 0;
    if (b_yellow)
        gripper_on = 1;

    if (gripper_on)
    {
        endToolData.endtoolDO[0] = 1;
        NRMKIndy::SharedData::setEndToolDO(*indyShm, endToolData);
    }else{
        endToolData.endtoolDO[0] = 0;
        NRMKIndy::SharedData::setEndToolDO(*indyShm, endToolData);
    }

  // Compute gravitational torque
  JointVec tauGrav;
  robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
  tauGrav = robot.tau();

  // Update torque control input
  torque = tauGrav;

  return 0;
}