search

start-jsk / rtmros_common

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

eusからIMUの値を取得したい #69

Closed k-okada closed 10 years ago

k-okada commented 10 years ago

From nakaokat on April 10, 2013 17:28:33

ロボットが転んだ状態を認識できるように、euslispからジャイロや加速度センサの値を取得したいのですが、 現状できるようになっていますでしょうか。

HrpsysSeqStateROSBridgeにはgsensorやgyrometerのInPortが宣言されているようですが、 ROS側に出力されているようには見えないので、ここから変更する必要があるかもしれません。

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

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 10, 2013 01:39:07

まえどこかで議論されていた気がしますが，tfでロボット腰までの情報をだしてあるはずで，rvizで見た時にロボットが転べば，rvizの中のロボットも転ぶようにしておいて，そのtfをeusでよんでくるのでいいとおもいます． http://www.ros.org/reps/rep-0120.html をみてぱっと考えるに，base_footprint -> base_link は歩行プログラムがだす，footprintからのロボットの腰位置姿勢にしておきつつ，gyroをつかって /odom /base_footprintをだしていく，というのがいい気がしましたが，どうでしょうか．

k-okada commented 10 years ago

From nothewo...@gmail.com on April 10, 2013 02:00:10

各値に対してreferenceな値とactualな値と 両方ほしくなりますが、そのあたりはどうしましょうか。 (ZMP, WaistRPY, WaistPos, JointAngles, Forces, Moments, ...)

今はjointに関しては、joint_statesがactualで trajectoryのdesiredがreferenceな値になっていて、 別々なtopicになっていますね。

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 10, 2013 05:43:38

/imu /lhand_force みたいにルートにあるものはactualなもので， referenceは各ノード総体ででいろいろ出すのが，流れでしょうか． まずはactualなものでいいのでは．

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 10, 2013 11:02:14

とすると

base_footprint -> base_link は歩行プログラムがだす，footprintからのロボットの腰位置姿勢にしておきつつ，gyroをつかって /odom /base_footprintをだしていく

のではなくbase_footprint -> base_link をimuからつくるので良さそうですね 実際はKalmanFilterをとおせばRpyになってstateholderで姿勢が出力されていると思います。

k-okada commented 10 years ago

From nakaokat on April 10, 2013 22:01:05

カルマンフィルタとしては以下のようなものがあります。 /imu/data_rawを読んで/imu/dataに出してくれるようです。 http://ros.org/wiki/imu_filter_madgwick

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 10, 2013 22:02:54

RTCでKalmanFilter.soがあります．

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 10, 2013 22:05:52

補足 hrpsys/lib/KalmanFilter.so です．

k-okada commented 10 years ago

From nakaokat on April 10, 2013 23:36:32

なるほど、RTCの世界ではこれを使って RobotHardware.accOut,rateOut->KalmanFilter.accIn,rateIn KalmanFilter.rpyOut->StateHolder.baseRpyIn と繋いでやればよさそうですね。

そうするとStateHolderのbaseTformがHrpsysSeqStateROSBridgeに繋がれているので、 /odomからbase_linkを見たときにIMUの姿勢が見えるということで良いでしょうか。

また、HrpsysSeqStateROSBridgeにはこれとは別にgsensorInとgyrometerInというポートがあるので、 これを別途/imuに出してやると良さそうです。

k-okada commented 10 years ago

From kei.ok...@gmail.com on April 11, 2013 08:29:41

それでいいとおもいます．hrpsys/scripts/hrpsys.pyはそうなっています．

で，activateCompsで　serializeComponentsして*.start()していますが， serializeComponentsの中身をかえて，もう一回やってうまく動けばオリジナルRTCがある ロボットでもhrpsys.pyを使いまわせますのでそれで上手く行くか試してください．

deserializeComponentsみたいな関数あるいは，rh.stopしてserializeしてstartするなど 上手く行く条件はある気がします．

k-okada commented 10 years ago

From nakaokat on April 23, 2013 22:36:31

遅くなりました。

hrpsys/scripts/hrpsys.pyでは、seq.baseRpy->sh.baseRpyInとなっているようで。 この繋ぎ方だとkfからの出力がどこにも繋がらず、HRP4C.launchで転ばせてもrviz上では転びませんでした。 kf.rpyからsh.baseRpyInに繋いでやると、rviz上でも転びます。

それとも、shのTformをHrpsysSeqStateROSBridgeに繋いで/odomを出しているのは、 shの繋ぐ位置を間違えて使っていた頃の名残でしょうか。

