HIROのハンドからオフセットのついた現在角度を取得する

[k-okada]

From nao.luna...@gmail.com on August 29, 2013 22:16:26

HIROのハンドの角度を見る時に，hironx.sc_svc.getJointAngleを使って角度を得ているのですが，

hiro.setHandJointAngles('rhand', [0,0,0,0],3) hiro.setHandJointAngles('lhand', [0,0,0,0],3) でハンドの角度をすべて"0"に指定した後に

hironx.sc_svc.getJointAngleをサーボの個数分だけ繰り返してすべての角度を得ると rhand = [-48.4, 0.1, -47.2, -0.1] lhand = [-48.7, -0.3, -46.5, -0.3]

と違う値が帰ってきます． これはQNXの/opt/jsk/etc/HIRONX/hrprtc/Robot.confの中の servo.offset: -0.85,0.0,-0.82,0.0, -0.85,0.0,-0.82,0.0 ## というオフセットをhiro.sc_svc.setJointAnglesOfGroupの方では考慮していますが， hironx.sc_svc.getJointAngleでは考慮していないためだと考えています．

できれば，オフセットを考慮したハンドの角度を得たいです．

その際の対策としては 1) QNXのRTCのプログラムである/home/hiro/src/hrpsys-base-source/rtc/ServoController/ServoController.cpp に対してオフセットを考慮するように変更して， hiro.sc_svc.getJointAnglesで帰ってくる値をオフセットがついた値にする．

2) Ubuntu側でhironx.pyなどにオフセットの値を書き， hironx.pyのsetHandJointAnglesの中にオフセットを考慮する処理を書く．

の2つが考えられましたが，１ができたほうが良いと思うので，１を実装してみると，以下のようになりました． (ServoController中には，getJointAngleとgetJointAnglesがありましたが， 一気に複数の値を取るgetJointAnglesの方は，直すのが難しそうだったので， とりあえず一度に一つだけ角度を取得するgetJointAngleの方だけ直しました．)

＊変更点

1. setJointAngleの方は，degではなくradで指定するようになっていたので，degで指定できるように直しました．
2. getJointAngle中で，Offsetをdegに直してからangleに適用しました．

確認して，修正とコミットをお願いしたいです． また，問題があれば指摘してください． (for文を使って，idに合うoffsetを探していますが，もっとスマートなのがあれば変えてください)

$ svn diff

Index: ServoController.cpp

--- ServoController.cpp ( revision 832 ) +++ ServoController.cpp (working copy) @@ -230,7 +230,10 @@ bool ServoController::setJointAngle(short id, double angle, double tm) { if ( ! serial ) return true;

• serial->setPosition(id, angle, tm);
• double rad = angle * M_PI / 180;
• for(int i=0; i<servo_id.size(); i++){
• if(servo_id[i]==id) serial->setPosition(id, rad+servo_offset[i], tm);
• } return true; }

@@ -259,6 +262,12 @@ if ( ! serial ) return true;

 int ret = serial->getPosition(id, &angle);

• for(int i=0; i<servo_id.size(); i++){
• if(servo_id[i]==id){
• double servo_offset_angle = servo_offset[i] * 180 / M_PI;
• angle -= servo_offset_angle;
• }

• }

  if (ret < 0) return false; return true;

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

[k-okada]

From kei.ok...@gmail.com on August 29, 2013 06:51:06

対応しました．確認して下さい．

[k-okada]

From nao.luna...@gmail.com on August 29, 2013 07:11:47

In [2]: hiro.sc_svc.setJointAngles([0,0,0,0,0,0,0,0],2) Out[2]: True

をした後，以下をすると，1度以内の誤差になりました． 実用できそうなレベルなので，これで大丈夫だと思います．

In [3]: hiro.get_hand_angles() Out[3]: [0.601412586119972, 0.2, -0.4174607992724901, -0.4, -0.7985874138800266, -0.1, 0.3825392007275071, 0.0]

