JointPathExに定義されている逆運動学関数について

[fkanehiro]

From kei.ok...@gmail.com on June 24, 2013 22:04:56

// This should be merged with calcInverseKinematics2 and calcInverseKinematics2Loop
void JointPathEx::solveLimbIK とあるんだけど，マージできるかな． そもそもこの関数がLimbIKとあるけど，速度入力だったり，あるいは calcInverseKinematics2でspeed_limitをみていて，ある程度大きい動きが出来なかったりしています．

まずは直したらいいポイントをリストアップしてもらえるとうれしいです．

Original issue: http://code.google.com/p/hrpsys-base/issues/detail?id=114

[fkanehiro]

From noz...@jsk.imi.i.u-tokyo.ac.jp on June 24, 2013 11:06:49

solveLimbIKは、Impedancecontroller.cppの中の 関節角度更新の部分が、calcInverseKinematics2やcalcInverseKinematics2loopなどを 使ってなかったので、抜き出して関数化して、 あとでマージしよう、としていたテンポラリなものです。 また、おっしゃるとおりcalcInverseKinematics2loopと近い速度計算(と関節角度更新)の 関数になっています。

ざっとみたかんじでは、calcInverseKinematics2Loopに足りなくてsolveLimbIKにあるものは、

1. 可動限界をさけるnull-space計算(l.357-378)
2. 入力関節角度への追従項(l.382-388) で、calcInverseKinematics2でかつfor文の中にあるものとしては、
3. 速度・可動域リミット
4. NANチェック などがありそうです。

なのでマージするとしたら、 solveLimbIKから1., 2.を、 calcInverseKinematics2から3, 4を calcInverseKinematics2Loopへと移すかんじになるかと思います。

他には、Weighted SR Inverseのweightの計算や、 null space計算、リミットの制限コードが 関数になって、他から呼び出せるようになっているとうれしい、という状況です。

[fkanehiro]

From kei.ok...@gmail.com on June 25, 2013 00:57:10

幾つか修正しました．問題ないか確認してみて下さい．

SR-InverseのなかではPseudoInverseでなくInverseを使う，というのと，calcInverseKinematics2の中でvel_pを計算するところが間違っている気がします．

Index: build/hrpsys-base-source/rtc/ImpedanceController/JointPathEx.cpp

--- build/hrpsys-base-source/rtc/ImpedanceController/JointPathEx.cpp (リビジョン 742) +++ build/hrpsys-base-source/rtc/ImpedanceController/JointPathEx.cpp (作業コピー) @@ -52,7 +52,7 @@

 dmatrix at = _a.transpose();
 dmatrix a1(c, c);

• hrp::calcPseudoInverse((_a * _w * at + _sr_ratio * dmatrix::Identity(c,c)), a1);

• a1 = (_a * _w * at + _sr_ratio * dmatrix::Identity(c,c)).inverse();

  //if (DEBUG) { dmatrix aat = _a * at; std::cerr << " a*at :" << std::endl << aat; }

@@ -212,7 +212,7 @@ std::cerr << " iter : " << iter << " / " << MAX_IK_ITERATION << ", n = " << n << std::endl; }

• Vector3 dp(end_p - target->p);
• Vector3 dp(target->R.transpose() * (end_p - target->p)); Vector3 omega(target->R * omegaFromRot(target->R.transpose() * end_R));

[fkanehiro]

From kei.ok...@gmail.com on June 25, 2013 07:55:29

r767 move speed limit code under calcInverseKinematics2Loop r766 fix dp, Jacobian is calculated wrt the world-frame? that's is different from euslisp's jacobian results, Issue 114 r765 fix avoid_weight_gain in JointPathEx.cpp, Issue #114 r764 fix debug comment JointPathEx.cpp, Issue #114 r763 fix manipulability_gain to 0.001 and check dp/omega (vel-p, vel-r) speed limit to 0.1 and 0.5, Issue #114 r762 fix InverseKinematics2, do not use calcPseudoInverse, use normal inverse in SRInverse, fix dp to wrt target frame, Issue 114 と直しました．これで添付のテストプログラムで大体解けている気がします．

以下の，３の速度は r767 で対応しました．リミットも個々で制限するのかな． ４は２８０行でやっていないかな． １は８９行は違うかな．ただ，以下のものとは少し式が違う気がする．eusと８９行は同じに見えるけどな．

1. 可動限界をさけるnull-space計算(l.357-378)
2. 入力関節角度への追従項(l.382-388) で、calcInverseKinematics2でかつfor文の中にあるものとしては、
3. 速度・可動域リミット
4. NANチェック

solveLimbIKから1., 2.を、 calcInverseKinematics2から3, 4を calcInverseKinematics2Loopへと移すかんじになるかと思います。

Attachment: hrp4c_testik.py

[fkanehiro]

From noz...@jsk.imi.i.u-tokyo.ac.jp on June 27, 2013 04:38:06

hrpsys-base-sourceを更新してみましたが、 solve ik Falseなどのように表示されて、setTargetPoseの返り値が Falseになっているようですが、 他の環境ではうまくいっていますでしょうか。

[fkanehiro]

From kei.ok...@gmail.com on June 27, 2013 04:57:13

SequencePlayerでlen=1としてみてください．

[fkanehiro]

From noz...@jsk.imi.i.u-tokyo.ac.jp on July 01, 2013 03:19:58

ありがとうございます、コミットされているバージョンでpythonスクリプトが正しく動いているのを確認できました。