Skip to content

작업경로 보간

삼차 스플라인을 이용한 보간

IndySDK에서는 저수준의 제어기 뿐만이 아니라 로봇의 모션을 생성하는데 있어서 사용자가 직접 궤적을 수정할 수 있다. 본 예제에서는 삼차 스플라인 기법을 이용해서 작업경로를 보간하고 일정한 시간후에 다음 경로점에 도달하도록 관절궤적을 구한다. 기본적인 내용은 관절경로 보간 예제와 비슷하며 작업공간에서의 회전에 대한 경로 보간 부분이 다르다 (삼차 스플라인에 대한 개요는 관절경로 보간 참조).

  • 지수적 사상 (exponential mapping) 이용하여 작업공간에서 회전에 대한 보간을 수행한다.

예제 코드 작성

작업 궤적 보간 컴포넌트(Task Trajectory Interpolation component)를 생성하고 다음과 같은 함수 및 변수들을 헤더파일에 선언한다.

 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
//interpolate paths
void Interpolate(Eigen::MatrixXd *a);

// coefficients: q(s) = a0*s^3 + a1*s^2 + a2*s + a3
Eigen::MatrixXd *disp_a;
Eigen::MatrixXd *rot_a;

LieGroup::Rotation  _R0;
double  _t0;
double  _s;
double  _nSegment;
double  _segTime;
PosType _readyPos;

bool    _isTargetReached = false;
bool    _isTrajSet = false;

std::vector<double> _distDisp;
std::vector<double> _distRot;

std::vector<LieGroup::Vector3D> _Waypoint;
int nPts;
int nSplines;
int nCoeffs;
int dim;

본 예제에서는 삼차 스플라인을 이용하여 보간을 수행하기 때문에 다음과 같이 메모리를 할당하여 포인터 변수들을 초기화 한다.

1
2
3
4
5
6
7
TaskTrajectoryInterpolationComponent::TaskTrajectoryInterpolationComponent()
: AbstractFPathInterpolator<3>(USERNAME, EMAIL, SERIAL)
, nCoeffs(4)
{
    disp_a = new Eigen::MatrixXd[nCoeffs];
    rot_a = new Eigen::MatrixXd[nCoeffs];
}

종료 시 메모리를 해제하는 코드를 다음과 같이 작성한다.

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

    if (rot_a != NULL)
        delete rot_a;
}

작업 궤적 보간 컴포넌트는 크게 setPath 함수와 traj 함수로 구성되어 있다. setPath 는 콘티 (또는 다른 인터페이스)를 통해서 입력된 웨이포인트를 기반으로 경로점을 지정하는 함수이다. traj 는 경로점을 기반으로 위치제어기가 매 제어주기 마다 추종할 수 있는 궤적을 생성하는 함수이다.

벡터형 포인터의 타입을 통해서 전달되는 웨이포인트들을 이용하여 다음과 같이 setPath 함수를 작성한다.

 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
void TaskTrajectoryInterpolationComponent::setPath(const PosType *path,
const int len, double * maxDeviation, const int *dirAngle,
bool simulCheck)
{
    std::vector<LieGroup::Vector3D> wp_disp;
    std::vector<LieGroup::Vector3D> wp_rot;

    LieGroup::Rotation R;
    dim = 3;

    _readyPos = path[0];
    printf("pos: %f||%f||%f\n", _readyPos.r()[0], _readyPos.r()[1], _readyPos.r()[2]);
    wp_disp.push_back(path[0].r());
    _R0 = path[0].R();
    wp_rot.push_back(LieGroup::Vector3D::Zero());

    for (int i = 1; i < len; i++)
    {
        printf("pos: %f||%f||%f\n", _readyPos.r()[0], _readyPos.r()[1], _readyPos.r()[2]);

        wp_disp.push_back(path[i].r());
        R = _R0.icascade(path[i].R());
        wp_rot.push_back(R.expCoord());
        _distDisp.push_back((wp_disp[i] - wp_disp[i-1]).norm());
        _distRot.push_back((wp_rot[i] - wp_rot[i-1]).norm());
    }

    _Waypoint = wp_disp;
    nPts = wp_disp.size();
    nSplines = wp_disp.size()-1;

    for (int i=0; i < nCoeffs; i++)
    {
        disp_a[i].resize(dim, nSplines);
        rot_a[i].resize(dim, nSplines);
    }

    Interpolate(disp_a);

    _nSegment = len-1;
}

