search

start-jsk / rtmros_common

OpenRTM - ROS interoperability packages
http://wiki.ros.org/rtmros_common
12 stars 52 forks source link

setTargetPoseで許容誤差の値が今は0.005になっているので変えたい． #265

Closed k-okada closed 10 years ago

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 22, 2013 22:41:06

setTargetPoseで許容誤差の値が今は0.005になっているのを変えたいです． また，いまは位置と姿勢が一緒のパラメータですが個別に与えたいです．

Original issue: http://code.google.com/p/rtm-ros-robotics/issues/detail?id=267

k-okada commented 10 years ago

From gm130s on November 23, 2013 01:08:39

Temporary な patch (これを dev repo に送るつもりはない) を作成中です．以下のようにしたら，動作はしています．入力値によっては IK Error が出たりしますが．これで正しいでしょうか．

Edit:

%catkinws%/src/openrtm_common/hrpsys/build/hrpsys-base-source/rtc/SequencePlayer/SequencePlayer.cpp https://code.google.com/p/hrpsys-base/source/browse/trunk/rtc/SequencePlayer/SequencePlayer.cpp#396 -    manip->setMaxIKError(0.005);
+    double temp_limit = 0.001;
+    manip->setMaxIKError(temp_limit);

Run:

$ rm -fr `find ./devel/ -iname 'hrpsys'`
$ rm -fr build/rtm-ros-robotics/openrtm_common/hrpsys/
$ catkin_make

Result in ipython

$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py

In [1]: hiro.goInitial()
:
Out[1]: True

In [2]: pos = hiro.getReferencePosition('RARM_JOINT5')
In [3]: rpy = hiro.getReferenceRPY('RARM_JOINT5')
In [4]: tm = 3
In [5]: pos
Out[5]: [0.3255627368715471, -0.1823638733778268, 0.07462449717662004]

In [6]: pos[2]=0.075
In [7]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[7]: True

In [8]: pos[2]=0.076
In [9]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[9]: True

In [10]: pos[0]=0.324
In [11]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[11]: True

In [12]: pos[1]=-0.183
In [13]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[13]: True

In [14]: pos[1]=-0.182
In [15]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[15]: True

In [16]: tm = 1
In [17]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[17]: True

In [18]: pos[1]=-0.185
In [19]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[19]: True

In [20]: pos[1]=-0.19
In [21]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[21]: True

In [22]: pos[1]=-0.39
In [23]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[23]: True

Status: Started
Labels: -Type-Defect Type-Enhancement

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 23, 2013 01:20:03

