Skip to content

작업공간 임피던스 제어

시스템 및 제어기 구성

본 예제에서는 관절공간 임피던스 제어를 작업공간으로 확장한다. 즉, 소개 될 제어기는 역 자코비안을 이용하여 작업공간의 목표궤적을 관절공간에서 정의하며 관절공간 임피던스 제어기를 이용하며 레귤레이션 제어를 수행한다. 예제에 앞서, 시스템으로는 엔드이펙터(end-effector)에 F/T 센서가 부착된 Indy7을 사용하며 F/T센서를 통해서만 상호작용이 발생한다고 가정한다 (F/T센서 설치에 대한 자세한 사항은 작업공간 임피던스 제어를 참고하기 바란다).

본 예제는 다음을 구현하는데에 목적이 있다.

  • 관절 임피던스 제어를 이용한 작업공간 위치제어
  • 역 자코비안을 이용한 관절공간 오차 획득

Note

안전상의 이유로 위치제어오차가 일정수준 이상일 때 실제로봇은 비상정지 되도록 되어있다. 즉, 작성된 알고리즘이 시뮬레이션 환경에서는 제어이득에 관계없이 잘 작동하지만 실제로봇에서는 비상정지 상황이 발생할 수 있다.

예제 코드

먼저 개발환경에서 Task Control Component 를 생성한다. 다음으로 SharedData.h 를 추가하고 공유메모리를 다루기 위해서 ShmemManager 를 정의한다 (공유메모리는 F/T센서 값을 획득하기 위해서 사용된다).

1
2
3
4
//for the use of shared memeory
#include "NRMKFramework/Indy/Sharedmemory/SharedData.h"
NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::RobotControlSharedData ctrData; // control data

생성된 헤더파일의 TaskControlComponent 클래스를 아래 작성된 코드와 같이 수정하도록 한다.

  • JointVec 과 JointMat 을 정의한다.
  • 임피던스 제어 함수를 정의하기 위해서 computeImpedanceControlTorq 함수를 선언한다.
  • 매 제어마다 시스템을 초기화 해주기 위한 초기화 함수를 정의하기 위해 resetNominalSystem 함수를 선언한다.
  • F/T 센서 편향을 보상하기 위해서 편항 값을 설정하는 함수 setBias 를 선언한다.
  • F/T 센서로 부터 측정된 값을 엔드이펙터의 Wrench 값으로 변환하기 위해 readFTSensor 를 사용한다.
  • 그 외 몇 가지 필요한 변수들을 정의한다 (자세한 사항은 주석 참조).
 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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
class TaskControlComponent : public AbstractTaskController<AbstractRobot6D>
{
    enum
    {
        JOINT_DOF = AbstractRobot6D::JOINT_DOF
    };
    typedef AbstractTaskController<AbstractRobot6D> AbstractController;
    typedef Eigen::Matrix<double, 6, 1> TaskVec;
    typedef Eigen::Matrix<double, 6, 6> TaskJacobian;
    typedef typename ROBOT::JointVec JointVec;
    typedef typename ROBOT::JointMat JointMat;

public:
    TaskControlComponent();
    virtual ~TaskControlComponent();

    void initialize(ROBOT & robot, double delt);
    void setPassivityMode(bool enable);
    void setGains(TaskVec const & kp, TaskVec const & kv, TaskVec const & ki);
    void setImpedanceParams(TaskVec const & iMass, TaskVec const & iDamping,
    TaskVec const & iStiffness, TaskVec const & iKi = TaskVec::Zero());

    void setImpedanceGains(TaskVec const & iKp, TaskVec const & iKv, TaskVec
    const & iKi, TaskVec const & iKf = TaskVec::Zero());

    void reset();
    int computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir,
    TaskPosition const & posDesired, TaskVelocity const & velDesired,
    TaskAcceleration const & accDesired, JointVec & torque);

    void computeImpedanceControlTorq(TaskPosition const & posDesired,
    TaskVelocity const & velDesired, JointVec const & q, JointVec const &
    qdot, JointVec & torque);

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

