Merge pull request #4 from pazeshun/fix-gripper-state

[panda_eus] Make :gripper method work & enable to get gripper status while moving

jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l

@@ -264,8 +264,14 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka
      ))
   (:send-gripper-action-method
    (arm actions method &rest args)
-   (let ((action (gethash (send self :expand-arm-alias arm) actions)))
-     (if action (send* action method args) nil))
+   (case arm
+     (:arms
+       (mapcar #'(lambda (a)
+                   (send self :send-gripper-action-method a actions method))
+               (send self :arm2arms arm)))
+     (t
+       (let ((action (gethash (send self :expand-arm-alias arm) actions)))
+         (if action (send* action method args) nil))))
    )
   (:gripper-action-postprocess
    (arm actions wait)
@@ -274,7 +280,8 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka
        (dolist (a arms)
          (setq wait-res
                (and (send self :send-gripper-action-method a actions :wait-for-result)
-                    wait-res))))
+                    wait-res)))
+       (send self :spin-once))
      (if (not wait-res)
        (progn
          (setq wait-res t)
@@ -288,13 +295,7 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka
                (setq wait-res nil))))
          (if (not wait-res)
            (return-from :gripper-action-postprocess nil))))
-     (case arm
-       (:arms
-         (mapcar #'(lambda (a)
-                     (send self :send-gripper-action-method a actions :get-result))
-                 arms))
-       (t
-         (send self :send-gripper-action-method arm actions :get-result)))
+     (send self :send-gripper-action-method arm actions :get-result)
      ))
   (:gripper-method-helper
    (action-sender actions arm wait)
@@ -315,10 +316,16 @@ Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html
    (send self :gripper-method-helper
          :send-gripper-stop-action gripper-stop-actions arm wait)
    )
-  (:get-stop-gripper-result
+  (:get-stop-gripper-status
    (arm)
+   "Return status of :stop-gripper (`status` of `actionlib_msgs::GoalStatus`)"
+   (send self :spin-once)
+   (send self :send-gripper-action-method arm gripper-stop-actions :get-state)
+   )
+  (:get-stop-gripper-result
+   (arm &key (wait t))
    "Return result of :stop-gripper (`franka_gripper::StopActionResult`)"
-   (send self :gripper-action-postprocess arm gripper-stop-actions t)
+   (send self :gripper-action-postprocess arm gripper-stop-actions wait)
    )
   (:homing-gripper
    (arm &key (wait nil))
@@ -330,10 +337,16 @@ Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.ht
      (send self :gripper-method-helper
            :send-gripper-homing-action gripper-homing-actions arm wait)
      ))
-  (:get-homing-gripper-result
+  (:get-homing-gripper-status
    (arm)
+   "Return status of :homing-gripper (`status` of `actionlib_msgs::GoalStatus`)"
+   (send self :spin-once)
+   (send self :send-gripper-action-method arm gripper-homing-actions :get-state)
+   )
+  (:get-homing-gripper-result
+   (arm &key (wait t))
    "Return result of :homing-gripper (`franka_gripper::HomingActionResult`)"
-   (send self :gripper-action-postprocess arm gripper-homing-actions t)
+   (send self :gripper-action-postprocess arm gripper-homing-actions wait)
    )
   (:gripper
    (&rest args)
@@ -395,20 +408,31 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm
            :send-gripper-grasp-action gripper-grasp-actions arm wait width tm
            effort :inner inner :outer outer)
      ))
