Armed turtlebot in Gazebo

dxl_armed_turtlebot/README.md

@@ -0,0 +1,88 @@
+## Smaples in Gazebo
+
+まずは、下記のコマンドを実行し、gazeboシステム及び仮想ロボットを立ち上げます
+```
+$ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch
+```
+
+その場合、以下のような画面になりと思います。
+
+![gazebo_turtlebot](images/gazebo_turtlebot2.png)
+
+#### 注意:
+gazeboのgui (右図) を立ち上げると、PCの処理負荷が一気に上がるので、通常は`gui:=false`というオプションをつけて、rvizのみ表示させることを推奨します。
+その場合、コマンドは以下のようになります。
+
+```
+$ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch gui:=false
+```
+
+### 1. Grasping test in gazebo via roseus
+
+以下のコマンドを実行し、roseusから目の前にある物体を掴んでみましょう。
+
+```
+$ roscd dxl_armed_turtlebot/euslisp
+$ roseus gazebo-grasp-test.l
+```
+
+このサンプルでは、eusのロボットモデルである`*ri*`を駆使して、台車移動、アーム操作、グリッパー操作という基本動作を実行しています。
+成功すると以下のようになります(gazebo guiの表示画面)。
+
+![gazebo_turtlebot_grasp](images/gazebo_turtlebot_grasp.png)
+
+### 2. Object recognition and approaching via roseus
+前の例では、物体認識を行っておりません。物体認識を踏まえた自律動作は以下のように実行します。
+
+1. 物体認識を実行する. これは演習で習ったもので、詳細は演習資料を参考してください。
+   ```
+   $ roslaunch dxl_armed_turtlebot hsi_color_filter.launch DEFAULT_NAMESPACE:=/camera/depth INPUT:=points h_min:=-120 h_max:=-20 s_min:=120
+   ```
+2. 認識結果を元に、自律行動を実行する
+   ```
+   $ roscd dxl_armed_turtlebot/euslisp
+   $ roseus gazebo-grasp-with-recognition.l
+   ```
+
+### 3. SLAM in Gazebo
+gazebo内でも地図作成は、実機と同様、下記のコマンドを実行するだけ:
+```
+$ rosrun slam_karto slam_karto
+```
+
+rviz上で、2D地図ができていることが確認できるでしょう。
+ロボットの移動はroseusからでもできるが、ここでは、下記のコマンドを紹介します:
+```
+$ roslaunch turtlebot_teleop keyboard_teleop.launch
+```
+機体を一周旋回させると、以下のようになります。
+
+![gazebo_turtlebot_slam](images/gazebo_turtlebot_slam.png)
+
+### 4. Detect Checkerboard in Gazebo
+gazebo内でも実機と同様にcheckerboardを検出することができる
+
+まず、checkerboader_detectorを起動しましょう:
+```
+$ roslaunch roseus_tutorials checkerboard-detector.launch rect0_size_x:=0.02 rect0_size_y:=0.02 grid0_size_x:=7 grid0_size_y:=4 translation0:="0 0 0" image:=image_raw  group:=/camera/rgb frame_id:=camera_rgb_frame
+```
+
+
+次に、上記と同じコマンドを使って、ロボットを旋回させましょう:
+```
+$ roslaunch turtlebot_teleop keyboard_teleop.launch
+```
+
+最後に、rviz上でchecker_boardが見つかれば、下記のコマンドでroseus上でcheckerboardを表示しましょう:
+
+```
+$ roscd dxl_armed_turtlebot/euslisp
+$ roseus display-checkerboard.l
+```
+
+最終てきには、以下のようになります。
+
+![gazebo_checkerboard](images/gazebo_checkerboard.png)
+
+
+

dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml

@@ -3,50 +3,63 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50  
 # Trajectory Controllers ---------------------------------------
-trajectory_controller:
+fullbody_controller:
   #type: effort_controllers/JointTrajectoryController
   type: velocity_controllers/JointTrajectoryController
   joints:
-    - arm_link1_joint
-    - arm_link2_joint
-    - arm_link3_joint
-    - arm_link4_joint
-    - arm_link5_joint
-    - arm_link6_joint
-    - arm_link7_joint
+    - arm_joint1
+    - arm_joint2
+    - arm_joint3
+    - arm_joint4
+    - arm_joint5
+    - arm_joint6
   constraints:
     goal_time: 0.5                   # Override default
     stopped_velocity_tolerance: 0.02 # Override default