private:
    ROBOT * _robotNom;
    // flag to initialize nominal robot
    bool _reset_ctrl;
    // flag to set control mode (true: simulation mode, false: real mode)
    bool _sim_mode;
    // sampling time
    double _delT;
private:
    //Task space position (R,r) and velocity (v,w)
    TaskPosition _tpos, _tposNom;
    TaskVelocity _tvel, _tvelNom;
    // Task jacobian
    TaskJacobian _J, _JNom;
    TaskJacobian _Jdot, _JdotNom;
    // Robot dynamic parameters
    JointMat _Mn, _Cn, _Dn, _Mhat;
    JointVec _gn;
    // control input for gravity compensation of real and nominal robots
    JointVec _tauGrav, _tauGravNom;

private:
    // with respect to ft sensor
    JointVec _tauExt;
    LieGroup::Wrench    _ftBias;
    LieGroup::Wrench    _ftSensor;
    LieGroup::Rotation  _ft_sensor_orientation;
};

생성자에서 포인터 변수 indyShm 에 메모리를 할당하고 공유메모리로 부터 F/T 센서 데이터를 획득할 수 있도록 한다 (더욱 자세한 사항은 IndyFramework나 F/T센서 사용 및 그리퍼 구동 예제를 참고하기 바란다).

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
TaskControlComponent::TaskSpaceComplianceControl()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{

  // initialize shared memory
    // INDY_SHM_NAME: "indySHM"
    // INDY_SHM_LEN: 0x1000000 16MB == 0x000000~0xFFFFFF (~16777216)
    indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);
    for (int i=0; i<6; i++) _ftBias[i] = 0;

    // initialization
    _delT = 0.00025;
    _Dn.setZero();
    _J.setZero();
    _Jdot.setZero();
    _Mhat.setZero();
    _tauExt.setZero();
}

파괴자에서는 공칭로봇 및 공유메모리에 할당된 메모리를 헤제한다.

1
2
3
4
5
6
7
8
9
TaskControlComponent::~TaskSpaceComplianceControl()
{
  // destroy nominal robot
    if (_robotNom != NULL) delete _robotNom;

    // destroy shared memory
    if (indyShm != NULL)
        delete indyShm;
}

위에서 언급했던 것과 같이 computeImpedanceControlTorq 함수는 관절공간 임피던스 제어기를 이용하여 작업공간에서 레귤레이션 제어를 구현하기 위해서 정의되었다. 본 예제에서는 비례-미분 유형의 임피던스 제어방법을 사용하도록 한다.

 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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
void TaskControlComponent::computeImpedanceControlTorq(TaskPosition const
& posDesired, TaskVelocity const & velDesired, JointVec const & q, JointVec
const & qdot, JointVec & torque)
{

    // calculate jacobian inverse
    TaskJacobian JJtrans;
    JJtrans.setZero();
    JJtrans = _J*_J.transpose();
    Eigen::ColPivHouseholderQR<TaskJacobian> qr(JJtrans);

    TaskJacobian Jinv;
    Jinv.setZero();
    Jinv = _J.transpose()*qr.inverse();

    // calculate task error
    TaskVec e, edot;
    e.setZero();
    edot.setZero();

    _robotNom->computeTaskErr(_tposNom, _tvelNom, posDesired, velDesired,
                            e, edot);

    JointMat kp, kd;
    kp.setZero();
    kd.setZero();

    for (int i=0; i < JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            kp(i,i) = 70;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 25;
            break;
        case 2:
            kp(i,i) = 25;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 15;
            break;
        case 3:
        case 4:
            kp(i,i) = 25;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 15;
            break;
        case 5:
            kp(i,i) = 15;
            kd(i,i) = 5;
            break;
        }
    }

    torque = kp*(Jinv*e) - kd*qdot;
}

