Skip to content

튜토리얼

로봇 매니퓰레이터 PD 제어

본 장에서는 IndySDK 사용방법에 대한 개발자의 이해를 돕기 위해 IndySDK를 이용하여 관절공간 PD 제어기를 구현하는 예제를 튜토리얼로써 제공합니다.

PD 제어 는 다자유도 로봇 매니퓰레이터의 수동성 (passivity)과 점근 안정성 (asymptotic stability)을 보장하는 제어기로 잘 알려져 있습니다. 본 튜토리얼에서는 이러한 PD 제어를 직접 구현할 수 있도록 step-by-step 예제를 제공합니다. 이론적인 내용은 다음 링크의 리뷰논문을 참조해주시기 바랍니다.

IndySDK 컴포넌트 생성

  • NRMK Launcher 를 실행 후 Eclipse 아이콘을 누르면 Eclipse IDE가 실행됩니다.
  • Eclipse가 실행되면 File > New Project > C++ Project > C++ Managed Build 를 차례로 클릭한 후 Next 버튼을 누릅니다.
  • 프로젝트 생성창이 뜨면 Project name 을 쓰고 Project typeShared Library > Joint Control Component 선택합니다.
  • Toolchains 으로 Cygwin GCC 선택 후 Next를 누릅니다.
  • Controller Name을 기입하고 Robot Type은 로봇의 자유도를 선택 (Indy7은 6-DOF, IndyRP2는 7-DOF)한 후 Finish 버튼을 누르면 프로젝트 생성이 완료 됩니다.


Joint Control Component 생성

프로젝트가 생성되면 아래 그림과 같이 생성된 프로젝트의 템플릿을 담고 있는 헤더파일과 cpp 파일이 자동으로 생성됩니다.


자동 생성된 Joint Control Component 템플릿

프로젝트 우클릭 후 Build Configuration > Set Active > Release 를 통해 릴리즈를 Active로 설정하십시오.


Release로 빌드 환경 설정

프로젝트 설정

STEP2에 대한 프로젝트 설정

아래 그림과 같이 프로젝트 우클릭 후 Properties 창을 연 후 C/C++ Build > Environment > PATHNRMK_TOOLCHAIN_PC2 변수를 추가하십시오.

Note

NRMK_TOOLCHAIN_PC2 변수를 경로에 추가할 때 세미콜론(;) 을 변수들 사이에 적절하게 입력하였는지 확인바랍니다 (위 그림에서는 NRMK_TOOLCHAIN_PC2와 CYGWIN_HOME 변수 사이에 추가함).


Project properties (build environment) 설정 (STEP2)

다음으로 C/C++ Build > Settings 에서 Command 를 아래 그림과 같이 설정하십시오.

  • GCC Assembler Command: i686-unknown-linux-gnu-as
  • Cygwin C++ Compiler Command: i686-unknown-linux-gnu-g++
  • Cygwin C Compiler Command: i686-unknown-linux-gnu-gcc
  • Cygwin C++ Linker Command: i686-unknown-linux-gnu-g++


Project properties (build settings) 설정 (STEP2)

Build Artifact 탭에서 Artifact extensioncomp 로 수정하십시오. Artifact Type은 Shared Library 가 되어야 합니다. Artifact name의 디폴트 값은 프로젝트 이름이며 본 예제에서는 임의로 JointGravityPDControl로 바꾸었습니다.


Project properties (build settings) 설정 (STEP2)

참고

위 예제처럼 Artifact name을 프로젝트 이름이 아닌 다른이름으로 바꿀경우 .cpp 코드 맨 아래 부분의 코드가 수정되어야 합니다.

  • (변경 전) typedef JointPDControl IndySDKTest;
  • (변경 전) POCO_EXPORT_CLASS(IndySDKTest)
  • (변경 후) typedef JointPDControl JointGravityPDControl;
  • (변경 후) POCO_EXPORT_CLASS(JointGravityPDControl)