今のところ、 sh.basePosOut -> seq.basePosInit sh.baseRpyOut -> seq.baseRpyInit seq.basePos -> sh.basePosIn seq.baseRpy -> sh.baseRpyIn seq.zmpRef -> sh.zmpIn seq.accRef -> kf.accRef rh.rate -> kf.rate rh.acc -> kf.acc と繋がっていて、shで保持されている値がROSBridge経由で/odomに反映されるようになっています。 seqにPosやRpyが入っている理由については、setInitialStateしたときにbasePosInitやbaseRpyInitが使われているようです。 ソースを見ると、zmpの計算に使われているように見えます。

stateholderのbasePosInとbaseRpyInはセンサ情報にするのか指令値にするのか、というところについて、方針はありますでしょうか。

k-okada commented 10 years ago

From noz...@jsk.imi.i.u-tokyo.ac.jp on April 24, 2013 00:29:17

StateHolderは指令値を入力にするので、 HrpsysSeqStateRosBridgeに入力するのは別にする必要があります。

HrpsysSeqSateRosBridgeの方は、baseTformが指令値・計測値の 2種必要だとして、さらに悩みどころは

k-okada commented 10 years ago

From nakaokat on April 24, 2013 03:06:01

ということでしょうか。 ポートの名前はrsなど付けたほうが統一感が出るでしょうか。

rvizでは現在状態が見えた方が良いと思うのですが、そうすると今はStateHolderのbaseTformから/odomを作っていますが、 これを計測値(baseRpy, basePos)から作るように変えたほうが良いと思います。

gyro,accがないロボットの場合は、単にポートを繋がずに初期値の0を出しておけばよさそうですが、問題ありますでしょうか。

k-okada commented 10 years ago

From nakaokat on April 24, 2013 04:38:33

いろいろなlaunchが動かなくなるのでまだコミットしていませんが、 baseRpy, basePosを追加し、tfをこれらから計算するようにしてみました。

kf.RpyからbaseRpyに繋ぎ、初期値はStateHolderを参考にモデルから取ってくるようにしました。

hrpsys_ros_bridge_tutorials hrp4c.launchで検証していますが、 rvizでFixedFrameを/odomとして起動するので、gyroもaccもない場合/odomが吐かれず表示が更新されません。 kfが出力しない場合を考慮して、baseTformを受信したタイミングで baseRpyとbasePosから計算した/odomを吐くようにしてみました。

k-okada commented 10 years ago

From nakaokat on May 01, 2013 00:45:50

r3825 で、HrpsysSeqStateROSBridgeにbasePos, baseRpyを追加し、 RobotHardwareからkfに、またkfからHrpsysSeqStateROSBridgeにIMUのデータを繋ぐようにしました。

また、/odomはそのままで、base_link -> /base_footprintをIMUのデータを使って出すようにしました。

初期状態や姿勢によって、出力が暴れるときがあるようですが、 とりあえずはhrpsysの世界からROSにIMUデータを取ってこられるようになりました。

eusにはtfを扱う関数など定義されているのでしょうか。

k-okada commented 10 years ago

From nakaokat on May 01, 2013 02:49:34

atlasなどモデルにIMUを持っていないロボットでは、RobotHardwareにrateとaccのポートが生成されないようで、 rtconnectが途中で止まってしまいサービスなどがROSから動かなくなってしまうようです。

とりあえずのところ、hrpsys_ros_bridge.launchで当該部分をコメントアウトしておきました。

ポートがあるかどうか確認して、hrpsys.pyで繋ぐようにするのがよさそうです。

k-okada commented 10 years ago

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

hrpsys_ros_bridge.launchの

はいりますか？ 今は、

  • simulatorの加速度・ジャイロのデータポートは VRMLのセンサの名前と同じ名前になっていると思います。 hrpsys/bin/ProjectGenerator がこれに関係するROBOT.xmlを生成しています。 RobotHardwareのデータポートも、VRMLに即した名前のデータポートになっていると思います。
  • hrpsys/scripts/hrpsys.pyの次の箇所で、 kfに自動でつないでいます。 s_acc=filter(lambda s : s.type == 'Acceleration', self.getSensors(self.url)) if (len(s_acc)>0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
    connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc')) s_rate=filter(lambda s : s.type == 'RateGyro', self.getSensors(self.url)) if (len(s_rate)>0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
    connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))
k-okada commented 10 years ago

From noz...@jsk.imi.i.u-tokyo.ac.jp on May 01, 2013 03:28:18

すいません途中でおくってしまいました。

なので、kfと$(simulator)をつなぐのは、書かなくても もうつながっていると思うので、確認していただけると助かります。

