実機とシミュレーションのjoint trajectory actionの統一

[tongtybj]

@k-okada 実機とシミュレーションのjoin_trajectory_actionの定義の仕方が違うようですが、なにか理由があるのでしょうか。

実機: https://github.com/jsk-enshu/robot-programming/blob/master/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml#L92-L110
シミュレーション: https://github.com/jsk-enshu/robot-programming/blob/master/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml#L6-L52

シミュレーションは7DOF全部まとめて管理しているため、gazeboではeusのriと連動できないようです（この理解はただしいでしょうか）。実機みたくfull_bodyとgripperに分けるべきでしょうか。

[tongtybj]

trajectory_controllerをfullbodyとgripperに分けてみましたが, tongtybj@d29c4ca
今度は下記のところで躓きました。この"set_compliance_slope"はグリッパ把持に使っているようですね。
gazeboのjoint controllerだとどれになるのでしょうかな。

eustf roseus_c_util 1.irteusgl$ (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
;; (dxl-armed-turtlebot-init) ;; for initialize 
t
2.irteusgl$ (dxl-armed-turtlebot-init)

;; extending gcstack 0x6ab2b30[32738] --> 0x7bb3690[65476] top=7f9b
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;4 face-to-face alignment(s) found.
Color name not found ;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
Color name not found ;;1 face-to-face contact(s) found.
;;1 face-to-face contact(s) found.
Color name not found [ WARN] [1535944901.489334160, 30.600000000]: [/default_robot_interface_1535944900980618698] waiting /clock... 0.001526 seconds elapsed.
[ INFO] [1535944901.489434823, 30.600000000]: [/default_robot_interface_1535944900980618698] /clock is now published.
[ INFO] [1535944901.841059403, 30.950000000]: waitForService: Service [/arm_j1_controller/set_compliance_slope] has not been advertised, waiting...

[k-okada]

#149 (comment) ですね． 目標はシミュレータと実機と同じように動かすことです． で，前提として実機が正しいわけではない，というのがありますので，実機を変えるのも有りだと思ってすすめるのがよいですが， 問題としては #149 (comment) ですね． https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_naoqi_robot/peppereus/pepper-interface.l#L14 みたいな対応でもいいとおもいますし, set_compliance_slope + gazebo で調べるといかが出てくるので，使えるか調べてみてもらえると嬉しいです． https://github.com/team-diana/gazebo_ros_dynamixel_motor_plugin/blob/master/src/gazebo_ros_dynamixel_motor.cpp

…

-- ◉ Kei Okada 2018年9月3日(月) 12:28 趙 漠居(Zhao, Moju) <notifications@github.com>:

trajectory_controllerをfullbodyとgripperに分けてみましたが, ***@***.*** <tongtybj@d29c4ca> 今度は下記のところで躓きました。この"set_compliance_slope"はグリッパ把持に使っているようですね。 gazeboのjoint controllerだとどれになるのでしょうかな。 eustf roseus_c_util 1.irteusgl$ (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") ;; (dxl-armed-turtlebot-init) ;; for initialize t 2.irteusgl$ (dxl-armed-turtlebot-init) ;; extending gcstack 0x6ab2b30[32738] --> 0x7bb3690[65476] top=7f9b ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;4 face-to-face alignment(s) found. Color name not found ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. Color name not found ;;1 face-to-face contact(s) found. ;;1 face-to-face contact(s) found. Color name not found [ WARN] [1535944901.489334160, 30.600000000]: [/default_robot_interface_1535944900980618698] waiting /clock... 0.001526 seconds elapsed. [ INFO] [1535944901.489434823, 30.600000000]: [/default_robot_interface_1535944900980618698] /clock is now published. [ INFO] [1535944901.841059403, 30.950000000]: waitForService: Service [/arm_j1_controller/set_compliance_slope] has not been advertised, waiting... — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3AFBBqhHlFQ6laxULUdnIWFopiYDks5uXKHUgaJpZM4WWyNg> .

[tongtybj]

set_compliance_slope + gazebo で調べるといかが出てくるので，使えるか調べてみてもらえると嬉しいです．

よく見たら、実装されてませんね・・・TODOリストになっています。

https://github.com/team-diana/gazebo_ros_dynamixel_motor_plugin/blob/master/src/gazebo_ros_dynamixel_motor.cpp#L210-L214

#161 のように、何かのフラグでcompliance処理を無視するとかならすぐにできそうです。

 (:start-grasp
   (&optional (arm :arm) &key ((:gain g) 0.5) ((:objects objs) objects))
   "Start grasp mode."
 (unless (ros::get-param "use_sim_time" nil)
   (send self :set-compliance-slope 7 1023)
   (send self :set-torque-limit 7 g))
   (send robot :gripper arm :angle-vector
         (send-all (send robot :gripper arm :joint-list) :min-angle))
   (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller)
   (send self :wait-interpolation :gripper-controller)
   (send self :state)
   (send robot :gripper arm :angle-vector
         (mapcar #'(lambda (x) (- x 5)) (send-all (send robot :gripper arm :joint-list) :joint-angle))) ;; 5[deg]                                                                                           
   (send self :angle-vector (send robot :angle-vector) 200 :gripper-controller)
   (send self :wait-interpolation :gripper-controller)
   )

[tongtybj]

また、complianceという機能はDynamixel AXシリーズ特有のもので、擬似PID制御みたいなもののようです。

他の上位互換のモデルでは、コントロールテーブルのコンプライアンス設定部分がPID設定に取り替えられました。使用時にご注意ください。 らしいです。

ということは、より一般的な調整方法はcompliance_slopeのパラチュンより、サーボプラグインのPIDゲインをdynamic_reconfigureから動的に調整すればいいのかなと思いました。

[k-okada]

よく見たら、実装されてませんね・・・TODOリストになっています。

お，それはラッキーだから実装してみたらよいとおもいます．

…

-- ◉ Kei Okada 2018年9月11日(火) 9:17 趙 漠居(Zhao, Moju) <notifications@github.com>:

set_compliance_slope + gazebo で調べるといかが出てくるので，使えるか調べてみてもらえると嬉しいです． よく見たら、実装されてませんね・・・TODOリストになっています。 https://github.com/team-diana/gazebo_ros_dynamixel_motor_plugin/blob/master/src/gazebo_ros_dynamixel_motor.cpp#L210-L214 #161 <#161> のように、何かのフラグでcompliance処理を無視するとかならすぐにできそうです。 (:start-grasp (&optional (arm :arm) &key ((:gain g) 0.5) ((:objects objs) objects)) "Start grasp mode." (unless (ros::get-param "use_sim_time" nil) (send self :set-compliance-slope 7 1023) (send self :set-torque-limit 7 g)) (send robot :gripper arm :angle-vector (send-all (send robot :gripper arm :joint-list) :min-angle)) (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller) (send self :wait-interpolation :gripper-controller) (send self :state) (send robot :gripper arm :angle-vector (mapcar #'(lambda (x) (- x 5)) (send-all (send robot :gripper arm :joint-list) :joint-angle))) ;; 5[deg] (send self :angle-vector (send robot :angle-vector) 200 :gripper-controller) (send self :wait-interpolation :gripper-controller) ) — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3GFpSGbwWi8sWVquXg6bjAQBG0Hnks5uZwEBgaJpZM4WWyNg> .

[tongtybj]

関連で一つ質問ですが、
https://github.com/jsk-enshu/robot-programming/blob/master/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml#L42-L49
の部分のgainsはeffortベースですか、それともvelocityベースですか？

https://github.com/jsk-enshu/robot-programming/blob/master/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro#L9
を見るとvelocity ベースでが、 effortベースをやめた理由はなんだったんでしょうか。

[k-okada]

実機はエフォートな気がします。 gazeboはシミュレーションの仕方の問題で、当時はvelocityを使うと安定するという噂があったんだと思います。 今はバージョンも違うからeffortでもうまく動きそうな気がします シミュレータの中でうまくものがつかめるか、というのがポイントで、 例えばOpenHRP3のサンプルを動かして見たらいいと思いますが、 歩いて言って机の上のものをつかんでいますが、グリッパの部分だけ velocityかなにかで動かしていたはずです。これぐらい気を使って 調整しないとものが掴めないんだーと思ったのが10年以上前の話でした 2018年9月12日(水) 4:15 趙 漠居(Zhao, Moju) <notifications@github.com>:

関連で一つ質問ですが、 https://github.com/jsk-enshu/robot-programming/blob/master/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml#L42-L49 の部分のgainsはeffortベースですか、それともvelocityベースですか？ https://github.com/jsk-enshu/robot-programming/blob/master/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro#L9 を見るとvelocity ベースでが、 effortベースをやめた理由はなんだったんでしょうか。 — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3BaX2usKWGm7EqjLcNV-r2My5DkGks5uaAvNgaJpZM4WWyNg> .

-- -- ◉ Kei Okada

[tongtybj]

gazeboはシミュレーションの仕方の問題で、当時はvelocityを使うと安定するという噂があったんだと思います。 今はバージョンも違うからeffortでもうまく動きそうな気がします

hardware_interface/EffortJointInterfaceに切り替えて見ましたが、以下の問題が起こりました。

[urdf_spawner-7] process has finished cleanly
log file: /home/chou/.ros/log/5ccaca46-b983-11e8-aafb-54a05056e2bd/urdf_spawner-7*.log
rviz: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreNode.cpp:630: virtual void Ogre::Node::setScale(const Ogre::Vector3&): Assertion `!inScale.isNaN() && "Invalid vector supplied as parameter"' failed.
[ WARN] [1537083452.199473832, 0.330000000]: Gazebo ROS Kobuki plugin: NaN in d1. Step time: 0.01, WD: 0.07, velocity: -nan
[ WARN] [1537083452.199515712, 0.330000000]: Gazebo ROS Kobuki plugin: NaN in d2. Step time: 0.01, WD: 0.07, velocity: -nan
[ WARN] [1537083452.299705087, 0.430000000]: Gazebo ROS Kobuki plugin: NaN in d1. Step time: 0.01, WD: 0.07, velocity: -nan
[ WARN] [1537083452.299738738, 0.430000000]: Gazebo ROS Kobuki plugin: NaN in d2. Step time: 0.01, WD: 0.07, velocity: -nan

出処は
https://github.com/yujinrobot/kobuki_desktop/blob/devel/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp#L78-L96

wheelの制御は
https://github.com/yujinrobot/kobuki_desktop/blob/devel/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp#L78-L96

となると、車輪がsetVelocity()だと、上に付いているアームジョイントもVelocityベースに合わせないとODEがバグるのが根本的に原因なのでしょうかね。

kobukiのリポジトリに以下のissueを投下
yujinrobot/kobuki_desktop#57

[k-okada]

fetchのシミュレーションは台車とグリッパが動いているので、どう動かしているか調べてみると良いと思います 2018年9月17日(月) 5:07 趙 漠居(Zhao, Moju) <notifications@github.com>:

gazeboはシミュレーションの仕方の問題で、当時はvelocityを使うと安定するという噂があったんだと思います。 今はバージョンも違うからeffortでもうまく動きそうな気がします hardware_interface/EffortJointInterfaceに切り替えて見ましたが、以下の問題が起こりました。 [urdf_spawner-7] process has finished cleanly log file: /home/chou/.ros/log/5ccaca46-b983-11e8-aafb-54a05056e2bd/urdf_spawner-7*.log rviz: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreNode.cpp:630: virtual void Ogre::Node::setScale(const Ogre::Vector3&): Assertion `!inScale.isNaN() && "Invalid vector supplied as parameter"' failed. [ WARN] [1537083452.199473832, 0.330000000]: Gazebo ROS Kobuki plugin: NaN in d1. Step time: 0.01, WD: 0.07, velocity: -nan [ WARN] [1537083452.199515712, 0.330000000]: Gazebo ROS Kobuki plugin: NaN in d2. Step time: 0.01, WD: 0.07, velocity: -nan [ WARN] [1537083452.299705087, 0.430000000]: Gazebo ROS Kobuki plugin: NaN in d1. Step time: 0.01, WD: 0.07, velocity: -nan [ WARN] [1537083452.299738738, 0.430000000]: Gazebo ROS Kobuki plugin: NaN in d2. Step time: 0.01, WD: 0.07, velocity: -nan 出処は https://github.com/yujinrobot/kobuki_desktop/blob/devel/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp#L78-L96 wheelの制御は https://github.com/yujinrobot/kobuki_desktop/blob/devel/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp#L78-L96 となると、車輪がsetVelocity()だと、上に付いているアームジョイントもVelocityベースに合わせないとODEがバグるのが根本的に原因なのでしょうかね。 kobukiのリポジトリに以下のissueを投下 yujinrobot/kobuki_desktop#57 <yujinrobot/kobuki_desktop#57> — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3BP1JC9fMMLtmIF6w22YbiMrOb9_ks5ubq-cgaJpZM4WWyNg> .

-- -- ◉ Kei Okada

[tongtybj]

Fetchのシステムを一通り調査してみて、いろいろ違いはありましたが、
本質的には、車輪もアーム関節もすべて、SetForceでODEに渡されているようですね。
https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/fetch_gazebo/include/fetch_gazebo/joint_handle.h#L236

Fetchはros_controlやro_controllerを使わずに、独自のcontrollerを使っているようですね。
https://github.com/fetchrobotics/robot_controllers　

今後の方針として以下3つの候補を考えています。

1. 現状維持, kobuki_gazebo + velocity_controller based arm joints
2. kobuki_gazeboの車輪制御をeffort(force)ベースにして, プラスeffor_controller based arm joints
3. fetchシステムを移植してみる

[k-okada]

ところで，今のgazeboで，演習の課題（チェッカーボード見つけると，色がついた赤いボールに手を伸ばすとか，Mapを作るとか）できているかな．それができれば，今のままでOKなんだけど，できないなら，２，３とできるようにしていきましょう． ◉ Kei Okada 2018年9月17日(月) 13:23 趙 漠居(Zhao, Moju) <notifications@github.com>:

…

Fetchのシステムを一通り調査してみて、いろいろ違いはありましたが、 本質的には、車輪もアーム関節もすべて、SetForceでODEに渡されているようですね。 https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/fetch_gazebo/include/fetch_gazebo/joint_handle.h#L236 Fetchはros_controlやro_controllerを使わずに、独自のcontrollerを使っているようですね。 https://github.com/fetchrobotics/robot_controllers 今後の方針として以下3つの候補を考えています。 1. 現状維持, kobuki_gazebo + velocity_controller based arm joints 2. kobuki_gazeboの車輪制御をeffort(force)ベースにして, プラスeffor_controller based arm joints 3. fetchシステムを移植してみる — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3E234Lh3E3wUAhSpZn2CcPXLrKiBks5ubyOjgaJpZM4WWyNg> .

[tongtybj]

velocity_controller ベースで円柱を掴んでみました。

つかむのはこれが限界でしょうか。

次に、

• チェッカーボード見つける
• 色がついた赤いボールに手を伸ばす
• Mapを作る
  をさくさくやってみます

[k-okada]

すばらしいです。 サンプルプログラムとしてどこかにおいて 使い方のREADMEを作って置いてくれると嬉しいです dxl_armed_turtlebot/samples とか作ってプログラム置いて、 dxl_armed_turtlebot/README.md とかでしょうか。 2018年9月27日(木) 20:45 趙 漠居(Zhao, Moju) <notifications@github.com>:

velocity_controller ベースで円柱を掴んでみました。 [image: grasp3] <https://user-images.githubusercontent.com/3666095/46143829-27001580-c296-11e8-8071-3c9d8cb18801.gif> つかむのはこれが限界でしょうか。 次に、 - チェッカーボード見つける - 色がついた赤いボールに手を伸ばす - Mapを作る をさくさくやってみます — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#241 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3OhCCYcJQ5RxEwn73M6nfgSpCIF8ks5ufLpigaJpZM4WWyNg> .

-- -- ◉ Kei Okada

[tongtybj]

#245 に整理したものを上げました

サンプルは３つ用意しています。
README.mdを参考に試してみてください。

• ひとつ目は決め打ちで物体把持するわけですが、たまに失敗します(10%).

• ふたつ目は物体認識を入れた把持ですが、成功しません。手法は正しいのですが、ひとつバグを発見しました。eusモデルとgazeboのurdfモデルの関節の回転正負方法が逆のものが混じっています。具体的には,joint1,4,6です。それで、tuck-armポーズをすると、eusとrvizは以下のようになります。これを直せば、普通につかめると思います。多分eusモデルのほうが正しいのですが、念の為実機と照らしあわせてからurdfの方を治したいです。
  

• gazeboでslamをする。 send *ri* :go-pos x y dで普通にできます。

[tongtybj]

残りの仕事:

• サンプルコードの修正:
  eusの使い方が拙いので,　このようになっていますが、記憶の中では、send *ri* :wait-interpolationがありましたが、やはり次のコマンドが直後に実行されてしまいます。ご教示いただければ

• move-base-actionの追加
  send *ri* :move-toを実行するとエラーができます。move-base-actionが内容ですね。来月は台車周りを直してみます。台車のナビゲーションの部分はNEDOの業績にもなりますね。