본 예제에서 프로젝트 이름이 IndySDKTest 이기 때문에 자동으로 생성된 코드입니다. Artifact name을 임의로 JointGravityPDControl로 바꾸었기 때문에 코드 부분도 수정이 되어야 합니다. Artifact name을 디폴트 ${ProjName} 그대로 사용하실 경우 해당되지 않습니다.

STEP3에 대한 프로젝트 설정

아래 그림과 같이 프로젝트 우클릭 후 Properties 창을 연 후 C/C++ Build > Environment > PATHNRMK_TOOLCHAIN_PC3 변수를 추가하십시오.

Note

NRMK_TOOLCHAIN_PC3 변수를 경로에 추가할 때 세미콜론(;) 을 변수들 사이에 적절하게 입력하였는지 확인바랍니다 (위 그림에서는 NRMK_TOOLCHAIN_PC3와 CYGWIN_HOME 변수 사이에 추가함).


Project properties (build environment) 설정 (STEP3)

다음으로 C/C++ Build > Settings 에서 Command 를 아래 그림과 같이 설정하십시오.

  • GCC Assembler Command: x86_64-unknown-linux-gnu-as
  • Cygwin C++ Compiler Command: x86_64-unknown-linux-gnu-g++
  • Cygwin C Compiler Command: x86_64-unknown-linux-gnu-gcc
  • Cygwin C++ Linker Command: x86_64-unknown-linux-gnu-g++


Project properties (build settings) 설정 (STEP3)

나머지 설정과정은 STEP2의 설정과정과 같습니다.

사용자 라이센스 정보 기입

IndySDK를 이용하여 개발한 컴포넌트를 IndyFramework에서 사용하기 위해서 뉴로메카에서 발급한 라이센스 정보를 아래그림과 같이 JointPDControl.cpp 상단에 기입합니다. 라이센스 정보를 기입한 후 컴포넌트를 빌드하여야 IndyFramework에 적용하여 사용할 수 있다는 점을 상기하십시오.

1
2
3
4
5
/***** License Information *****/
#define USERNAME "username"
#define EMAIL "user@company.com"
#define SERIAL "AABBCC112233"
/******************************/

이제 Joint Control Component 를 사용할 준비가 끝났습니다. Joint Control Component는 제어모드 표에서 볼 수 있듯이 세 가지 모드에서 활성화 됩니다. 첫 번째는 준비단계로 관절공간 제어기가 자세를 유지하기 위해 사용됩니다. 두 번째로 관절공간 궤적 추종 제어를 위해서 사용됩니다. 마지막으로 직접교시를 위해 중력보상제어기가 사용됩니다. 그러므로 Joint Control Component 에서는 두 가지 제어기 (관절공간 제어기 및 중력보상 제어기)에 대한 설계가 수행되어야 합니다.

중력보상 제어기 구현

중력보상 제어기는 앞서 언급한 대로 직접교시 모드에서 활성화 되며 computeGravityTorq 함수에 구현되어야 합니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
int JointPDControl::computeGravityTorq(ROBOT & robot, JointVec & torque)
{
    // Compute gravitational torque
    JointVec tauGrav;
    robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    tauGrav = robot.tau();

    // Update torque control input
    torque = tauGrav;

    return true;
}

여기서 robot 객체의 idyn_gravity() 함수를 사용하여 로봇의 중력토크를 구하고, 이를 제어 입력으로 넣어준 예제입니다.

관절공간 제어기 구현

관절공간 제어기는 조인트무브와 준비상태일 때 활성화 되며 computeControlTorq 함수내에 구현되어야 합니다. 본 예제에서는 중력보상제어와 PD 제어를 동시에 구현합니다.