なお、hrpsys_ros_bridge.launchはROS Bridge系のデータポート接続をやるので、 kfと$(simulator)などのROS Bridgeなくても動くべきポート同士の接続は、 hrpsys/scripts/hrpsys.pyでやるのがおすすめです。

k-okada commented 10 years ago

From nakaokat on May 01, 2013 03:50:28

なるほど、更新されていたのに気づきませんでした。 hrpsys.pyで繋ぐのが良いですね。

HRP4C.xmlの中身でgsensorとgyrometerに書き換えると、こちらも正常に動作しました。

k-okada commented 10 years ago

From noz...@jsk.imi.i.u-tokyo.ac.jp on May 01, 2013 16:20:11

r3812 のコミットで、

RobotHardwareからkfに、またkfからHrpsysSeqStateROSBridgeにIMUのデータを繋ぐようにしました。

のほかにRobotHardwareServiceが

のようにつなげられていましたが、 SimulatorはRobotHardwareServiceをもたないので、現状コメントアウトしておきました。 実機の.launchの部分に上記を書く必要があるので、注意してください。

k-okada commented 10 years ago

From nakaokat on May 06, 2013 23:48:59

今/BODY -> /base_footprintという親子関係でimuから計算した姿勢を出しているのですが、 jsk-ros-pkgのpr2eus/robot-interface.lを参考にすると、 (setq c (send tfl :lookup-transform "/base_footprint" "/BODY" (ros::time))) とするとこの姿勢を取ってこられるようです。

k-okada commented 10 years ago

From nakaokat on May 08, 2013 00:53:17

eus側で(send ri :state :worldcoords)は/map -> /base_footprintを読んでいるので、 IMUで/BODY -> /base_footprintを出していると姿勢がおかしくなってしまうようです。

とりあえずのところ、歩行器は/odom -> /body_linkを出すとして、 IMUはIMUの付いているリンク -> /imu_floorを出す、 eus側のインターフェースとして、rtm-ros-robot-interface.lに姿勢を取ってくる:state :imucoordsを追加、 という方向で進めてみようと思います。 http://www.ros.org/reps/rep-0105.htmlを読む限り、 将来的には歩行器の出す情報とIMUから/odom -> /body_linkを出し、 この位置姿勢情報に加えてポイントクラウドなどから自己位置推定した結果で、 /map -> /body_linkが推定結果と一致するように/map -> /odomを出すというようにするのが良さそうです。

ちなみに、topicでIMUの値として/imuを出してしまうと、gazeboとぶつかってしまいます。

k-okada commented 10 years ago

これはどうなっているんだっけ？ @eisoku9618

garaemon commented 10 years ago

これどうなっていますか? @eisoku9618 @tnaka

garaemon commented 10 years ago

今staroでは/odom -> /body_linkのtfは出ていますか?

snozawa commented 10 years ago

これは自分の記憶が正しければ imu_floorというframeがでていて、

(send *ri* :state :imucoords)

ではEuslispでは取得できると思います。 imucoordsは、imuの値を反映させたルートリンクの姿勢がかえってきます。 ただ、今コードをみると、ルートリンク座標と(send robot :worldcoords)が違う ロボットだと正常にうごかないきがしてきました。

garaemon commented 10 years ago

imu_floorってなんなのでしょうか？ 試して見た感じ、位置が変わらない水平面でしょうか？

2014年3月27日木曜日、snozawanotifications@github.comさんは書きました:

これは自分の記憶が正しければ imu_floorというframeがでていて、

(send ri :state :imucoords)

ではEuslispでは取得できると思います。 imucoordsは、imuの値を反映させたルートリンクの姿勢がかえってきます。 ただ、今コードをみると、ルートリンク座標と(send robot :worldcoords)が違う ロボットだと正常にうごかないきがしてきました。

— Reply to this email directly or view it on GitHubhttps://github.com/start-jsk/rtmros_common/issues/69#issuecomment-38694762 .

from iPhone

tnaka commented 10 years ago

出ているはずです。 /odom -> /body_link はStateHolderの出すものをそのまま出していたはずです。 STAROでは普段は/odomのかわりに/imu_floorを使用していて、こちらではIMUの姿勢が反映されていますが、歩いても位置に関しては移動しません。

garaemon commented 10 years ago

hrp2でもshの値を出しているように見えるけど、歩行では位置姿勢が動いていません。

2014年3月27日木曜日、tnakanotifications@github.comさんは書きました:

出ているはずです。 /odom -> /body_link はStateHolderの出すものをそのまま出していたはずです。 STAROでは普段は/odomのかわりに/imu_floor を使用していて、こちらではIMUの姿勢が反映されていますが、歩いても位置に関しては移動しません。

