From 0a345011835926528255ce644997f6b627e924d3 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 16 Oct 2023 15:23:57 +0900 Subject: [PATCH 1/4] [panda_eus] Make :gripper method work --- jsk_panda_robot/panda_eus/models/dual_panda.yaml | 8 ++++---- jsk_panda_robot/panda_eus/models/panda.yaml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml index 351154873a..14c48cfbfe 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml +++ b/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: diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml index 08b55ee3f2..73b9e3f980 100644 --- a/jsk_panda_robot/panda_eus/models/panda.yaml +++ b/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: From 72a2bc66edbb3c0a469d78d41fee5d1f66bb3497 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 16 Oct 2023 23:13:55 +0900 Subject: [PATCH 2/4] [panda_eus] Make URDF follow euslisp model --- jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro index f47eb6c525..eac361f09f 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro +++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro @@ -109,17 +109,17 @@ - + - + - + - + From 141b6cf8478d30139b01162dbe8f3196c6006b15 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 17 Oct 2023 18:28:44 +0900 Subject: [PATCH 3/4] [panda_eus] Enable to get gripper method result without waiting --- .../euslisp/franka-common-interface.l | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 69f90bf8d9..7ffb3a675c 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -274,7 +274,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) @@ -316,9 +317,9 @@ Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html :send-gripper-stop-action gripper-stop-actions arm wait) ) (:get-stop-gripper-result - (arm) + (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)) @@ -331,9 +332,9 @@ Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.ht :send-gripper-homing-action gripper-homing-actions arm wait) )) (:get-homing-gripper-result - (arm) + (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) @@ -396,9 +397,9 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm effort :inner inner :outer outer) )) (:get-start-grasp-result - (arm) + (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)) @@ -406,9 +407,9 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm (send self :move-gripper arm width :tm 500 :wait wait) ) (:get-stop-grasp-result - (arm) + (arm &key (wait t)) "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)" - (send self :get-move-gripper-result arm) + (send self :get-move-gripper-result arm :wait wait) ) (:move-gripper (arm width &key (tm 500) (wait nil)) @@ -421,9 +422,9 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html :send-gripper-move-action gripper-move-actions arm wait width tm) )) (:get-move-gripper-result - (arm) + (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) ) ) From f8d3cc4106d872ef3e9d7b32250c7e407d6f7a41 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 17 Oct 2023 19:10:56 +0900 Subject: [PATCH 4/4] [panda_eus] Enable to get status of gripper actions --- .../euslisp/franka-common-interface.l | 49 +++++++++++++++---- 1 file changed, 39 insertions(+), 10 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 7ffb3a675c..f7251cf696 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/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) @@ -289,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) @@ -316,6 +316,12 @@ 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-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`)" @@ -331,6 +337,12 @@ 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-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`)" @@ -396,6 +408,12 @@ 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-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`)" @@ -406,9 +424,14 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm "Open the gripper to the target `width` [m]" (send self :move-gripper arm width :tm 500 :wait wait) ) + (:get-stop-grasp-status + (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-gripper (`franka_gripper::MoveActionResult`)" + "Return result of :stop-grasp (`franka_gripper::MoveActionResult`)" (send self :get-move-gripper-result arm :wait wait) ) (:move-gripper @@ -421,6 +444,12 @@ 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-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`)"