우선 Matrix 타입의 제어이득 값을 설정하기 위해서 JointPDControl.h 헤더파일의 클래스 정의에 아래와 같이 typedef typename ROBOT::JointMat JointMat 키워드 선언을 하십시오.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
class JointPDControl : public AbstractJointController<AbstractRobot6D>
{
    enum
    {
        JOINT_DOF = AbstractRobot6D::JOINT_DOF
    };
    typedef AbstractRobot6D ROBOT;
    typedef typename ROBOT::JointVec JointVec;
    typedef typename ROBOT::JointMat JointMat;
                             ...

그리고 JointPDControl.cpp 파일의 computeControlTorq 함수에 아래와 같이 중력보상 PD 제어기를 구현하십시오.

 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
int JointPDControl::computeControlTorq(
                ROBOT & robot,
                JointVec const & qDesired,
                JointVec const & qdotDesired,
                JointVec const & qddotDesired,
                JointVec & torque)
{
    JointMat kp, kd;
    JointVec tauGrav;
    JointVec tauPD;

    // Initialize local variables
    kp.setZero();
    kd.setZero();
    tauPD.setZero();
    tauGrav.setZero();

    // Set PD control gains
    for (int i = 0; i < JOINT_DOF; ++i)
    {
        switch(i)
        {
        case 0:
        case 1:
            kp(i,i) = 70;
            kd(i,i) = 55;
            break;
        case 2:
            kp(i,i) = 40;
            kd(i,i) = 30;
            break;
        case 3:
        case 4:
            kp(i,i) = 25;
            kd(i,i) = 15;
            break;
        case 5:
            kp(i,i) = 18;
            kd(i,i) = 3;
            break;
        }
    }

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

    // Joint-space PD control input
    tauPD = kp*(qDesired - robot.q()) - kd*robot.qdot() + tauGrav;

    // Update torque control input
    torque = tauPD;

    return true;
}

다축 로봇 매니퓰레이터를 위한 중력보상제어와 PD 제어

중력토크는 앞서 사용한 것과 같이 역동역학을 통해 계산됩니다. 그리고 관절공간 PD 제어기는 함수인자 qDesired와 robot 객체의 q, qdot 을 이용하여 계산합니다. 이후 PD 제어기와 관절토크 값을 더하여 최종 입력토크를 업데이트 합니다.

빌드 및 실행하기

코드 작성이 완료되었으니 이제 프로젝트를 빌드 해야 합니다. Project Explorer의 프로젝트를 클릭 후 Project > Build Project 를 클릭하시거나, 프로젝트 이름 우클릭후 Build Project 를 누르시면 빌드가 됩니다.


빌드가 완료되면 Release 경로에 컴포넌트 파일 (공유라이브러리)이 생성됩니다.


그림 6. 컴포넌트 파일 생성

이제 생성된 컴포넌트 파일을 STEP으로 옮긴 후 실행할 수 있습니다.

생성 된 컴포넌트파일 (JointGravityPDControl.comp)을 STEP의 /home/user/release/LeanDeployment/Component 폴더 디렉토리에 복사합니다. 그리고 JSON 설정파일에 빌드한 컴포넌트 이름을 기입합니다. JSON 설정파일 양식과 로봇 실행은 구성하기를 참고하시기 바랍니다.

시뮬레이션모드로 실행하기 위해 Indy7DControlTask_sim.so를 이용하며, Joint Controller Component를 제외한 다른 컴포넌트는 기본적으로 제공되는 컴포넌트를 그대로 사용합니다. JSON 설정파일 중 JController 부분만 방금 새로 빌드한 컴포넌트로 아래와 같이 대체합니다.

 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
{
    "TimerRate" : 4000, /*Hz*/
    "HighRate" : 4000, /*Hz*/
    "MediumRate" : 20, /*Hz*/
    "LowRate" : 1, /*Hz*/

    "Path" : "/home/user/release/LeanDeployment/",

    "QueueHigh" :
    [
        {
            "TaskName" : "Indy7ControlTask",
            "SharedLib" : "Indy7ControlTask_sim.so",
            "Params":
            {
              "RobotModel": "Indy7Model",
              "JInterpolator": "JointInterpolator6D",
              "JController" : "JointGravityPDControl", // Built component
              "TInterpolator": "TaskInterpolatorIndy",
              "TController": "TaskController6D"
            }
        },
        {
            "TaskName" : "Indy6DLoggingTask",
            "SharedLib" : "Indy6DLoggingTask.so"
        }
    ]
}

JSON 설정파일

/home/user/release/LeanDeployment 경로에서 다음의 명령어로 로봇을 실행합니다.

1
./TaskManager -j deployIndy7.json

이제 Conty를 이용하여 PD controller 의 성능을 시뮬레이션 환경에서 검증할 수 있습니다. 개발 된 관절공간 제어기와 중력보상 제어기는 다음의 세가지 경우에 대해서 작동합니다.

