F/T센서 값 획득 및 그리퍼 구동
시스템 구성
IndySDK는 공유메모리를 제공하여 다양한 입출력 데이터를 다룰 수 있도록 제공한다. 본 예제에서는 공유메모리를 이용하여 F/T 센서 값을 획득하고 디지털 출력을 이용한 그리퍼 구동에 대한 내용을 다룬다. 예제에서 고려하는 시스템은 엔드이펙터(또는 엔드툴 ; End Tool)에 6축 F/T센서와 24V전원으로 구동되는 전동 그리퍼가 함께 붙어 있고 그리퍼에 온오프(On/Off) 신호를 전달할 수 있도록 스위치가 설치되어있다.
F/T센서, Gripper 및 Digital 입력을 사용하기 위한 시스템 구성 개략도
본 예제에서는 다음을 살펴본다.
- 공유 메모리를 이용하여 F/T센서 값을 획득한다.
- 디지털 입출력을 이용하여 그리퍼를 구동한다.
예제 코드 작성
관절 제어 컴포넌트(Joint Control Component)를 생성한 후, 공유메모리를 사용하기 위해서 다음과 같이 헤더파일을 추가하고 ShmemManager 형의 포인터 변수를 선언합니다.
| #include <NRMKFramework/Indy/Sharedmemory/SharedData.h>
NRMKFramework::ShmemManager * indyShm = NULL;
|
공유메모리에 등록된 데이터 구조체 사용을 위해서 다음과 같이 변수들을 선언한다. ExtraIOData 는 컨트롤박스(CB)의 입출력 데이터 정보, RobotControlSharedData 는 로봇의 상태변수들 및 로봇제어에 관련된 정보, EndToolDOSharedData 는 로봇 엔드툴의 디지털 출력에 대한 정보를 나타낸다 (자세한 사항은 IndyFramework 섹션을 참조).
| NRMKIndy::SharedData::ExtraIOData extioData;
NRMKIndy::SharedData::RobotControlSharedData ctrData;
NRMKIndy::SharedData::EndToolDOSharedData endToolData;
|
다음과 같이 생성자 및 파괴자 부분에 메모리를 할당하고 해제하는 코드를 작성한다.
| //========== 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;
}
|