앞서 소개된 비례-미분 형태의 임피던스 제어방법은 로봇내 존재하는 마찰효과 및 로봇에 작용하는 중력의 영향으로 인해서 적절하게 동작하지 않는다. 이 문제를 해결하기 위해서 강인제어기를 내부루프에 구성하도록하며 computeControlTorque() 함수에 작성한다. 이는 결과적으로 마찰의 효과를 보상하며 시스템이 안정한 범위내에서 관절질량을 줄여주는 효과를 가져다 준다.

  • 외력감지를 위해서 F/T 센서 데이터를 공유메모리로 부터 획득한다. F/T센서 사용 및 그리퍼 구동예제에서 언급되었던 것과 같이 사용자는 ControlData 구조체로 부터 F/T 센서 데이터를 획득할 수 있다. 이와 관련된 내용은 아래에서 소개 될 readFTSensor 함수에 정의된다.

  • F/T 센서에서 획득한 힘/토크 값의 구성을 변경한다. 이는 측정된 값은 (힘, 모멘트)의 순으로 값이 획득되지만 IndySDK에서 사용되는 Wrench의 값은 (모멘트, 힘) 순서로 정의되기 때문이다. 이와 관련된 내용은 아래에서 소개 될 readFTSensor 함수에 정의된다.

  • 새로운 모션이 시작할 때 공칭로봇의 기구학적 정보를 실제 로봇의 기구학적 정보로 초기화 한다. 그 후, 마찬가지로 로봇의 상태변수와 속도 기구학 정보를 업데이트 한다. 이와 관련된 내용은 아래에서 소개 될 resetNominalSystem 함수에 정의된다.

  • 실제로봇과 공칭로봇의 순기구학을 계산하며 측정, 계산된 외력(tauExt), 중력 힘(tauGrav), 임피던스 제어 값(tauImp)을 기반으로 공칭로봇의 가속도 값(qddotNom)을 계산한다.

  • 마지막으로, 마찰 및 모델 불확실성 등을 보상하기 위해서 공칭로봇과 실제로봇간의 오차를 기반으로 하여 보조입력 값을 계산한다.

  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
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
int TaskControlComponent::computeControlTorq(ROBOT & robot, LieGroup::Vector3D
const & gravDir, TaskPosition const & posDesired, TaskVelocity const &
velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    // read ft sensor value; the function is defined below
    readFTSensor(robot);

    // update initial states of nominal robot with those of real robot
    if(_reset_ctrl)
    {
        printf("initialized\n");

        //reset nominal system "robotNom"; the function is defined below
        resetNominalSystem(robot);

        //init_robotNom = false;
        _reset_ctrl = false;
    }

    // calculate robot dynamics
    _robotNom->computeDynamicsParams(gravDir, _Mn, _Cn , _gn);
    _Mhat = _Mn;

    for (int i = 0; i < JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 40;
            break;
        case 2:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 20;
            break;
        case 3:
        case 4:
        case 5:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 10;
            break;
        }
    }

    robot.idyn_gravity(gravDir);
    robot.computeFK(_tpos, _tvel);
    robot.computeJacobian(_tpos, _tvel, _J, _Jdot);

    _robotNom->idyn_gravity(gravDir);
    _robotNom->computeFK(_tposNom, _tvelNom);
    _robotNom->computeJacobian(_tposNom, _tvelNom, _JNom, _JdotNom);

    _tauExt = _J.transpose()*_ftSensor;
    _tauGrav = robot.tau();
    _tauGravNom = _robotNom->tau();

    //calculate impedance control input
    JointVec tauImp, tauAux;
    tauImp.setZero();
    tauAux.setZero();

    computeImpedanceControlTorq(posDesired, velDesired, _robotNom->q(),
                                _robotNom->qdot(), tauImp);

    JointVec K, KC, KD;
    if (_sim_mode)
    {
        K << 1.0, 1.0, 1.0, 1.0, 1.0, 0.02;
        KC << 80, 80, 40, 25, 25, 25;
        KD << 55, 55, 30, 15, 15, 15;
    }
    else
    {
        K << 5.0, 5.0, 5.0, 5.0, 5.0, 5.0;
        KC << 80, 80, 40, 25, 25, 25;
        KD << 55, 55, 30, 15, 15, 15;
    }

    JointVec qddotNom, err, err_dot;
    qddotNom.setZero();
    err.setZero();
    err_dot.setZero();

    qddotNom = _Mhat.inverse()*(-(_Cn + 3.0*_Dn)*_robotNom->qdot()
              + K.cwiseProduct(tauImp + _tauExt));

    // update nominal states
    _robotNom->qdot() += (float) _delT*qddotNom;
    _robotNom->q() += (float) _delT*_robotNom->qdot();
    _robotNom->update();
    _robotNom->updateJacobian();

    // define errors between real and nominal robots
    err = _robotNom->q() - robot.q();
    err_dot = _robotNom->qdot() - robot.qdot();

    // calculate auxiliary control input
    tauAux = 1.0*KC.cwiseProduct(err) + 1.0*KD.cwiseProduct(err_dot);

  // final robust impedance control input
    torque = _tauGrav + _tauExt + K.cwiseProduct(tauImp + tauAux + _tauExt);

    return 0;
}

