ikfastによる逆運動学（その２）
１７．ikfastの利用方法
openraveが公開してくれているikfastのサンプルc++コードを、前回ベタに使えるように少し書き換えています。その使い方を少し説明します。
引数の渡し方
[入力] 姿勢：3x3行列,
[入力] 位置：3ベクトル、
[出力] 解：可変長二次元配列 大きさは[解の数][自由度の数]、
[入力] 自由関節：可変長一次元配列
//ofVec3f rpy(180,0,0), pos(0.5,0.5,0);
aiMatrix4x4 mat;
mat.FromEulerAnglesXYZ(rpy.x*M_PI/180.0, rpy.y*M_PI/180.0, rpy.z*M_PI/180.0);
IkReal eerot[9] = {mat.a1, mat.a2, mat.a3, mat.b1, mat.b2, mat.b3, mat.c1, mat.c2, mat.c3};
IkReal eetrans[3] = {pos.x, pos.y, pos.z};
std::vector<std::vector<IkReal> > sol;
std::vector<IkReal> free_joint;
free_joint.push_back(chest*M_PI/180.0);
呼び方
//int ret = IKFAST_LOOKAT3D::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_LOOKAT3DF0::ik_solve(eerot, eetrans, sol, free_joint);
int ret = IKFAST_RIGHT6D::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_RIGHT6DF0::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_LEFT6D::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_LEFT6DF0::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_RIGHT5DF8::ik_solve(eerot, eetrans, sol, free_joint);
//int ret = IKFAST_LEFT5DF18::ik_solve(eerot, eetrans, sol, free_joint);
ikfastは、逆運動学の数値解を高速に解いてくれるライブラリです。逆運動学の計算は連立方程式を解くのと同じで可能な解を全て求めますから、計算過程で符号違いの複数の解が出てきます。ロボットの関節可動範囲を超えた解も含まれていますので、まずこれらを排除します。
int IKRangeCheck(std::vector<std::vector<IkReal> >& sol, std::vector<ofParameter<float>>& sol_range)
{
vector<vector<IkReal> >::iterator it = sol.begin();
while(it != sol.end()){
bool check = false;
for(int i=0; i<it->size(); i++){
if((sol_range[i].getMin() >= it->at(i)*180/M_PI) || (it->at(i)*180/M_PI >= sol_range[i].getMax())){
check = true;
break;
}
}
if(check) it = sol.erase(it);
else it++;
}
return sol.size();
}
それでも答えは一つに絞られず、ロボットが取りうる複数の姿勢が解として残ることがあります。どれも数値的には正しいのですが、どれか一つを選ばなければロボットを動かすことができませんので、何らかのルールを導入して一つ選ぶ必要があります。
ここでは、ロボットの動作がコンパクトに収まるように、直前の姿勢からの関節角度の移動量の最大値が最小の解を選択することにします。
int IKSuitable(std::vector<std::vector<IkReal> >& sol, std::vector<ofParameter<float>>& sol_range)
{
IkReal min_dist = DBL_MAX;
int min_idx = -1;
for(int i=0; i<sol.size(); i++){
IkReal tmp_dist = -1;
for(int j=0; j<sol[i].size(); j++){
tmp_dist = max(fabs(sol_range[j].get()-sol[i][j]*180.0/M_PI), tmp_dist);
}
if(tmp_dist<min_dist){
min_dist = tmp_dist;
min_idx = i;
}
}
if(min_idx>=0){
IkReal tmp;
for(int j=0; j<sol[min_idx].size(); j++){
tmp = sol[0][j];
sol[0][j] = sol[min_idx][j];
sol[min_idx][j] = tmp;
}
}
return sol.size();
}
