ROSTankControllerが動かない

RyodoTanaka · August 19, 2019, 8:33pm

こんにちは．
表題の通り，公式のchoreonoid_ros_samplesから公開されているROSTankControllerが動かせません．
具体的な挙動は以下のとおりです．

1. プラグインの読み込みと実行

choreonoidのGUIから読み込んでいるプラグインは下図のとおりです．
Tankの下にSimpleControllerをインポートし，ROSTankControllerをロードしています．

roscoreを実行し，スタートボタンを押すと，とりあえずシミュレーションは動きます．

2. joyCallback()とcontrol()

ROSTankController.cpp 内のjoyCallbackは問題なく動作しているようです．

github.com/Rogiken-TNK-ROS/choreonoid_ros_samples

src/ROSTankController.cpp

develop


      
              pos[i] = joystick.axes[axisID[i]];
              if(fabs(pos[i]) < 0.2){
                  pos[i] = 0.0;
              }
          }
          // set the velocity of each tracks
          if(usePseudoContinousTrackMode){
              double k = 1.0;
              trackL->dq_target() = k * (2.0 * pos[1] - pos[0]);
              trackR->dq_target() = k * (2.0 * pos[1] + pos[0]);
          } else {
              double k = 4.0;
              trackL->dq_target() = k * (pos[1] - pos[0]);
              trackR->dq_target() = k * (pos[1] + pos[0]);
          }
          
          static const double P = 200.0;
          static const double D = 50.0;
          
          for(int i=0; i < 2; ++i){
              Link* joint = turretJoint[i];

上記リンクのソースコードで検証した所，joyトピックより受け取った値を各リンクの速度司令に渡すことは成功しているようです．
ログ(stdout)は下記の様になりました．

[ INFO] [1566246338.026477026]: trackR->dq() : 0.898837
[ INFO] [1566246338.027734067]: trackL->dq() : 0.898837
[ INFO] [1566246338.027758633]: trackR->dq() : 0.898837
[ INFO] [1566246338.029043965]: trackL->dq() : 0.898837

3. シミュレータ内のモデルが動かない

上記まで検証しましたが，クローラーが動かず，モデルが動きません．
一方，砲塔のJointはjoyトピックを介して動かすことができました．
以上から，LINK::JOINT_SURFACE_VELOCITYに関する部分がうまく起動できていないと思われます．
どのように対処したらよろしいでしょうか？
なお，試している環境とビルドオプションは以下のとおりです．

環境

項目	内容
OS	Ubuntu 18.04 LTS
Kernel	5.0.0-25-generic #26
ROS	melodic
ビルド方法	catkin build
cmake-args	-DBUILD_MULTICOPTER_PLUGIN=ON -DBUILD_MULTICOPTER_SAMPLE=ON -DBUILD_SCENE_EFFECTS_PLUGIN=ON -DBUILD_WRS2018=ON -DUSE_PYTHON3=OFF -DUSE_PYBIND11=OFF -DBUILD_PYTHON_PLUGIN=OFF -DBUILD_PYTHON_SIM_SCRIPT_PLUGIN=OFF -DBUILD_COLLISION_HANDLER_SAMPLE=ON -DBUILD_CONTACT_FORCE_EXTRACTION=ON -DBUILD_DOCUMENTS=ON -DBUILD_TRACKED_VEHICLE_SAMPLE=ON

以上，よろしくお願いします．

RyodoTanaka · August 20, 2019, 6:37am

原因がわかりました．
速度司令を与える際に，上記ソースコードではdq()methodを使用していますが，dq_target()methodを使用していなかったのが原因でした．
choreonoidに用意されているサンプルコード（TankJoystickController.cpp）にdq_target()の使用があったため，それに沿って変更した所，うまく行きました．

github.com

s-nakaoka/choreonoid/blob/master/sample/SimpleController/TankJoystickController.cpp#L134


      
          virtual bool control() override
          {
              joystick.readCurrentState();
              
              double pos[2];
              for(int i=0; i < 2; ++i){
                  pos[i] = joystick.getPosition(
                      i==0 ? Joystick::L_STICK_H_AXIS : Joystick::L_STICK_V_AXIS);
                  if(fabs(pos[i]) < 0.2){
                      pos[i] = 0.0;
                  }
              }
              // set the velocity of each tracks
              if(usePseudoContinousTrackMode){
                  double k = 1.0;
                  trackL->dq_target() = k * (-2.0 * pos[1] + pos[0]);
                  trackR->dq_target() = k * (-2.0 * pos[1] - pos[0]);
              } else {
                  double k = 4.0;
                  trackL->dq_target() = k * (-pos[1] + pos[0]);
                  trackR->dq_target() = k * (-pos[1] - pos[0]);

dq_target() method に変更した所，問題なく動作しました．
なお，本修正分を@nakaoka さんの公式リポジトリにPullRequestを出しました．

お手数をお掛けしますが，レビューをお願いします．

nakaoka · August 20, 2019, 9:31am

本件について、ご報告、調査の上、修正までいただき誠にありがとうございます。
PullRequestをマージしておきました。

ちなみにこの件は以前この掲示板でも以下のトピックで報告していたのですが、ROSサンプルの方まで修正がいっていませんでした。大変申し訳ありません。

https://discourse2.choreonoid.org/t/topic/135

さらに、マニュアルの以下のページにもまだ反映していなかったことに気が付きました。

https://choreonoid.org/ja/manuals/latest/simulation/howto-implement-controller.html

こちらも近いうちに修正しますので、今後ともよろしくお願いします。

RyodoTanaka · August 20, 2019, 9:53am

@nakaoka
ありがとうございます．
下記トピックに気づいておりませんでした．
申し訳ありませんでした．
下記トピックの内容，合わせて承知しました．
ありがとうございます．
また，マージありがとうございました！
https://discourse2.choreonoid.org/t/topic/135

nakaoka · September 5, 2019, 1:51am

これについて、マニュアルの以下のページを修正しておきました。

コントローラの実装
- コントローラの実装 — Choreonoid 開発版ドキュメント
無限軌道の簡易シミュレーション
- 無限軌道の簡易シミュレーション — Choreonoid 開発版ドキュメント
Tankチュートリアルステップ4: クローラの制御
- ステップ4: クローラの制御 — Choreonoid 開発版ドキュメント