-    arm_link1_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint1:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link2_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint2:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link3_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint3:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link4_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint4:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link5_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint5:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link6_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+    arm_joint6:
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link7_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+
+  gains: # Required because we're controlling an effort interface
+    arm_joint1: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint2: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint3: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint4: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint5: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint6: {p: 25.0, i: 0.0, d: 0.0}
+  state_publish_rate:  100            # Override default
+  action_monitor_rate: 100            # Override default
+  stop_trajectory_duration: 0        # Override default
+
+gripper_controller:
+  #type: effort_controllers/JointTrajectoryController
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+    - gripper_joint
+  constraints:
+    goal_time: 0.5                   # Override default
+    stopped_velocity_tolerance: 0.02 # Override default
+    gripper_joint:
+      #trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
 
   gains: # Required because we're controlling an effort interface
-    arm_link1_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link2_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link3_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link4_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link5_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link6_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link7_joint: {p: 5.0, i: 0.0, d: 0.0}
+    gripper_joint: {p: 10.0, i: 0.0, d: 0.0}
   state_publish_rate:  100            # Override default
   action_monitor_rate: 100            # Override default
   stop_trajectory_duration: 0        # Override default

dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l

@@ -14,7 +14,7 @@
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 (defclass dxl-armed-turtlebot-interface
   :super robot-interface
-  :slots ())
+  :slots (gripper-action))
 
 (eval `(defmethod dxl-armed-turtlebot-interface
          ;; dxl-7dof-arm-interface, turtlebot-interfaceのメソッドをそれぞれdefmethodする
@@ -48,4 +48,4 @@
   (send *irtviewer* :change-background #f(0.9 0.9 0.9))
   (send *irtviewer* :draw-objects)
   )
-(warn ";; (dxl-armed-turtlebot-init) ;; for initialize ~%")
+(warn ";; (dxl-armed-turtlebot-init) ;; for initialize ~%")

dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l

@@ -63,6 +63,8 @@
      (send self :method-copying "-pose")
      (send self :method-copying "_joint")
      (send self :method-copying "inverse-kinematics" t)
+     (send self :method-copying "gripper" t)
+     (send self :method-copying "arm" t)
      t))
   (:method-copying
    (substr &optional (use-args nil))
@@ -79,10 +81,12 @@
             ,(if use-args `(send* arm-robot ,me args) `(send arm-robot ,me))
             )
            ))
-       )))
+       )
+     methods))
+
   ;; limbs
-  (:arm (&rest args)
-        (unless args (setq args (list nil))) (send* self :limb :rarm args))
+  ;;(:arm (&rest args)
+  ;;(unless args (setq args (list nil))) (send* self :limb :rarm args))
   )
 
 ;; アーム台車モデル生成関数

dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l

@@ -0,0 +1,19 @@
+;; robotの初期化
+(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
+(dxl-armed-turtlebot-init)
+
+;; arm approach  pose
+(send *ri* :angle-vector #f(0 0 -90 0 90 0) 3000)
+(send *ri* :wait-interpolation)
+
+;; open gripper
+(send *ri* :stop-grasp)
+
+;; go-pos
+(send *ri* :go-pos 0.56 0 1)
+
+;; grasp
+;; still some bug in start-grasp, so we have to directly use :move-gripper
+(send *ri* :move-gripper -30 :tm 1500 :wait t)
+(send *ri* :wait-interpolation)
+(send *ri* :angle-vector #f(90 0 45 0 45 45)) ;;  arm tuck pose

dxl_armed_turtlebot/euslisp/gazebo-grasp-with-recognition.l

@@ -0,0 +1,91 @@
+;; robotの初期化
+(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
+(dxl-armed-turtlebot-init)
+
+(ros::load-ros-manifest "jsk_recognition_msgs")
+
+(defvar *topic-name* "/camera/depth/boxes")
+(defvar *bounding-box-list* nil)
+
+(setq found-obj nil)
+(setq obj-pos #f(0 0 0))
+
+;; ros::initする
+(ros::roseus "boundingboxarray_subscriber")
+
+;; コールバック関数
+(defun bounding-box-array-cb (msg)
+  (setq *bounding-box-list* (send msg :boxes)) ;; boxesは、BoundingBoxのArray(Euslispではlist)
+  (when *bounding-box-list*
+    (let* ((b (elt *bounding-box-list* 0))
+           (cam->obj-coords (ros::tf-pose->coords (send b :pose)))
+           (cam-coords (send (send *dxl-armed-turtlebot* :camera_depth_optical_frame_lk) :copy-worldcoords)))
+      (setq obj-pos (scale 0.001 (send (send cam-coords :transform cam->obj-coords) :worldpos)))
+      (setq found-obj t)
+      obj-pos))
+  )
+
+(ros::subscribe *topic-name* jsk_recognition_msgs::BoundingBoxArray #'bounding-box-array-cb 1)
+
+(setq found-obj nil)
+(until found-obj
+       (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう
+       (ros::spin-once)
+       (ros::sleep)
+  )
+
+(ros::ros-warn "found target ojbect ~A, appoaching" obj-pos)
+
+(setq 2d-pos (float-vector (elt obj-pos 0) (elt obj-pos 1)))
+
+(send *ri* :go-pos
+      (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 0)
+      (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 1)
+      (rad2deg (atan  (elt obj-pos 1) (elt obj-pos 0))))
+
+;; open gripper
+(send *ri* :stop-grasp)
+
+
+(setq found-obj nil)
+(until found-obj
+       (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう
+       (ros::spin-once)
+       (ros::sleep)
+  )
+
+(ros::ros-warn "re-found target object ~A" obj-pos)
+(setq target-cds (make-coords :pos (scale 1000 obj-pos)))
+(send target-cds :translate #f(-200 0 50)) ;;z should be 0, but the link is not rigid in gazebo, so 100 is the height offset for end effector.
+(objects (list *dxl-armed-turtlebot* target-cds))
+
+(send *dxl-armed-turtlebot* :angle-vector #f(0 0 -90 0 90 0))
+
+(send *dxl-armed-turtlebot* :arm :end-coords :translate #f(0 30 0)) ;; change the end-effector position
+
+(send *dxl-armed-turtlebot* :inverse-kinematics target-cds :rotation-axis :y)
+
+#|
+(send *dxl-armed-turtlebot* :angle-vector (map float-vector #'(lambda(ang)
+                                     (cond
+                                      ((> ang 90) (- ang 180))
+                                      ((< ang -90) (+ ang 180))
+                                      (t ang)
+                                      ))
+                                 (send *dxl-armed-turtlebot* :angle-vector)))
+|#
+
+(ros::ros-warn "the grap arm pose is ~A" (send *dxl-armed-turtlebot* :angle-vector))
+
+(send *ri* :angle-vector  (send *dxl-armed-turtlebot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
+
+(send *ri* :go-pos 0.1 0 0)
+
+;; grasp
+;; still some bug in start-grasp, so we have to directly use :move-gripper
+(send *ri* :move-gripper -30 :tm 1500 :wait t)
+(send *ri* :wait-interpolation)
+
+(send *ri* :angle-vector #f(80 0 45 0 45 45)) ;;  arm tuck pose
+(send *ri* :go-pos -0.6 0 0) ;;  arm tuck pose

dxl_armed_turtlebot/launch/dxl_armed_turtlebot_gazebo.launch

@@ -53,14 +53,23 @@
     <remap from="$(arg camera)/scan" to="$(arg scan_topic)"/>
   </node>
 
+  <!-- Velocity muxer -->
+  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
+        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
+    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
+    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
+  </node>
+
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <!-- load the controllers -->
   <rosparam file="$(find dxl_armed_turtlebot)/config/dxl_armed_turtlebot_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager"
         type="spawner" respawn="false"
         output="screen"
         args="joint_state_controller
-              trajectory_controller
+              fullbody_controller
+              gripper_controller
               "/>
   <!-- send tuck pose -->
   <node name="tuck_arm" pkg="dxl_armed_turtlebot" type="tuck_arm.py" args="-t" />

dxl_armed_turtlebot/package.xml

@@ -53,4 +53,9 @@
   <run_depend>teleop_twist_keyboard</run_depend>
   <run_depend>roseus_tutorials</run_depend>
   <run_depend>slam_karto</run_depend>
+
+  <export>
+    <gazebo_ros gazebo_model_path="${prefix}/worlds/model" />
+  </export>
+
 </package>