  • 첫 번째로 아래 그림과 같이 중력보상 제어기가 Direct Teaching 에서 동작합니다.


  • 두 번째로 개발 된 PD 제어기는 Home PositionZero Position 에서 관절공간 제어기로 동작합니다.


  • 마지막으로 개발 된 PD 제어기는 jointMove 에서 궤적을 추종하기 위해서 작동합니다. Conty 를 이용하여 궤적을 쉽게 만들 수 있기 때문에 jointMove 을 이용하여 개발 된 제어알고리즘을 쉽게 검증 할 수 있습니다.


주의

본 예제에서의 PD 제어 게인은 임의의 값이기 때문에 실제 로봇은 제대로 제어가 되지 않을 수 있습니다.

데이터 로깅

다음은 제어 실험 결과를 확인하기 위한 데이터 로깅 방법에 대하여 예제코드를 통해 설명하겠습니다. 데이터 로깅을 통해 관절각도, 관절속도, 작업공간 위치, 작업공간 속도 등 실시간 제어 데이터 뿐만아니라 개발자가 직접 정의한 실시간 데이터도 파일로 저장하여 결과를 확인할 수 있습니다.

본 예제에서는 IndySDK 내부적으로 계산되는 관절각도 (q), 관절각속도 (qdot)와 사용자가 계산한 입력토크 (tau)에 대한 데이터 로깅을 예시로 하겠습니다. 중력보상 PD 제어를 구현한 코드에서 추가적인 데이터 로깅 클래스와 이를 사용하는 코드가 추가됩니다.

전체 코드는 위 링크의 Github 리파지토리에서 확인할 수 있습니다.

  • 데이터를 실시간으로 로깅하고 파일로 저장할 수 있는 DataLogger 클래스가 아래와 같이 추가되었습니다.


  • 획득하고자 하는 데이터 정보는 DataConfiguration.h 에 아래와 같이 선언되어 있습니다.
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
                                    ...

struct LoggedData
{
    enum
    {
        JOINT_DOF = 6
    };

    double time;
    double q[JOINT_DOF];
    double qdot[JOINT_DOF];
    double tau[JOINT_DOF];

                                      ...
};
                                      ...
  • 데이터 수집을 위해서 DataLogger.hDataLogger.cpp 에 DataLogger 클래스가구현되어 있습니다.
  • DataLogger.h에 선언된 BUFF_SIZE는 4kHz 제어주기를 기준으로 5초 동안의 데이터를 저장할 수 있도록 버퍼의 크기를 지정한 것입니다.
  • SAMPLE_LOG_FILE 은 로그파일이 저장될 경로와 파일이름을 나타냅니다.
1
2
#define BUFF_SIZE           5*4000  //5sec * 4000Hz
#define SAMPLE_LOG_FILE     "/home/user/release/SampleLoggingFile.csv"
  • 버퍼에 저장되는 데이터를 특정 타이밍에 파일로 쓰기 위한 용도로 비실시간 스레드를 사용합니다. IndySDK 설치시 자동으로 포함되는 3rd party 라이브러리인 Poco에서 제공하는 Runnable클래스를 상속합니다.
1
class DataLogger : public Poco::Runnable
  • 멤버변수 updateLoggedData(.)는 버퍼에 로깅할 실시간 데이터를 쌓는 역할을 합니다. 여기서는 버퍼 인덱스가 최종값이 되었을 때 0으로 초기화해주는 순환버퍼 (circular buffer)로 구현하였습니다.

