Skip to content

F/T센서 값 획득 및 그리퍼 구동

시스템 구성

IndySDK는 공유메모리를 제공하여 다양한 입출력 데이터를 다룰 수 있도록 제공한다. 본 예제에서는 공유메모리를 이용하여 F/T 센서 값을 획득하고 디지털 출력을 이용한 그리퍼 구동에 대한 내용을 다룬다. 예제에서 고려하는 시스템은 엔드이펙터(또는 엔드툴 ; End Tool)에 6축 F/T센서와 24V전원으로 구동되는 전동 그리퍼가 함께 붙어 있고 그리퍼에 온오프(On/Off) 신호를 전달할 수 있도록 스위치가 설치되어있다.


F/T센서, Gripper 및 Digital 입력을 사용하기 위한 시스템 구성 개략도

본 예제에서는 다음을 살펴본다.

  • 공유 메모리를 이용하여 F/T센서 값을 획득한다.
  • 디지털 입출력을 이용하여 그리퍼를 구동한다.

예제 코드 작성

관절 제어 컴포넌트(Joint Control Component)를 생성한 후, 공유메모리를 사용하기 위해서 다음과 같이 헤더파일을 추가하고 ShmemManager 형의 포인터 변수를 선언합니다.

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

공유메모리에 등록된 데이터 구조체 사용을 위해서 다음과 같이 변수들을 선언한다. ExtraIOData 는 컨트롤박스(CB)의 입출력 데이터 정보, RobotControlSharedData 는 로봇의 상태변수들 및 로봇제어에 관련된 정보, EndToolDOSharedData 는 로봇 엔드툴의 디지털 출력에 대한 정보를 나타낸다 (자세한 사항은 IndyFramework 섹션을 참조).

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

다음과 같이 생성자 및 파괴자 부분에 메모리를 할당하고 해제하는 코드를 작성한다.

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

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

F/T센서 값을 획득하기 위해서 다음과 같이 공유메모리에 접근하여 앞서 선언한 변수에 복사한다. F/T 센서 값은 ExtraIOData 를 통해서 미가공 데이터를 직접 획득 할 수도 있고 ControlData 를 통해서 제어기로 부터 가공된 데이터를 획득 할 수도 있다.

 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++;

다음으로 On/Off 스위치를 이용한 그리퍼 구동은 SmartDIO 함수와 setEndToolDO 함수를 이용한다. SmartDIO 를 이용하여 On/Off 스위치의 상태정보를 획득하고 setEndToolDO 함수를 이용하여 그리퍼 구동을 수행한다.

 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);
}

본 예제에서는 F/T센서 기반의 힘 제어를 구현하여 위치제어를 대체하고 직접교시모드에서 사용자가 직접 로봇을 움직여 물체를 집게한다는 가상의 시나리오를 고려한다.

  • F/T센서 값을 위치제어모드(computeControlTorq)에서 획득하도록 구성한다.
  • 직접교시모드(computeGravityTorq)에서 그리퍼를 구동시킬 수 있도록 구성한다.

구체적인 예제는 다음과 같다.

F/T 센서 값 획득

 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;
}

전동 그리퍼 구동

 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;
}