본 예제에서는 삼차 스플라인을 이용하여 보간을 수행하기 때문에 아래와 같은 방법으로 스플라인의 계수들을 도출한다 (조건에 대한 설명은 관절경로 보간 참조).

 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
void TaskTrajectoryInterpolationComponent::Interpolate(Eigen::MatrixXd *a)
{
    Eigen::MatrixXd coeff_A;
    Eigen::MatrixXd coeff_b;
    Eigen::MatrixXd coeff;
    for (int j=0; j<dim; j++){
        coeff_A = Eigen::MatrixXd::Zero(4*nSplines,4*nSplines);
        coeff_b = Eigen::MatrixXd::Zero(4*nSplines,1);
        coeff = Eigen::MatrixXd::Zero(4*nSplines,1);
        for (int i=0; i<nSplines; i++){
            // N  = nSplines
            // N conditions: x = f_i(s_i)
            coeff_A(i,i*4+0) = pow(i,3);
            coeff_A(i,i*4+1) = pow(i,2);
            coeff_A(i,i*4+2) = i;
            coeff_A(i,i*4+3) = 1.0;
            coeff_b(i,0) = _Waypoint[i][j];

            // N conditions: x = f_i(s_(i+1))
            coeff_A(nSplines+i,i*4+0) = pow(i+1,3);
            coeff_A(nSplines+i,i*4+1) = pow(i+1,2);
            coeff_A(nSplines+i,i*4+2) = i+1;
            coeff_A(nSplines+i,i*4+3) = 1.0;
            coeff_b(nSplines+i,0) = _Waypoint[i+1][j];

            // 2*(N-1) conditions: f_i'(s_(i+1))
            //= f_(i+1)'(s_(i+1)), f_i''(s_(i+1)) = f_(i+1)''(s_(i+1))
            if(i<nSplines-1){
                coeff_A(2*nSplines+i, i*4+0) = 3.0*pow(i+1,2);
                coeff_A(2*nSplines+i, i*4+1) = 2.0*(i+1);
                coeff_A(2*nSplines+i, i*4+2) = 1.0;
                coeff_A(2*nSplines+i,(i+1)*4+0) = -3.0*pow(i+1,2);
                coeff_A(2*nSplines+i,(i+1)*4+1) = -2.0*(i+1);
                coeff_A(2*nSplines+i,(i+1)*4+2) = -1.0;
                coeff_b(2*nSplines+i,0) = 0.0;

                coeff_A(3*nSplines-1+i, i*4+0) = 6.0*(i+1);
                coeff_A(3*nSplines-1+i, i*4+1) = 2.0;
                coeff_A(3*nSplines-1+i,(i+1)*4+0) = -6.0*(i+1);
                coeff_A(3*nSplines-1+i,(i+1)*4+1) = -2.0;
                coeff_b(3*nSplines-1+i,0) = 0.0;
            }

            // 2 conditions: f_0''(s_0) = 0, f_n''(s_(n+1)) = 0;
            coeff_A(4*nSplines-2, 0*4+0) = 6.0*0;
            coeff_A(4*nSplines-2, 0*4+1) = 2.0;
            coeff_b(4*nSplines-2, 0) = 0.0;

            coeff_A(4*nSplines-1, (nSplines-1)*4+0) = 6.0*nSplines;
            coeff_A(4*nSplines-1, (nSplines-1)*4+1) = 2.0;
            coeff_b(4*nSplines-1, 0) = 0.0;
        }

        coeff = coeff_A.inverse()*coeff_b;
        for (int i = 0; i<nSplines; i++){
            a[0](j,i) = coeff(4*i);
            a[1](j,i) = coeff(4*i+1);
            a[2](j,i) = coeff(4*i+2);
            a[3](j,i) = coeff(4*i+3);
        }
    }
}

다음으로 궤적을 생성하기 위해 필요한 파라미터들을 setTraj 함수에 정의한다. 본 예제에서는 모든 경로점간 이동을 10초로 설정하기 위해 다음과 같이 파라미터들을 설정한다.

1
2
3
4
5
6
7
void TaskTrajectoryInterpolationComponent::setTraj(double t0)
{
    _t0 = t0;
    _segTime = 10.0;
    _isTargetReached = false;
    _isTrajSet = true;
}

다음으로 traj 함수에 보간을 통해서 생성된 계수들을 기반으로 실시간 경로생성을 수행한다. 경로를 생성할 떄 경로점에서의 스플라인의 값을 임의의 정수(s=0, 1, 2, ...)로 설정하고 계수들을 계산하였기 때문에 10초후에 다음 경로점에 도달하게 하기 위해서 경로를 1/10의 기울기를 가지는 1차 함수로 설정하였다.

 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
