ikfastによる逆運動学（その３）
１８．PA10の逆運動学
OpenRaveのikfastを使って、pa10でもIKを使えるようにしてみます。
ここで使っているOpenHRP用PA10モデルはOpenRaveのモデルとルートの座標系のとり方が違っているようなので、IKターゲットを座標変換してからikfastに入力します。HIRONXと同じようにインターフェースを整えてやれば動くようになります。
// IKfastのモデルと、openrave-pa10のモデルの座標系が合わない
ofMatrix4x4 adjMat(0,-1,0,0, 1,0,0,0, 0,0,1,0, 0,0,0,1);
aiMatrix4x4 mat;
std::vector<std::vector<IkReal> > sol;
std::vector<IkReal> free_joint;
mat.FromEulerAnglesXYZ(0,0,90.0*M_PI/180.0); //
ofMatrix4x4 yaw(mat.a1, mat.a2, mat.a3, mat.a4, mat.b1, mat.b2, mat.b3, mat.b4, mat.c1, mat.c2, mat.c3, mat.c4, mat.d1, mat.d2, mat.d3, mat.d4);
mat.FromEulerAnglesXYZ(rpy.get().x*M_PI/180.0, rpy.get().y*M_PI/180.0, rpy.get().z*M_PI/180.0);
ofMatrix4x4 tgtMat(mat.a1, mat.a2, mat.a3, xyz.get().x, mat.b1, mat.b2, mat.b3, xyz.get().y, mat.c1, mat.c2, mat.c3, xyz.get().z, 0, 0, 0, 1);
tgtMat = adjMat * tgtMat * yaw;
IkReal eerot[9] = {tgtMat(0,0), tgtMat(0,1), tgtMat(0,2), tgtMat(1,0), tgtMat(1,1), tgtMat(1,2), tgtMat(2,0), tgtMat(2,1), tgtMat(2,2)};
IkReal eetrans[3] = {tgtMat(0,3), tgtMat(1,3), tgtMat(2,3)};
妙に長いですが、IKターゲットの４×４行列を座標変換して適当な辻褄あわせをしているだけです。
ところが、時々くるっと姿勢が回ってしまっているところがあります。PA10はシンメトリな構造なので、ほぼ同じ判定条件に入る複数の解が得られることが原因のようです。肘の位置もあまり動かないものを選ぶ、など、IKの最適な解の選択アルゴリズムにもう少し工夫が必要です。
ダミーモデルを導入して、複数の解のそれぞれの関節角度を適用して、関節の位置と姿勢を求めてみます。
int PA10GUIControler::inverseKinematics()
{
// IKfastのモデルと、openrave-pa10のモデルの座標系が合わない
ofMatrix4x4 adjMat(0,-1,0,0, 1,0,0,0, 0,0,1,0, 0,0,0,1);
aiMatrix4x4 mat;
std::vector<std::vector<IkReal> > sol;
std::vector<IkReal> free_joint;
mat.FromEulerAnglesXYZ(0,0,90.0*M_PI/180.0); //
ofMatrix4x4 yaw(mat.a1, mat.a2, mat.a3, mat.a4, mat.b1, mat.b2, mat.b3, mat.b4, mat.c1, mat.c2, mat.c3, mat.c4, mat.d1, mat.d2, mat.d3, mat.d4);
mat.FromEulerAnglesXYZ(rpy.get().x*M_PI/180.0, rpy.get().y*M_PI/180.0, rpy.get().z*M_PI/180.0);
ofMatrix4x4 tgtMat(mat.a1, mat.a2, mat.a3, xyz.get().x, mat.b1, mat.b2, mat.b3, xyz.get().y, mat.c1, mat.c2, mat.c3, xyz.get().z, 0, 0, 0, 1);
tgtMat = adjMat * tgtMat * yaw;
IkReal eerot[9] = {tgtMat(0,0), tgtMat(0,1), tgtMat(0,2), tgtMat(1,0), tgtMat(1,1), tgtMat(1,2), tgtMat(2,0), tgtMat(2,1), tgtMat(2,2)};
IkReal eetrans[3] = {tgtMat(0,3), tgtMat(1,3), tgtMat(2,3)};
int ret = 0;
std::vector<ofParameter<float>> sol_range;
if(ik_target.get() == "6DF51"){
free_joint.push_back(arm[5]*M_PI/180.0);
ret = IKFAST_6DF51::ik_solve(eerot, eetrans, sol, free_joint);
}
else if(ik_target.get() == "6DF41"){
free_joint.push_back(arm[4]*M_PI/180.0);
ret = IKFAST_6DF41::ik_solve(eerot, eetrans, sol, free_joint);
}
else if(ik_target.get() == "6DF3"){ // うまく動かない
free_joint.push_back(arm[3]*M_PI/180.0);
ret = IKFAST_6DF3::ik_solve(eerot, eetrans, sol, free_joint);
cout << "ret =" << ret << endl;
for(int i=0; i<sol.size(); i++){
for(int j=0; j<sol[i].size(); j++){
cout << sol[i][j] << ", ";
}
cout << endl;
}
}
else if(ik_target.get() == "6DF21"){
free_joint.push_back(arm[2]*M_PI/180.0);
ret = IKFAST_6DF21::ik_solve(eerot, eetrans, sol, free_joint);
}
else if(ik_target.get() == "6DF11"){
free_joint.push_back(arm[1]*M_PI/180.0);
ret = IKFAST_6DF11::ik_solve(eerot, eetrans, sol, free_joint);
}
else if(ik_target.get() == "6DF01"){
free_joint.push_back(arm[0]*M_PI/180.0);
ret = IKFAST_6DF01::ik_solve(eerot, eetrans, sol, free_joint);
}
if(!ret){
ofVec3f p0[7], p1[7];
ofQuaternion q0[7], q1[7];
for(int i=0; i<7; i++){
p0[i] = model.getMeshHelper(i).matrix.getTranslation();
q0[i] = model.getMeshHelper(i).matrix.getRotate();
}
for(int i=0; i<7; i++) sol_range.push_back(arm[i]);
IKRangeCheck(sol, sol_range);
IKSuitable(sol, sol_range);
float min_d = 10000;
int min_d_idx = 0;
float min_dq = 10000;
int min_dq_idx = 0;
for(int i=0; i<sol.size(); i++){
ofParameter<float> dummy_arm[7];
for(int j=0; j<sol[i].size(); j++){
dummy_arm[j] = sol[i].at(j) * 180.0/M_PI;
}
jointAngleUpdate(dummy, TransformationOrigin, dummy_arm);
dummy.update();
for(int j=0; j<7; j++){
p1[j] = dummy.getMeshHelper(j).matrix.getTranslation();
q1[j] = dummy.getMeshHelper(j).matrix.getRotate();
}
float d = 0;
float dq = 0;
for(int j=0; j<7; j++){
d += p0[j].distance(p1[j]);
dq += (q0[j]-q1[j]).length();
}
if(d<min_d){
min_d = d;
min_d_idx = i;
}
min_dq_idx = i;
}
}
if(sol.size()){
for(int i=0; i<7; i++){
arm[i] = sol[min_dq_idx].at(i)*180.0/M_PI;
}
}
}
return 0;
}
一つの安易なルールで最適な姿勢をずばり決めるのは難しいですね。多次元尺度解析や最適化計算を導入して知的に決めたいところです。また、姿勢の変化が大きくならざるを得ないところで、free_jointを切り替えてIKそのものを変更して、もっとうまく腕を動かせるようにしたいところです。が、PA10はあまり使う予定が無いので、この程度で止めておきます。