はい．もう少し言うと，rtc/ImpedanceController/JointPathEx.cppの bool JointPathEx::calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R)で，

    const double errsqr = dp.dot(dp) + omega.dot(omega);
    if ( DEBUG ) std::cerr << "  err : " << std::setw(18) << std::setiosflags(std::ios::fixed) << std::setprecision(\

14) << errsqr << " < " << maxIKErrorSqr << std::endl; if(errsqr < maxIKErrorSqr){

となっているところを if ( dp.dot(dp) < 0.001 && omega.dot(omega) < 0.001??? ) とするのが要望におもいます．

ただ条件が厳しくなっているので解きづらい可能性があります． その場合は，MAX_IK_ITERATIONを増やしてみて下さい．

k-okada commented 10 years ago

From gm130s on November 23, 2013 01:57:44

Degree の方も 0.01 ずつ increment して動きました．まだ十分テストしたとはとても言えませんが．

Edit: %catkinws%/src/rtm-ros-robotics/openrtm_common/openhrp3/build/OpenHRP-3.1/hrplib/hrpModel/JointPath.cpp#251

        //if(errsqr < maxIKErrorSqr){
        // Temporary patch 11/23/2013
        if(dp.dot(dp) < 0.001 && omega.dot(omega) < 0.001???){

Result:

In [4]: hiro.goInitial()
Out[4]: True
In [5]: pos
Out[5]: [0.32556275164378523, -0.18236394191140365, 0.07462472659364472]
In [9]: tm = 1

In [7]: pos[0]=0.324
In [8]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[8]: True

In [10]: pos[0]=0.323
In [11]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[11]: True

In [12]: pos[1]=-0.181
In [13]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[13]: True

In [14]: rpy
Out[14]: (3.0732223717039675, -1.56902192154894, -3.073032132693228)

In [16]: rpy=(3.08, -1.56902192154894, -3.073032132693228)
In [17]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[17]: True

In [18]: rpy=(3.09, -1.56902192154894, -3.073032132693228)
In [19]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[19]: True

In [20]: rpy=(3.09, -1.55, -3.073032132693228)
In [21]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[21]: True

In [22]: rpy=(3.09, -1.54, -3.073032132693228)
In [23]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[23]: True

In [24]: rpy=(3.09, -1.54, -3.08)
In [25]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[25]: True

In [26]: rpy=(3.09, -1.54, -3.09)
In [27]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[27]: True

Cc: iisaac.s...@gmail.com

k-okada commented 10 years ago

From gm130s on November 23, 2013 01:58:29

これで良いのであれば，実機に deploy する計画をしたいです．実機にアクセスできないので分からないのですが，バイナリを入れ替えようと思った場合どこにあるどのファイルを入れ替えれば良いのでしょうか．．

catkinws$ find install/ -iname 'SequencePlayer*'
install/lib/hrpsys/SequencePlayerComp
install/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.pyc
install/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.py
install/lib/python2.7/dist-packages/hrpsys_ros_bridge/SequencePlayerService_idl.pyc
install/lib/python2.7/dist-packages/hrpsys_ros_bridge/SequencePlayerService_idl.py
install/lib/hrpsys_ros_bridge/SequencePlayerServiceROSBridgeComp
install/include/hrpsys/idl/SequencePlayerService.hh
install/include/openhrp3/include/hrpsys/idl/SequencePlayerService.hh
install/include/openrtm_aist/include/hrpsys/idl/SequencePlayerService.hh
install/share/hrpsys/lib/SequencePlayer.so
install/share/hrpsys/share/hrpsys/idl/SequencePlayerService.idl
n130s@130s-serval:~/link/ROS/groovy_precise/catkinws$ find install/ -iname 'JointPath*'
install/include/openhrp3/include/OpenHRP-3.1/hrpModel/JointPath.h
install/include/openrtm_aist/include/OpenHRP-3.1/hrpModel/JointPath.h
k-okada commented 10 years ago

From kei.ok...@gmail.com on November 23, 2013 02:00:35

向こうが送ってきた install/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.py や， install/share/hrpsys/share/hrpsys/idl/SequencePlayerService.idl が手元とおなじだったら，同じ環境なので（本当はIDL以下全てを調べたい） install/share/hrpsys/lib/SequencePlayer.so と，SquencePlayerComp を入れ替えれば動くとおもいます．

k-okada commented 10 years ago

From emijah.s on November 24, 2013 19:06:04

できれば誤差を0.0001くらいにできると良いのですが、可能ですかね?

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 24, 2013 19:29:59

いまはヤコビアンベースなので出来ますが，繰り返しのリミットのパラメータが難しそうです． ところで，むかしIKFASTで作られていたとおもいますが，あればオープンにはならないでしょうか．

k-okada commented 10 years ago

From gm130s on November 24, 2013 20:35:32

取りあえず，最小の動作単位を 0.01 mm (0.1 mm ではなく) にしたいとの要望があり，対応しようとしています．以下のような，0.1 mm の時に試した方法だと，0.1 mm 以下の指令値に対し，エラーは出ませんが，期待通りにも動いてないように見えます (何も動作してないようにみえる)．

Edit:

hrpsys/build/hrpsys-base-source/rtc/SequencePlayer/SequencePlayer.cpp

    double temp_limit = 0.0001;

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

    if ( dp.dot(dp) < 0.0001 && omega.dot(omega) < 0.0001??? )

Result:

In [11]: hiro.goInitial()
[hrpsys.py]  self.setJointAnglesOfGroup( torso , [0] ,  7 ,wait=False)
[hrpsys.py]  self.setJointAnglesOfGroup( head , [0, 0] ,  7 ,wait=False)
[hrpsys.py]  self.setJointAnglesOfGroup( rarm , [-0.6, 0, -100, 15.2, 9.4, 3.2] ,  7 ,wait=False)
[hrpsys.py]  self.setJointAnglesOfGroup( larm , [0.6, 0, -100, -15.2, 9.4, -3.2] ,  7 ,wait=False)
Out[11]: True

In [12]: pos = hiro.getReferencePosition('RARM_JOINT5')

In [13]: pos
Out[13]: [0.3255627368715471, -0.1823638733778268, 0.07462449717662004]

In [14]: pos[0] = 0.324

In [15]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[15]: True

In [16]: hiro.getReferencePosition('RARM_JOINT5')
Out[16]: [0.32418474871917624, -0.18237057940923737, 0.07461220803684657]

In [17]: pos[0] = 0.3239

In [18]: hiro.setTargetPose('rarm', pos, rpy, tm)
Out[18]: True

In [19]: hiro.getReferencePosition('RARM_JOINT5')
Out[19]: [0.32418474871917624, -0.18237057940923737, 0.07461220803684657]
k-okada commented 10 years ago

From gm130s on November 24, 2013 21:13:48

要件は

誤 0.01mm 正 0.1 mm

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 24, 2013 21:15:33

if ( dp.dot(dp) < 0.0001 && omega.dot(omega) < 0.0001??? )

の ??? は実際のコードには入っていないですよね？hrpsysを立ち上げたターミナルでどういう表示があるかを確認してみて下さい．

k-okada commented 10 years ago

From gm130s on November 24, 2013 22:00:33

??? は実際のコードには入っていないですよね？

はい，入ってないです (上の表記からは消し忘れました)．

hrpsysを立ち上げたターミナルでどういう表示があるかを確認してみて下さい．

"rtmlaunch hironx_ros_bridge hironx_ros_bridge_simulation.launch" の結果をここに置きました． http://pastebin.com/zQgY4ren hiro.setTargetPose を実行する時に特に何も出力されてないように見えます．

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 24, 2013 22:11:39

Launch ファイルでoutput=screenしていますか？

k-okada commented 10 years ago

From gm130s on November 24, 2013 22:40:40

同じコードで 0.1 mm でも動いてるのを simulator で確認しまいた (動いてることを目視で気付いてなかっただけかも知れません)．以下のように loop させたら，じりじり動きました．もっとテストします (unittest にした方が良さそうですね)．

Edit ( http://code.google.com/p/rtm-ros-robotics/issues/detail?id=267#c8 と同じ):

hrpsys/build/hrpsys-base-source/rtc/SequencePlayer/SequencePlayer.cpp

    double temp_limit = 0.0001;

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

    if ( dp.dot(dp) < 0.0001 && omega.dot(omega) < 0.0001)

Result:

In [22]: tm = 0.1
In [25]: for i in range(1000):
    pos[2] += 0.0001
    time.sleep(0.11)
    hiro.setTargetPose('rarm', pos, rpy, tm)
k-okada commented 10 years ago

From gm130s on November 25, 2013 23:23:13

実機 (Nextage) の次のファイルを，上述のパッチをあてて実機上でビルドし，入替えましたが，期待する挙動は得られていないように思います．入れ替えるファイルに過不足があるでしょうか？

/opt/jsk/lib/SequencePlayer.so
/opt/jsk/lib/libhrpModel-3.1.so
/opt/jsk/lib/libhrpModel-3.1.so.0
/opt/jsk/lib/libhrpModel-3.1.so.0.0.0

Result：ループの中で，4〜6 回のみ (回数は実行によってまちまち) 腕が一気に移動する；毎ループ少しづつ動いているようには見えない．x 値のループ前と後の差は 0.05 になって欲しいところだがそうなっていない．

In [22]: tm=0.1
In [28]: print nxc.getReferencePosition('RARM_JOINT5')
[0.3089495374118162, -0.23856706956852192, 1.2486482098634109]

In [29]: for i in range(500):
    pos[0] += 0.0001
    time.sleep(0.11)
    nxc.setTargetPose('rarm', pos, rpy, tm)
   ....:     

In [30]: print nxc.getReferencePosition('RARM_JOINT5')
[0.34414884706249876, -0.23809412551840053, 1.2481938812572764]
k-okada commented 10 years ago

From kei.ok...@gmail.com on November 25, 2013 23:32:05

Printfをいれて、確認するのはどうでしょう

k-okada commented 10 years ago

From gm130s on November 25, 2013 23:40:23

今から print してみます．

説明が悪くてすみません．#14 の例では，pos[0] の値は毎ループごとに increment はソフトウェア上はされてはいるが，ハードはある程度 pos[0] の値が大きくなった時のみ動いているように見えていると思います．そこからくる仮設ですがループ 500 回に対し，実機で腕が動くのが 4〜6 回というのは，だいたい差が 0.0001 * 100 くらいになった時に動いているのではないかと．

差し替えるファイルは上述で足りてるでしょうか？

k-okada commented 10 years ago

From you...@jsk.imi.i.u-tokyo.ac.jp on November 25, 2013 23:56:54

ファイルは合っていると思います．

k-okada commented 10 years ago

From gm130s on November 26, 2013 01:45:40

実機で動き始めたように見えます (実際の displacement が 0.1 mm なのか調べる術を知らない)．まだ，x 方向を変えるシンプルな動きしか試してませんが．

この変更が不要だったようです．

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

    if ( dp.dot(dp) < 0.0001 && omega.dot(omega) < 0.0001)

色々な動きで試します．

Cc: emijah.s

k-okada commented 10 years ago

From gm130s on November 26, 2013 03:15:23

(補足) #c13 で，"simulator 上で動いた" と報告した際には，JointPath.cpp がきちんとビルドされておらず，未変更版バイナリを使っていたことが判りました．

k-okada commented 10 years ago

From gm130s on November 26, 2013 19:29:11

この ticket を乗っ取ってしまいましたがこちらの用件は一旦落ち着いたので，状況整理します．

  1. 元々の要件は，許容誤差を指定できるようにしたい，ということでしょうか？

  2. 現状実施できているのは，下記パッチにより 0.1 mm で動作させること．

    %catkinws%/src/openrtm_common/hrpsys/build/hrpsys-base-source/rtc/SequencePlayer/SequencePlayer.cpp https://code.google.com/p/hrpsys-base/source/browse/trunk/rtc/SequencePlayer/SequencePlayer.cpp#396 - manip->setMaxIKError(0.005);

    • double temp_limit = 0.0001;
    • manip->setMaxIKError(temp_limit);
  3. rtc/ImpedanceController/JointPathEx.cpp の変更が必要なかった理由はわかっていません．

1 が No であれば，SequencePlayer.cpp のパッチは hrpsys-base に出すべきでしょうか？

k-okada commented 10 years ago

From kei.ok...@gmail.com on November 26, 2013 19:31:51

許容誤差，繰り返し回数をIDL経由で掛けるようにするのがいいですね． 許容誤差は位置と姿勢は別々に指令したくなるとおもいます．

k-okada commented 10 years ago

From kei.ok...@gmail.com on December 25, 2013 20:40:06

move discussion to here -> https://code.google.com/p/hrpsys-base/issues/detail?id=168

Status: Fixed