관절경로 보간
삼차 스플라인을 이용한 보간
IndySDK에서는 저수준의 제어기 뿐만이 아니라 로봇의 모션을 생성하는데 있어서 사용자가 직접 궤적을 수정할 수 있다. 본 예제에서는 삼차 스플라인 기법을 이용해서 관절경로를 보간하고 일정한 시간후에 다음 경로점에 도달하도록 관절궤적을 구한다. 삼차 스플라인은 다음 4개의 조건을 만족시키도록 함수 S_{i}의 계수를 구하는 문제이다.
- S_{j}(x_{j}) = f(x_{j}), S_{j}(x_{j+1})=f(x_{j+1}), \forall j=0,1,\cdots n-1
- S_{j+1}(x_{j+1}) = S_{j}(x_{j+1}), \forall j=0,1,\cdots n-2
- S_{j+1}^{'}(x_{j+1}) = S_{j}^{'}(x_{j+1}), \forall j=0,1,\cdots n-2
- S_{j+1}^{''}(x_{j+1}) = S_{j}^{''}(x_{j+1}), \forall j=0,1,\cdots n-2
다음 둘 중 하나의 양 끝 쪽에서의 조건을 이용하면 삼차 스플라인의 계수를 계산할 수 있다.
- S^{''}(x_{0}) = S^{''}(x_{n}) = 0
- S^{'}(x_{0}) = f^{'}(x_{0}), S^{'}(x_{n}) = f^{'}(x_{n})
예제 코드 작성
관절 궤적 보간 컴포넌트(Joint 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 | //interpolate paths
void Interpolate();
// coefficients: q(s) = a0*s^3 + a1*s^2 + a2*s + a3
Eigen::MatrixXd a0;
Eigen::MatrixXd a1;
Eigen::MatrixXd a2;
Eigen::MatrixXd a3;
double _t0;
double _s;
double _nSegment;
double _segTime;
VecType _readyPos;
bool _isTargetReached = false;
bool _isTrajSet = false;
std::vector<VecType> _Waypoint;
int nPts;
int nSplines;
int dim;
|
관절 궤적 보간 컴포넌트는 크게 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 | void JointTrajectoryInterpolationComponent::setPath(const VecType *path,
int len, double * maxDeviation)
{
std::vector<VecType> wp_angle;
_readyPos = path[0];
wp_angle.push_back(path[0]);
for (int i = 1; i< len; i++)
{
wp_angle.push_back(path[i]);
}
_Waypoint = wp_angle;
nPts = wp_angle.size();
nSplines = wp_angle.size()-1;
dim = 6;
// interpolation
a0.resize(dim, nSplines);
a1.resize(dim, nSplines);
a2.resize(dim, nSplines);
a3.resize(dim, nSplines);
Interpolate();
_nSegment = len-1;
printf("set Path\n");
}
|
본 예제에서는 삼차 스플라인을 이용하여 보간을 수행하기 때문에 아래와 같은 방법으로 스플라인의 계수들을 도출한다. 주석으로 표시되었듯이 각각의 묶음은 처음에 개요에서 언급한 조건들을 표현한 것이다.
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 | void JointTrajectoryInterpolationComponent::Interpolate()
{
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++){
a0(j,i) = coeff(4*i);
a1(j,i) = coeff(4*i+1);
a2(j,i) = coeff(4*i+2);
a3(j,i) = coeff(4*i+3);
}
}
}
|
다음으로 궤적을 생성하기 위해 필요한 파라미터들을 setTraj 함수에 정의한다. 본 예제에서는 모든 경로점간 이동을 10초로 설정하기 위해 다음과 같이 파라미터들을 설정한다.
| void JointTrajectoryInterpolationComponent::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 | void JointTrajectoryInterpolationComponent::traj(double time, VecType &
posDes, VecType & velDes, VecType & accDes)
{
if (time <= _t0)
{
posDes = _readyPos;
velDes = VecType::Zero();
accDes = VecType::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;
}
for (int i=0; i< dim; i++)
{
posDes[i] = a0(i, idx)*pow(_s, 3) + a1(i, idx)*pow(_s, 2)
+ a2(i, idx)*_s + a3(i, idx);
velDes[i] = (3.0*a0(i, idx)*pow(_s, 2) + 2.0*a1(i, idx)*_s
+ a2(i, idx))*sdot;
accDes[i] = (3.0*a0(i, idx)*pow(_s, 2) + 2.0*a1(i, idx)*_s
+ a2(i, idx))*sddot + (6.0*a0(i, idx)*_s
+ 2.0*a1(i, idx))*sdot*sdot;
}
if (_t0 + _nSegment*_segTime <= time)
{
_readyPos = posDes;
_isTrajSet = false;
_isTargetReached = true;
_s = 0.0;
}
}
|
마지막으로 궤적설정을 위해 필요한 파라미터 및 로봇제어에 필요한 파라미터들을 제어기에서 역으로 이용할 수 있도록 다음의 함수들을 작성하여 준다.
- setInitialTime 는 제어기에서 측정한 궤적의 시작 시점을 궤적생성에 반영할 수 있도록 한다.
| void JointTrajectoryInterpolationComponent::setInitialTime(double t0)
{
_t0 = t0;
}
|
- getDuration 는 생성된 궤적을 이동하는데 필요한 총 시간을 제어기가 호출할 수 있도록 한다.
| double JointTrajectoryInterpolationComponent::getDuration()
{
return _nSegment*_segTime;
}
|
- getNTotalSegment 는 경로점의 개수를 제어기가 호출할 수 있도록 한다.
| int JointTrajectoryInterpolationComponent::getNTotalSegment()
{
return _nSegment;
}
|
- isTargetReached 는 목표 경로점에 도착했는지의 여부를 제어기가 호출할 수 있도록 한다.
| bool JointTrajectoryInterpolationComponent::isTargetReached()
{
return _isTargetReached;
}
|