Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
tongtybj opened this issue Sep 2, 2018 · 18 comments
Open

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

tongtybj opened this issue Sep 2, 2018 · 18 comments

Comments

@tongtybj
Copy link
Collaborator

tongtybj commented Sep 2, 2018

@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
Copy link
Collaborator Author

tongtybj commented Sep 3, 2018

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
Copy link
Member

k-okada commented Sep 4, 2018 via email

@tongtybj
Copy link
Collaborator Author

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
Copy link
Collaborator Author

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

dynamixel_ax12

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

dynamixel_mx28

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

@k-okada
Copy link
Member

k-okada commented Sep 11, 2018 via email

@tongtybj
Copy link
Collaborator Author

関連で一つ質問ですが、
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
Copy link
Member

k-okada commented Sep 11, 2018 via email

@tongtybj
Copy link
Collaborator Author

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
Copy link
Member

k-okada commented Sep 17, 2018 via email

@tongtybj
Copy link
Collaborator Author

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
Copy link
Member

k-okada commented Sep 18, 2018 via email

@tongtybj
Copy link
Collaborator Author

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

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

次に、

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

@k-okada
Copy link
Member

k-okada commented Sep 28, 2018 via email

@tongtybj
Copy link
Collaborator Author

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

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

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

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

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

@tongtybj
Copy link
Collaborator Author

残りの仕事:

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

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

@tongtybj
Copy link
Collaborator Author

tongtybj commented Nov 1, 2018

今月の進捗:

  1. urdfモデルの修正(46e62e8 )、以下の問題を解決し, さらに、物体把持時のIK部分を少し改善し(791dcac)、確実につかめるようにしました

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

  1. checkerboardの認識を入れました(b36b65e)
    gazebo_checkerboard

@tongtybj
Copy link
Collaborator Author

tongtybj commented Nov 2, 2018 •
edited
Loading

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

の解決にあたり、 joint trajectory controllerのpath_toleranceを緩和しました(f5881fa).

これで、path_tolerance_violatedは消え、eusの方の:wait-interpolationもちゃんと動作するようになりました。

ただ、不可解なのは、今度はgoal_tolerance_violated = -5のエーラコードが発生するようになり、これは、

robot-programming/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml

Line 21 in f5881fa

goal: 0.02 # Not enforced if unspecified

を0にしても治りませんでした。

このエラーは/fullbody_controllerの最後のタイミングで吐き出すので、eusへの影響はない模様。

@tongtybj
Copy link
Collaborator Author

tongtybj commented Nov 2, 2018

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

これで、gazeboで残る仕事は台車のナビゲーションの方で、move_baseが使えるようにrobot-interface.lを直して行きます。

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@k-okada @tongtybj