readFTSensor 함수는 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
void TaskControlComponent::readFTSensor(ROBOT & robot)
{

    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    TaskPosition _tposT;
    TaskVelocity _tvelT;
    LieGroup::Wrench _ftSensorTemp;
    _ftSensorTemp.setZero();

    // to transform the coordinate from F/T sensor frame to global frame
    robot.computeFK(_tposT, _tvelT);

    for (int i=0; i<6; i++) _ftSensorTemp[i] = ctrData.Fext[i];
    _ftSensorTemp = _ftSensorTemp - _ftBias;

    LieGroup::HTransform T_ft_target = robot.Ttarget().icascade(robot.Tft());
    LieGroup::Wrench _ftSensorTemp2 = T_ft_target.itransform(_ftSensorTemp);
    LieGroup::Wrench _ftSensorTransformed =
    LieGroup::Wrench(_tposT.R()*_ftSensorTemp2.f(), _ftSensorTemp2.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];
}

공칭로봇의 기구학적 정보 업데이트는 resetNominalSystem 함수에 아래와 같이 구현된다.

1
2
3
4
5
6
7
8
9
void TaskControlComponent::resetNominalSystem(ROBOT & robot)
{
    _robotNom->setTtarget(robot.Ttarget().R(), robot.Ttarget().r());
    _robotNom->setTReference(robot.Tref().R(), robot.Tref().r());
    _robotNom->q() = robot.q();
    _robotNom->qdot() = robot.qdot();
    _robotNom->update();
    _robotNom->updateJacobian();
}

그 외 initialize와 reset 함수들은 로봇 제어를 준비하는 단계에서 적용되며 본 예제에서는 다음과 같이 구현된다.

  • createRobotObj 함수는 메모리를 robotNom에 할당하기 위해서 사용한다.
  • setFTSensor 함수는 엔드이펙터와 F/T 센서 간의 좌표계 변환을 반영하기 위해서 사용한다.
  • reset 함수에서는 F/T 센서 편향 값을 보상하기 위해서 F/T 센서 값을 측정한다.
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
void TaskControlComponent::initialize(ROBOT & robot, double delt)
{
    // create nominal robot
    _robotNom = AbstractController::createRobotObj();

    // check shared memory
    if (indyShm == NULL)
    {
        printf("Shared memory is missed\n");
    }

    // align ft sensor coordinates with robot's tool frame's coordinates
    _ft_sensor_orientation = LieGroup::Rotation(1,0,0,0,1,0,0,0,1);
    robot.setTFTSensor(_ft_sensor_orientation, LieGroup::Displacement::Zero());

    // initialize task error calculator
    robot.initTaskErr(delt);
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
void TaskControlComponent::reset()
{
    // to initialize nominal robot
    _reset_ctrl = true;

    // sim_mode/real_mode
    _sim_mode = true;

   // get ft sensor values from shared memory
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    for (int i=0; i<6; i++) _ftSensor[i] = ctrData.Fext[i];

    // compensate ft sensor bias
    setBias();
}