Skip to content

F/T 센서를 이용한 마찰보상

시스템 구성

F/T 센서를 이용한 기본적인 제어기를 적용해 봄으로써 F/T센서의 좌표계 변환 및 F/T센서의 bias값 초기화에 대해서 설명한다. 이를 위해서 엔드이펙터 (또는 엔드툴; end tool)에 F/T센서가 설치된 로봇에 대해서 설명하며 F/T센서의 부착은 로봇 엔드이펙터의 x축 좌표계와 센서의 x축 좌표계가 일치하도록 설치한다.


F/T센서를 이용한 제어를 수행하기 위한 시스템 구성 개략도

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

  • F/T센서의 좌표계 변환을 수행한다.
  • 변환된 F/T센서 값을 이용하여 로봇의 관성질량 및 마찰효과를 줄여본다.

예제 코드 작성

관절 제어 컴포넌트(Joint Control Component)를 생성하고 F/T센서 값을 획득하기 위하여 공유 메모리 헤더파일을 추가하고 ShmemManager 포인터 변수와 데이터 획득을 위한 구조체 변수들을 선언한다 (공유메모리를 이용하여 F/T센서 값을 획득하는 구체적인 방법은 F/T 센서 값 획득 및 그리퍼 구동를 참고).

1
2
3
4
5
#include <NRMKFramework/Indy/Sharedmemory/SharedData.h>

NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::ExtraIOData extioData;
NRMKIndy::SharedData::RobotControlSharedData ctrData;

관절 제어 컴포넌트 헤더파일에 다음의 함수 및 변수들을 선언한다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
//
typedef typename ROBOT::JointMat JointMat;  //
typedef NRMKFoundation::CompositeHTransformTaskPosition TaskPosition; //
typedef NRMKFoundation::CompositeHTransformTaskVelocity TaskVelocity; //

typedef Eigen::Matrix<double, DIM, 1> TaskVec; //!< Typedef of task vector
typedef Eigen::Matrix<double, DIM, JOINT_DOF> TaskJac; //!< Typedef of task Jacobian matrix

void readFTSensor(ROBOT & robot);
void setBias(){_ftBias = _ftSensor;}

TaskPosition _tposT;
TaskVelocity _tvelT;

LieGroup::Wrench _ftBias;
LieGroup::Wrench _ftSensor;
LieGroup::Rotation _ft_sensor_orientation;

생성자와 파괴자에서 ShmemManager 포인터 변수 indyShm 에 메모리를 할당하고 및 해제하는 코드를 다음과 같이 작성한다.

1
2
3
4
5
6
JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);
}

1
2
3
4
5
6
7
8
JointControlComponent::~JointControlComponent()
{
    if (_robotNom != NULL) delete _robotNom;

    // TODO Auto-generated destructor stub
    if (indyShm != NULL)
        delete indyShm;
}

setTFTSensor() 함수를 이용하여 F/T센서와 엔드이펙터(또는 엔드툴; end tool)간의 좌표계를 설정한다. 아래의 두 가지 예시 중 첫 번째 예시에서는 엔드이펙터와 F/T센서의 좌표계가 z축은 일치하며 x,y축에 대해서는 부호가 반대인 것을 나타내며 두 번째 예시에서는 첫 번째 예시의 상황에서 z축으로 일정한 각도만큼 더 틀어져 있는 것을 나타낸다. 본 예제 에서는 첫 번째 경우에 해당하며 엔드이펙터에 직접 붙어있기 때문에 변위를 0으로 설정한다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
void JointControlComponent::initialize(ROBOT & robot, double delt)
{
  //define nominal robot (this is not the consideration of this example)
  _robotNom = AbstractController::createRobotObj();

  //example 1.
  ft_sensor_orientation = LieGroup::Rotation(-1,0,0,0,-1,0,0,0,1);

  //example 2.
  //ft_sensor_orientation = LieGroup::Rotation(2, -5.0/12.0*M_PI).
  //cascade(LieGroup::Rotation(-1,0,0,0,-1,0,0,0,1));

  robot.setTFTSensor(ft_sensor_orientation, LieGroup::Displacement::Zero());
}