ちなみに，get_hand_anglesの定義は以下です． def get_hand_angles(self): angles = [] for servo_id in self.HandGroups['rhand'] + self.HandGroups['lhand']: while(1): res = self.sc_svc.getJointAngle(servo_id) if res[0]: angles.append(res[1]) break return angles

[k-okada]

From nao.luna...@gmail.com on August 29, 2013 23:37:19

前の変更点の中で

「1. setJointAngleの方は，degではなくradで指定するようになっていたので，degで指定できるように直しました．」

と書きましたが，これは1つのサーボの現在角度を取得するgetJointAngleという関数がdegで返ってくるため， getとsetの関数で，引数とする角度の単位を揃えたほうが良いのではないか，と思って行いました．

しかし，それを修正した後に，ServoController.cppを見なおしてみると， 以下の関数でangleがradかdegのどちらかに統一されていないことがわかりました．

hrpsys-base-source/rtc/ServoController/ServoController.cpp

deg getJointAngle getJointAngles setJointAngle(元はradでしたが，昨日degに変更しました) rad setJointAngles setJointAnglesOfGroup

このままだと， 例えばgetJointAngleで取得した現在角度を，そのままsetJointAnglesの引数に入れることができないので不便です．

hrpsys-baseを見ると， Definition: hiro.getJointAngles(self) Source: def getJointAngles(self): return [x*180.0/math.pi for x in self.sh_svc.getCommand().jointRefs]

のように，rtcではrad, スクリプトではdegとなっているようなので，

これと統一するための解決策として， ・QNXのRTCはradに全て統一する ・hironx.py中のHandの角度の取得・設定の関数で，degへの変換を書く

が良いのではないかと思っていますが，いかがでしょうか？

[k-okada]

From ke...@jsk.imi.i.u-tokyo.ac.jp on August 30, 2013 00:33:20

ServoControllerを変更するにあたって、confファイルがの読まれ方が気になったのですが、

あるRTCが読むべきconfファイルは、どこで決めているのでしょうか？ rh.conf, seq.conf等の、(Component名).confというファイルがあれば、そこから読み、 対応するものがなければ、Robot.confを読む、 というのを、hrpsysがどこかに書いているのでしょうか？

他のロボットでは、confファイルがどのような構成になっているのか、 ということも含めてお聞きしたいです。

よろしくお願いします。

[k-okada]

From kei.ok...@gmail.com on August 31, 2013 22:42:47

・QNXのRTCはradに全て統一する ・hironx.py中のHandの角度の取得・設定の関数で，degへの変換を書く はい．それでいいとおもいます．変更パッチを送ってくれると嬉しいです．

あるRTCが読むべきconfファイルは、どこで決めているのでしょうか？ rtcd.shで rtcd -f /opt/jsk/etc/HIRONX/hrprtc/rtcdRobotMode.conf となって，まず，rtcdRobotModel.confが読まれます．次にこのなかで， example.ServoController.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf とかいてあるので，プラグイン起動時にRobot.confが読まれて， servo.id: 2,3,4,5, 6,7,8,9 などの情報がわたります．

[k-okada]

From ke...@jsk.imi.i.u-tokyo.ac.jp on September 01, 2013 17:07:54

ありがとうございます。

example.ServoController.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf とかいてあるので，プラグイン起動時にRobot.confが読まれて，

の部分は、上のようにRobot.confだったり、下のようにrh.confだったりしますが、 example.RobotHardware.config_file: /opt/jsk/etc/HIRONX/hrprtc/rh.conf

1つのファイルにまとめていくべきなのか、それともRTC毎に分けるべきなのかが 分かっていないので、教えてください。

[k-okada]

From kei.ok...@gmail.com on September 01, 2013 17:41:21

基本的には全部Robot.confに書いてあるというのでファイルは分けないのがいいとおもいますが， rh.confはロボットハードウェアに依存してメーカが外に出したくない情報を書いてある，という位置づけでいいと思います． （つまり僕達が変更してsvnに置いておくのはRobot.confだけ．)