— Reply to this email directly or view it on GitHubhttps://github.com/start-jsk/rtmros_common/issues/69#issuecomment-38694925 .

from iPhone

tnaka commented 10 years ago

hrp2でもshの値を出しているように見えるけど、歩行では位置姿勢が動いていません。

あれ、2月頃試したときには動いていましたよね？ たしかあの頃、野沢さんに手伝っていただいてhrpsys_ros_bridgeでも使えるようになっていたはずですが、 rtcの繋ぎ変えが必要だったような気もしてきました。

garaemon commented 10 years ago

あ、あれはOpenHRPの古いやつで、hrpsys-baseではないです。 ros bridge使うバージョンでは僕はまだ動いてるのを見たことないです。

staroで動いてると参考になると思っています

2014年3月27日木曜日、tnakanotifications@github.comさんは書きました:

hrp2でもshの値を出しているように見えるけど、歩行では位置姿勢が動いていません。 あれ、2月頃試したときには動いていましたよね？ たしかあの頃、野沢さんに手伝っていただいてhrpsys_ros_bridgeでも使えるようになっていたはずですが、 rtcの繋ぎ変えが必要だったような気もしてきました。

— Reply to this email directly or view it on GitHubhttps://github.com/start-jsk/rtmros_common/issues/69#issuecomment-38695507 .

from iPhone

tnaka commented 10 years ago

以下のメールを発掘しました。 shはうまく動いていなくて、abcからの出力を見てやる必要があるようですね。 ただその場合でも、RPYがセンサから見た表現かodomから見た表現かというところで食い違いが発生するようです。

abc.baseRpyをHrpsysSeqStateROSBridge.baseRpyに、 abc.basePosをHrpsysSeqStateROSBridge.basePosにつないでhrpsys-simulatorでうごかすと、 /imu_floorが動いていること確認しました。

ただ、以前からシミュレータでありましたが、/imu_floorからみるとロボットが逆さになってしまいます。 左右に腰を動かすのも逆に見えました。 これはkf.rpyを繋いでみても同じでした。

abcの出力を繋ぐと、実機でも逆さになりました。 AutoBalancerで hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot->rootLink()->R); m_baseRpy.data.r = baseRpy(0); m_baseRpy.data.p = baseRpy(1); m_baseRpy.data.y = baseRpy(2); としていますが、KalmanFilterでは hrp::Matrix33 realRotationMatrix = imaginaryRotationMatrix * m_sensorR; // inverse transform to real data hrp::Vector3 euler = realRotationMatrix.eulerAngles(2,1,0); m_rpy.data.y = euler(0); m_rpy.data.p = euler(1); m_rpy.data.r = euler(2); としていて、どうやらKalmanFilterのrpy出力はセンサ座標系で出ていて、 AutoBalancerのrpy出力はベースリンク座標系で出ているのが原因のようです。 HrpsysSeqStateROSBridgeで出すTFではセンサ座標系を想定しているので、 ROS側のTFの変換のせいで逆さになってしまっていそうです。 一般的にはどちらに揃えるのが良いでしょうか。

k-okada commented 10 years ago

memoです．

＝＝ http://www.ros.org/reps/rep-0120.html と整合性があるとやりやすいと思います． imu_floor は無いんじゃないかな．

本来は、 map -> odom -> (base_link) -> pelvis となるのが正しい。 参照： http://www.ros.org/reps/rep-0120.html (HrpsysSeqStateROSBridgeとしては imu_floorは base_link -> base_footprint に相当するか)

tnaka commented 10 years ago

https://github.com/start-jsk/rtmros_common/issues/69#issuecomment-34535504 にあるとおり、

というのが正しいです。 ただ、今のところ取得可能な情報が

だけで、abcとkfの両方からの情報がないというのと、自己位置推定モジュールがなく/map -> /odomが出ないというので現状のようになっていました。

garaemon commented 10 years ago

imuは/imuにsensor_msgs/Imuを出して、odomも/odomにgeometry_msgs/TwistWithCovarianceStampedに出しておくと、robot_pose_ekfが使えますね。

ただ、hrpsysのkfでカルマンフィルタは全部やってあげるのが良い気がします

garaemon commented 10 years ago

@eisoku9618 さんから催促があったのでこの問題にケリをそろそろつけましょう。

@snozawa さん、abcからtformを出すようにするという変更をお願いできますか？ ros bridge以降はこちらでも修正可能です。

k-okada commented 10 years ago

cllsing, if it still happens please reopen this.