위치제어기는 잘 설계되어 있다고 가정하고 본 예제에서는 직접교시모드에서 F/T센서 값을 이용하여 마찰보상을 적용한다. computeGravityTorq() 에 다음의 코드를 추가하도록 하자 (위치제어기에 대한 자세한 예시는 비선형 최적 위치제어기 참조).

  • readFTsensor 함수를 정의하여 공유메모리로 부터 F/T센서 값을 획득하고 좌표계 변환을 수행한다.
  • 중력보상을 위해 idyn_gravity 함수를 이용하여 중력 값을 계산한다.
  • 엔드이펙터에 가해지는 힘을 관절에 가해지는 힘으로 변환하기 위해서 순기구학을 계산하고 자코비안 행렬을 획득한다.
  • 본 예제에서는 로봇의 관성질량 및 마찰을 감소시키는데에 목적이 있으므로 로봇제어를 위해 사용될 제어 입력(u)는 정의하지 않는다.
 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
int IntertiaReshapingwFTsensor::computeGravityTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec & torque)
{
    if (indyShm == NULL)
    {
        printf("Shared memory is missed\n");
    }

    //read F/T sensor
    readFTSensor(robot);

    //calculate torque control input to compensate for the effect of friction
    JointVec u, tauGrav, tauExt, K_ir;
    TaskJacobian J, Jdot;

    //calculate the gravity
    robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    tauGrav = robot.tau();

    //calculate forward kinematics and jacobian
    robot.computeFK(_tposT, _tvelT);
    robot.computeJacobian(_tposT, _tvelT, J, Jdot);

    //calculate external torque from measured external force
    tauExt = J.transpose()*_ftSensor;

    //define new control input: in this example, this is set to zero
    u.setZero();

    //set inertia and friction reshaping gains
    K_ir << 5.0, 5.0, 3.0, 3.0, 3.0, 3.0;

    //update final control input
    torque = tauGrav - tauExt + K_ir.cwiseProduct(u + tauExt);

    return true;
}

공유메모리로 부터 F/T센서 값을 획득하고 좌표계를 변환하기위해 사용된 readFTSensor 함수는 다음과 같다.

  • getControlData 함수를 이용하여 F/T센서 값을 획득한 후, Bias 값을 반영한다.
  • F/T센서 좌표계에서 표현된 힘의 값을 타겟(엔드 이펙터) 좌표계에 대한 표현으로 변환한다.
  • 변환된 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
void JointControlComponent::readFTSensor(ROBOT & robot)
{
    NRMKIndy::SharedData::getExtraIOData(*indyShm, extioData);
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);

    //reflect the biased force value
    LieGroup::Wrench _ftSensorRaw;
    for (int i=0; i<6; i++) _ftSensorRaw[i] = ctrData.Fext[i];
    _ftSensorRaw -= _ftBias;


    //transform the coordinate
    // * Tft: transformation from reference coordinate to f/t sensor
    // * Ttarget: transformation from reference coordinate to end effector
    LieGroup::HTransform T_ft_target = robot.Ttarget().icascade(robot.Tft());
    LieGroup::Wrench _ftSensorTemp = T_ft_target.itransform(_ftSensorRaw);

    TaskPosition _tartget_pos;
    TaskVelocity _tartget_vel;
    robot.computeFK(_tartget_pos, _tartget_vel);

    LieGroup::Wrench _ftSensorTransformed
    = LieGroup::Wrench(_tartget_pos.R()*_ftSensorTemp.f(), _ftSensorTemp.n());

    for (int i=0; i<3; i++) _ftSensor[i] = _ftSensorTransformed[i+3];
    for (int i=0; i<3; i++) _ftSensor[i+3] = _ftSensorTransformed[i];
}

민감한 힘 제어를 수행하거나 센서의 반복성이 보장이 되지 않을 때 다음과 같이 작업이 종료된 후 재시작 시 bias 값을 업데이트 할 수 있도록 한다. 본 예제에서는 직접교시를 종료 시 센서의 bias 값이 설정되며 재시작 시 리셋을 수행한다.

1
2
3
4
5
6
7
void JointControlComponent::reset()
{
    NRMKIndy::SharedData::getExtraIOData(*indyShm, extioData);
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    for (int i=0; i<6; i++) _ftSensor[i] = ctrData.Fext[i];
    setBias();
}