ただ，rh.confを今見ていると，ここにある情報（part.{assigned_joint,BODY,RARM.}とはじまる情報）は現状で使われていないきもしています．（つまりrh.confをRobot.confに書き換えてもいいか？）　emijah.s さん，コメントお願いします．

Owner: kei.ok...@gmail.com
Cc: emijah.s ke...@jsk.imi.i.u-tokyo.ac.jp

[k-okada]

From nao.luna...@gmail.com on September 01, 2013 19:58:28

・QNXのRTCはradに全て統一する ・hironx.py中のHandの角度の取得・設定の関数で，degへの変換を書く はい．それでいいとおもいます．変更パッチを送ってくれると嬉しいです．

わかりました． 作成し次第，ご報告いたします．

[k-okada]

From nao.luna...@gmail.com on September 01, 2013 23:23:25

編集をしている際に気づいたことなのですが，

サーボのset・・・やget・・・の関数で以下のように「どのidのサーボでも指を開く方を正の値にしている関数」と「サーボのidによっては指を閉じる方向を正とする関数」ものがあることがわかりました．

・どのidのサーボでも指を開く方を正の値にしている関数 setJointAngle getJointAngle getJointAngles

・サーボのidによっては指を閉じる方向を正とする関数 setJointAngles setJointAnglesOfGroup

この正負の違いもどちらかに統一したほうが便利だと思っています．

質問の1つ目として， 出来れば，今回の編集の際に一緒に，すべて「開く方を正とする関数」の方にまとめたほうが使いやすいと考えているのですが，どうでしょうか．

また，関連する質問ですが，「サーボのidによっては指を閉じる方向を正とする関数」について 例えば，setJointAnglesでは， hrpsys-base-source/rtc/ServoController/ServoController.cpp の250行目あたりの

　　rad[i] = (angles.get_buffer()[i]*servo_dir[i]+servo_offset[i]); を 　　rad[i] = (angles.get_buffer()[i]+servo_offset[i]); とservo_dir[i]をなくすことで「どのidのサーボでも指を開く方を正の値にしている関数」に変えることが出来ました．

/opt/jsk/etc/HIRONX/hrprtc/Robot.confの 　　servo.dir 1,1,-1,-1,1,1,-1,-1 という記述を見た限りではservo.dirは開く方向を正とするようにしている補正のように見えたのですが， 実際はその逆で開く方向を負にしています．

質問の2つ目は servo.dirは開く方向を負にしたい何か目的があって入れられているのか． ということです．

申し訳ありませんが，よろしくお願いいたします．

[k-okada]

From kei.ok...@gmail.com on September 04, 2013 17:06:23

あれ？これ返事していなかったっけ？

開く方向が正，閉じる方向が正，座標系に合わせる

がかんがえられますが，今のしようになっている理由は特に無いです． しいてあれば，たしか現状のインターフェースとあわせたか，あるいはjskスタイル に合わせたかどちらかだったようね．

いまのGRX版での（山形大のHIROは同じハンドですか？）インターフェースの 一覧みたいなのはわかるかな．＞秋葉くん．なければ三宅くん前のGRX版のときの仕様はわかるかな． それと比較して同じかどうか．しりたいです．

[k-okada]

From ke...@jsk.imi.i.u-tokyo.ac.jp on September 04, 2013 17:19:18

三宅です。

GRX版、vpython版では、servo_dirを掛けて、「座標系に合わせる」ようになっていました。

ただ、サーボ1つ1つに対して指令値を送って凝った形を作るときには、 「開く方向が正」の方が考えやすいと思います。

内部では今までの仕様に合わせておき、 色々いじるのはpython上に留めておくのが良いでしょうか。

[k-okada]

From kei.ok...@gmail.com on January 30, 2014 03:02:53

Status: Fixed