void TaskTrajectoryInterpolationComponent::traj(double time, PosType & pos,
VelType & vel, AccType & acc)
{
   if (time<=_t0)
    {
        pos = _readyPos;
        vel = VelType::Zero();
        acc = AccType::Zero();
        return;
    }

    _s = (time-_t0)/_segTime;
    double sdot = 1.0/_segTime;
    double sddot = 0.0;

    if (_s > nSplines) _s = nSplines;

    int idx;
    if (_s < nSplines)
    {
        idx = (int)floor(_s);
    }else
    {
        idx = nSplines-1;
    }

    LieGroup::Vector3D xi_d, xidot_d, xiddot_d;
    for (int i=0; i< dim; i++)
    {
      pos.r()[i] = disp_a[0](i, idx)*pow(_s, 3)
                   + disp_a[1](i, idx)*pow(_s, 2)
                   + disp_a[2](i, idx)*_s + disp_a[3](i, idx);
      vel.v()[i] = (3.0*disp_a[0](i, idx)*pow(_s, 2)
                   + 2.0*disp_a[1](i, idx)*_s + disp_a[2](i, idx))*sdot;
      acc.v()[i] = (3.0*disp_a[0](i, idx)*pow(_s, 2)
                   + 2.0*disp_a[1](i, idx)*_s + disp_a[2](i, idx))*sddot
                   + (6.0*disp_a[0](i, idx)*_s
                   + 2.0*disp_a[1](i, idx))*sdot*sdot;

      xi_d[i] = rot_a[0](i, idx)*pow(_s, 3) + rot_a[1](i, idx)*pow(_s, 2)
                + rot_a[2](i, idx)*_s + rot_a[3](i, idx);
      xidot_d[i] = (3.0*rot_a[0](i, idx)*pow(_s, 2)
                    + 2.0*rot_a[1](i, idx)*_s + rot_a[2](i, idx))*sdot;
      xiddot_d[i] = (3.0*rot_a[0](i, idx)*pow(_s, 2)
                     + 2.0*rot_a[1](i, idx)*_s + rot_a[2](i, idx))*sddot
                     + (6.0*rot_a[0](i, idx)*_s
                     + 2.0*rot_a[1](i, idx))*sdot*sdot;
    }

    LieGroup::Rotation exp = xi_d.exp();
    Eigen::Matrix3d dexp = xi_d.dexp();
    Eigen::Matrix3d ddt_dexp = xi_d.ddt_dexp(xidot_d);

    pos.R() = _R0*exp;
    vel.w() = dexp.transpose()*xidot_d;
    acc.w() = dexp.transpose()*xiddot_d + ddt_dexp.transpose()*xidot_d;

    if ( _t0 +  _nSegment*_segTime <= time)
    {
        _readyPos = pos;
        _isTrajSet = false;
        _isTargetReached = true;
        _s = 0.0;
    }
}

마지막으로 궤적설정을 위해 필요한 파라미터 및 로봇제어에 필요한 파라미터들을 제어기에서 역으로 이용할 수 있도록 다음의 함수들을 작성하여 준다.

  • setInitialTime 는 제어기에서 측정한 궤적의 시작 시점을 궤적생성에 반영할 수 있도록 한다.
1
2
3
4
void TaskTrajectoryInterpolationComponent::setInitialTime(double t0)
{
    _t0 = t0;
}
  • getDuration 는 생성된 궤적을 이동하는데 필요한 총 시간을 제어기가 호출할 수 있도록 한다.
1
2
3
4
double TaskTrajectoryInterpolationComponent::getDuration()
{
    return _nSegment*_segTime;
}
  • getNTotalSegment 는 경로점의 개수를 제어기가 호출할 수 있도록 한다.
1
2
3
4
int TaskTrajectoryInterpolationComponent::getNTotalSegment()
{
    return _nSegment;
}
  • getCurrentWaypoint 는 현재 경로점의 위치를 제어기가 호출 할 수 있도록 한다.
1
2
3
4
int TaskTrajectoryInterpolationComponent::getCurrentWaypoint()
{
    return floor(_s);
}
  • isTargetReached 는 목표 경로점에 도착했는지의 여부를 제어기가 호출할 수 있도록 한다.
1
2
3
4
bool TaskTrajectoryInterpolationComponent::isTargetReached()
{
    return _isTargetReached;
}