  • 앞서 언급한 대로 q, qdot, tau 값에 대해서만 저장하도록 합니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
void DataLogger::updateLoggedData(const double time,
                                double const * const q,
                                double const * const qdot,
                                double const * const tau)
{
  // Update circular buffer
  if (!_isLogSaving)
  {
      _loggingBuff[_buffIdx].time = time;
      for (int i = 0; i < JOINT_DOF; i++)
      {
          _loggingBuff[_buffIdx].q[i] = q[i];
          _loggingBuff[_buffIdx].qdot[i] = qdot[i];
          _loggingBuff[_buffIdx].tau[i] = tau[i];
      }

      _buffIdx++;
      if (_buffIdx >= BUFF_SIZE)
          _buffIdx = 0;
  }

}
  • 앞서 PD 제어기를 구현한 computeControlTorq() 함수에서 제어입력 계산 다음부분에 updateLoggedData 함수를 통해 버퍼를 실시간으로 업데이트 합니다.

  • 본 예시에서는 10초마다 버퍼에 저장된 데이터를 파일로 쓰도록 하였습니다 (LOG_DATA_SAVE_PERIOD=10*4000). 사용자의 목적에 따라 파일로 저장하는 부분은 달리 구현하시면 됩니다.

 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
int JointPDControl::computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, JointVec const & qDesired, JointVec const & qdotDesired, JointVec const & qddotDesired, JointVec & torque)
{
                                    ...
    // Update torque control input
    torque = tauPD;

    // Logging data in real-time
    _time += 0.0025;

    double q[JOINT_DOF], qdot[JOINT_DOF], tau[JOINT_DOF];

    for (int i = 0; i < JOINT_DOF; i++)
    {
        q[i] = robot.q()(i,0);
        qdot[i] = robot.qdot()(i,0);
        tau[i] = tauPD(i,0);
    }
    _dataLogger.updateLoggedData(_time, q, qdot, tau);

    // Triggering logger saving
    _logCnt--;
    if (_logCnt <= 0)
    {
        _dataLogger.triggerSaving();
        _logCnt = LOG_DATA_SAVE_PERIOD; // 10s
    }

    return 0;
}
  • Runnable을 상속한 DataLogger 클래스의 스레드는 run() 함수에서 구현됩니다.

  • 위 예시는 순환버퍼의 데이터를 파일로 쓰도록 하였습니다. Trigger 이벤트가 발생하면 파일로 저장합니다.

 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
void DataLogger::run()
{
    std::string fileName = SAMPLE_LOG_FILE;

    while (_isRunning)
    {
        waitForEvent();

        printf("Start to Write!\n");
        _isLogSaving = true;

        FILE * logFile = NULL;
        logFile = fopen(fileName.c_str(), "a");

        if (logFile != NULL)
        {
            for (int k = _buffIdx; k < BUFF_SIZE; k++)
            {
                fprintf(logFile, "%f, ", _loggingBuff[k].time);

                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].q[i]);
                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].qdot[i]);
                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].tau[i]);

                fprintf(logFile, "\n");
            }

            for (int k = 0; k < _buffIdx; k++)
            {
                fprintf(logFile, "%f, ", _loggingBuff[k].time);

                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].q[i]);
                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].qdot[i]);
                for (unsigned int i = 0; i < JOINT_DOF; i++)
                    fprintf(logFile, "%f,  ", _loggingBuff[k].tau[i]);

                fprintf(logFile, "\n");
            }
            fprintf(logFile, "\n\n\n");

            fclose(logFile);
            logFile = NULL;
        }

        _isLogSaving = false;
    }
}