-  (:get-start-grasp-result
+  (:get-start-grasp-status
    (arm)
+   "Return status of :start-grasp (`status` of `actionlib_msgs::GoalStatus`)"
+   (send self :spin-once)
+   (send self :send-gripper-action-method arm gripper-grasp-actions :get-state)
+   )
+  (:get-start-grasp-result
+   (arm &key (wait t))
    "Return result of :start-grasp (`franka_gripper::GraspActionResult`)"
-   (send self :gripper-action-postprocess arm gripper-grasp-actions t)
+   (send self :gripper-action-postprocess arm gripper-grasp-actions wait)
    )
   (:stop-grasp
    (arm &key (wait nil) (width 0.08))
    "Open the gripper to the target `width` [m]"
    (send self :move-gripper arm width :tm 500 :wait wait)
    )
-  (:get-stop-grasp-result
+  (:get-stop-grasp-status
    (arm)
-   "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)"
-   (send self :get-move-gripper-result arm)
+   "Return status of :stop-grasp (`status` of `actionlib_msgs::GoalStatus`)"
+   (send self :get-move-gripper-status arm)
+   )
+  (:get-stop-grasp-result
+   (arm &key (wait t))
+   "Return result of :stop-grasp (`franka_gripper::MoveActionResult`)"
+   (send self :get-move-gripper-result arm :wait wait)
    )
   (:move-gripper
    (arm width &key (tm 500) (wait nil))
@@ -420,10 +444,16 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html
      (send self :gripper-method-with-width-helper
            :send-gripper-move-action gripper-move-actions arm wait width tm)
      ))
-  (:get-move-gripper-result
+  (:get-move-gripper-status
    (arm)
+   "Return status of :move-gripper (`status` of `actionlib_msgs::GoalStatus`)"
+   (send self :spin-once)
+   (send self :send-gripper-action-method arm gripper-move-actions :get-state)
+   )
+  (:get-move-gripper-result
+   (arm &key (wait t))
    "Return result of :move-gripper (`franka_gripper::MoveActionResult`)"
-   (send self :gripper-action-postprocess arm gripper-move-actions t)
+   (send self :gripper-action-postprocess arm gripper-move-actions wait)
    )
   )
 

jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro

@@ -109,17 +109,17 @@
   <link name="$(arg arm_id_1)_end_effector" >
   </link>
   <joint name="$(arg arm_id_1)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_1)_hand_tcp"/>
+    <parent link="$(arg arm_id_1)_hand"/>
     <child link="$(arg arm_id_1)_end_effector"/>
-    <origin xyz="0 0 0" rpy="0 -1.570796327 0"/>
+    <origin xyz="0 0 0.1034" rpy="0 -1.570796327 0"/>
   </joint>
 
   <link name="$(arg arm_id_2)_end_effector" >
   </link>
   <joint name="$(arg arm_id_2)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_2)_hand_tcp"/>
+    <parent link="$(arg arm_id_2)_hand"/>
     <child link="$(arg arm_id_2)_end_effector"/>
-    <origin xyz="0 0 0" rpy="0 -1.570796327 0"/>
+    <origin xyz="0 0 0.1034" rpy="0 -1.570796327 0"/>
   </joint>
 
 </robot>

jsk_panda_robot/panda_eus/models/dual_panda.yaml

@@ -20,12 +20,12 @@ larm:
 #  - pan_tilt_JOINT1 : head-neck-p
 
 rarm-end-coords:
-  parent: rarm_hand_tcp
-  translate: [0, 0, 0]
+  parent: rarm_hand  # If rarm_hand_tcp is used to delete the following translation, (send *dual_panda* :rarm :gripper :joint-list) does not include finger joints
+  translate: [0, 0, 0.1034]  # https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8
   rotate : [0, -1, 0, 90]
 larm-end-coords:
-  parent: larm_hand_tcp
-  translate: [0, 0, 0]
+  parent: larm_hand  # If larm_hand_tcp is used to delete the following translation, (send *dual_panda* :larm :gripper :joint-list) does not include finger joints
+  translate: [0, 0, 0.1034]  # https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8
   rotate : [0, -1, 0, 90]
 
 angle-vector:

jsk_panda_robot/panda_eus/models/panda.yaml

@@ -8,8 +8,8 @@ rarm:
   - panda_joint7 : rarm-wrist-y
 
 rarm-end-coords:
-  parent: panda_hand_tcp
-  translate: [0, 0, 0]
+  parent: panda_hand  # If panda_hand_tcp is used to delete the following translation, (send *panda* :rarm :gripper :joint-list) does not include finger joints
+  translate: [0, 0, 0.1034]  # https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8
   rotate : [0, -1, 0, 90]
 
 angle-vector: