fkanehiro/hrpsys-base — pull request #234 by 130s (2014-06-17 23:32:39+00:00)
Title: (hrpsys_config.py) getCurrentPosition w/o arg returns all EEF positions (fix to https://github.com/start-jsk/rtmros_hironx/issues/102)

[HIRONX の議論にした件](https://github.com/start-jsk/rtmros_hironx/issues/102)です．

これから [rtmros_hironx](https://github.com/start-jsk/rtmros_hironx) に開く PReq で以下のようにテスト済で，この結果を目視する限りでは良さそうです．引数無しの場合には以下のように指定された joint すべてに対してコマンドを実行し，print のみ行います．

```
In [1]: robot.getCurrentPose('RARM_JOINT5')
Out[1]:
[-0.0017702356144599085,
-0.00019034630541264752,
-0.9999984150158207,
0.32556275164378523,
-0.00012155879975329215,
0.9999999745367515,
-0.0001901314142046251,
-0.18236394191140365,
0.9999984257434246,
0.00012122202968358842,
-0.001770258707652326,
0.07462472659364472,
0.0,
0.0,
0.0,
1.0]

In [2]: robot.getReferencePose('RARM_JOINT5')
Out[2]:
[-0.0017702356144599085,
-0.00019034630541264752,
-0.9999984150158207,
0.32556275164378523,
-0.00012155879975329215,
0.9999999745367515,
-0.0001901314142046251,
-0.18236394191140365,
0.9999984257434246,
0.00012122202968358842,
-0.001770258707652326,
0.07462472659364472,
0.0,
0.0,
0.0,
1.0]

In [1]: robot.getCurrentPosition('RARM_JOINT5')
Out[1]: [0.32556275164378523, -0.18236394191140365, 0.07462472659364472]

In [2]: robot.getCurrentPosition()
LARM_JOINT5: [0.32556275164378523, 0.18236394191140365, 0.07462472659364472]
RARM_JOINT5: [0.32556275164378523, -0.18236394191140365, 0.07462472659364472]

In [3]: robot.getCurrentRotation('LARM_JOINT5')
Out[3]:
[[-0.0017702356144599085, 0.00019034630541264752, -0.9999984150158207],
[0.00012155879975329215, 0.9999999745367515, 0.0001901314142046251],
[0.9999984257434246, -0.00012122202968358842, -0.001770258707652326]]

In [4]: robot.getCurrentRotation()
LARM_JOINT5: [[-0.0017702356144599085, 0.00019034630541264752, -0.9999984150158207], [0.00012155879975329215, 0.9999999745367515, 0.0001901314142046251], [0.9999984257434246, -0.00012122202968358842, -0.001770258707652326]]
RARM_JOINT5: [[-0.0017702356144599085, -0.00019034630541264752, -0.9999984150158207], [-0.00012155879975329215, 0.9999999745367515, -0.0001901314142046251], [0.9999984257434246, 0.00012122202968358842, -0.001770258707652326]]

In [7]: robot.getCurrentRPY('RARM_JOINT5')
Out[7]: (3.0732223717039675, -1.56902192154894, -3.073032132693228)

In [8]: robot.getCurrentRPY()
LARM_JOINT5: (-3.0732223717039675, -1.56902192154894, 3.073032132693228)
RARM_JOINT5: (3.0732223717039675, -1.56902192154894, -3.073032132693228)

In [5]: robot.getReferencePosition('RARM_JOINT5')
Out[5]: [0.32556275164378523, -0.18236394191140365, 0.07462472659364472]

In [6]: robot.getReferencePosition()
LARM_JOINT5: [0.32556275164378523, 0.18236394191140365, 0.07462472659364472]
RARM_JOINT5: [0.32556275164378523, -0.18236394191140365, 0.07462472659364472]

In [9]: robot.getReferenceRotation('RARM_JOINT5')
Out[9]:
[[-0.0017702356144599085, -0.00019034630541264752, -0.9999984150158207],
[-0.00012155879975329215, 0.9999999745367515, -0.0001901314142046251],
[0.9999984257434246, 0.00012122202968358842, -0.001770258707652326]]

In [10]: robot.getReferenceRotation()
LARM_JOINT5: [[-0.0017702356144599085, 0.00019034630541264752, -0.9999984150158207], [0.00012155879975329215, 0.9999999745367515, 0.0001901314142046251], [0.9999984257434246, -0.00012122202968358842, -0.001770258707652326]]
RARM_JOINT5: [[-0.0017702356144599085, -0.00019034630541264752, -0.9999984150158207], [-0.00012155879975329215, 0.9999999745367515, -0.0001901314142046251], [0.9999984257434246, 0.00012122202968358842, -0.001770258707652326]]

In [11]: robot.getReferenceRPY('RARM_JOINT5')
Out[11]: (3.0732223717039675, -1.56902192154894, -3.073032132693228)

In [12]: robot.getReferenceRPY()
LARM_JOINT5: (-3.0732223717039675, -1.56902192154894, 3.073032132693228)
RARM_JOINT5: (3.0732223717039675, -1.56902192154894, -3.073032132693228)
```