From a63b5e355ef07d98f576aeb55c57e23c120795d0 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Mon, 30 Oct 2023 18:00:47 +0900 Subject: [PATCH] add single panda (#1799) * add single panda * update from latest franka_control.launch * add comments * remove unused config * remove gazebo descriptions * move code for panda.l and modify messages * change name * change to include franka_control.launch * change larm to rarm * panda.urdf.xacro is unnecessary * [panda_eus] Add panda.urdf and panda.l to .gitignore * [panda_eus] Extract common part of panda-interface.l and dual_panda-interface.l as panda-common-interface.l * [jsk_panda_robot] Add single panda to README * [jsk_panda_robot] Add notice about end-coords, reset-pose, and reset-manip-pose * [panda_eus] Improve error message from :set-joint-pd-gain * enable access to gripper-grasp-action * add missing bracket * Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from their return values * Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from separated methods * Consider compatibility with FR3 (Franka Research 3) * Remove unnecessary method * extract action from hash * Revert "extract action from hash" This reverts commit 0066eb7ea240d2471c1f17d89b9bb000225caed9. * Make errors from :get-*-result readable when * is not called * Force to call all :wait-for-result and postprocess even if some of them fail * [panda_eus] Change dual_panda's end-coords to the same as single panda * [panda_eus] Move end-coords of single panda and dual_panda to the center of the fingers * [panda_eus] Make :gripper method work * [panda_eus] Make URDF follow euslisp model * [panda_eus] Enable to get gripper method result without waiting * [panda_eus] Enable to get status of gripper actions --------- Co-authored-by: Shun Hasegawa --- jsk_panda_robot/README.md | 33 +- .../jsk_panda_startup/launch/franka.launch | 19 + .../jsk_panda_startup/launch/panda.launch | 17 + jsk_panda_robot/panda_eus/.gitignore | 2 + jsk_panda_robot/panda_eus/CMakeLists.txt | 29 +- .../panda_eus/euslisp/dual_panda-interface.l | 286 ++--------- .../panda_eus/euslisp/dual_panda-utils.l | 29 ++ .../euslisp/franka-common-interface.l | 465 ++++++++++++++++++ .../panda_eus/euslisp/panda-interface.l | 34 ++ .../panda_eus/euslisp/panda-utils.l | 26 + .../panda_eus/models/dual_panda.urdf.xacro | 8 +- .../panda_eus/models/dual_panda.yaml | 12 +- jsk_panda_robot/panda_eus/models/panda.yaml | 19 + jsk_panda_robot/panda_eus/package.xml | 1 + 14 files changed, 710 insertions(+), 270 deletions(-) create mode 100644 jsk_panda_robot/jsk_panda_startup/launch/franka.launch create mode 100644 jsk_panda_robot/jsk_panda_startup/launch/panda.launch create mode 100644 jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/panda-interface.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/panda-utils.l create mode 100644 jsk_panda_robot/panda_eus/models/panda.yaml diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index 7e41b7e428..d035983bce 100644 --- a/jsk_panda_robot/README.md +++ b/jsk_panda_robot/README.md @@ -49,6 +49,35 @@ ``` +## Running single Panda +### Boot robot +1. Please turn on the controller box and unlock joints by accessing desk. +### Via roseus +1. Start controller on controller PC: + ```bash + ssh leus@dual-panda1.jsk.imi.i.u-tokyo.ac.jp # Or ssh leus@dual-panda2.jsk.imi.i.u-tokyo.ac.jp + roslaunch jsk_panda_startup panda.launch robot_ip:= + ``` + +2. Controlling single Panda via roseus: + 1. Setting up network: + ```bash + rossetmaster dual-panda1.jsk.imi.i.u-tokyo.ac.jp # Or rossetmaster dual-panda2.jsk.imi.i.u-tokyo.ac.jp + rossetip + ``` + 2. Execute following script in roseus: + ```lisp + (load "package://panda_eus/euslisp/panda-interface.l") + (panda-init) + (send *robot* :angle-vector (send *robot* :reset-pose)) + (when (send *ri* :check-error) + (send *ri* :recover-error)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + ``` + - Notice + - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + + ## Running Dual-Panda ### Boot robot 1. Please turn on the controller box and unlock joints by accessing desk. @@ -76,7 +105,9 @@ (send *ri* :recover-error)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) ``` - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + - Notice + - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + - `dual_panda`'s `reset-pose` and `reset-manip-pose` are different from single `panda`'s ones for historical reasons. #### Record/play rosbag ```bash roslaunch jsk_panda_startup dual_panda1_record.launch # Or roslaunch jsk_panda_startup dual_panda2_record.launch diff --git a/jsk_panda_robot/jsk_panda_startup/launch/franka.launch b/jsk_panda_robot/jsk_panda_startup/launch/franka.launch new file mode 100644 index 0000000000..689d57318f --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/launch/franka.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch new file mode 100644 index 0000000000..be2638609b --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/jsk_panda_robot/panda_eus/.gitignore b/jsk_panda_robot/panda_eus/.gitignore index da9165908a..97a6d36fcf 100644 --- a/jsk_panda_robot/panda_eus/.gitignore +++ b/jsk_panda_robot/panda_eus/.gitignore @@ -1,2 +1,4 @@ models/dual_panda.l models/dual_panda.urdf +models/panda.l +models/panda.urdf diff --git a/jsk_panda_robot/panda_eus/CMakeLists.txt b/jsk_panda_robot/panda_eus/CMakeLists.txt index 4fea19581f..6727ab33cd 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/jsk_panda_robot/panda_eus/CMakeLists.txt @@ -10,9 +10,6 @@ find_package(franka_description) # Just in case when description is not release catkin_package() -### -### dual_panda.l generation -### set(_franka_description_min_ver "0.10.0") set(_xacro_min_ver "1.13.14") if(franka_description_FOUND @@ -25,6 +22,10 @@ if(franka_description_FOUND # xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14. # PR introducing xacro.load_yaml: https://github.com/ros/xacro/pull/283 # Related issue: https://github.com/ros/xacro/issues/298 + + ### + ### dual_panda.l generation + ### add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/dual_panda.l COMMAND rosrun euscollada collada2eus -I dual_panda.urdf -C dual_panda.yaml -O dual_panda.l WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models @@ -34,9 +35,27 @@ if(franka_description_FOUND WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.urdf.xacro) - add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l) + add_custom_target(generate_dual_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l) + + ### + ### panda.l generation + ### + set(_panda_xacro ${franka_description_SOURCE_PREFIX}/robots/panda/panda.urdf.xacro) # franka_description is installed from source + if(NOT EXISTS ${_panda_xacro}) + set(_panda_xacro ${franka_description_PREFIX}/share/franka_description/robots/panda/panda.urdf.xacro) # franka_description is installed from apt + endif() + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l + COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models + DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf + COMMAND rosrun xacro xacro --inorder ${_panda_xacro} hand:=true > ${PROJECT_SOURCE_DIR}/models/panda.urdf + DEPENDS ${_panda_xacro}) + + add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l) + else() - message(WARNING "Dependency is not met, so skip generating dual_panda.l") + message(WARNING "Dependency is not met, so skip generating panda.l and dual_panda.l") message(WARNING "franka_description version: ${franka_description_VERSION}, must be >= ${_franka_description_min_ver}") message(WARNING "xacro version: ${xacro_VERSION}, must be >= ${_xacro_min_ver}") endif() diff --git a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l index a8620e0b85..338e387357 100644 --- a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l @@ -1,264 +1,42 @@ -(require :robot-interface "package://pr2eus/robot-interface.l") -(require :dual_panda "package://panda_eus/models/dual_panda.l") - -(ros::roseus-add-msgs "franka_msgs") -(ros::roseus-add-msgs "franka_gripper") +(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l") +(require :dual_panda-utils "package://panda_eus/euslisp/dual_panda-utils.l") (defclass dual_panda-robot-interface - :super robot-interface - :slots (error-recovery-act - r-error l-error - gripper-grasp-actions gripper-move-actions gripper-homing-actions gripper-stop-actions - ) - ) + :super franka-common-interface + :slots ()) + (defmethod dual_panda-robot-interface (:init - (&rest args) - (prog1 - (send-super* :init :robot dual_panda-robot - :joint-states-topic "dual_panda/joint_states" - args) - ;; for error recovery - (ros::create-nodehandle "error_group") - (ros::subscribe "/dual_panda/rarm/has_error" std_msgs::Bool - #'send self :callback-rarm-error 1 :groupname "error_group") - (ros::subscribe "/dual_panda/larm/has_error" std_msgs::Bool - #'send self :callback-larm-error 1 :groupname "error_group") - (setq error-recovery-act (instance ros::simple-action-client :init - "/dual_panda/error_recovery" - franka_msgs::ErrorRecoveryAction - :groupname "error_group" - )) - ;; actions for gripper - (setq gripper-grasp-actions (make-hash-table)) - (setq gripper-homing-actions (make-hash-table)) - (setq gripper-move-actions (make-hash-table)) - (setq gripper-stop-actions (make-hash-table)) - (dolist (arm (list :rarm :larm)) - (sethash arm gripper-grasp-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/grasp" - (string-downcase (string arm))) - franka_gripper::GraspAction)) - (sethash arm gripper-homing-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/homing" - (string-downcase (string arm))) - franka_gripper::HomingAction)) - (sethash arm gripper-move-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/move" - (string-downcase (string arm))) - franka_gripper::MoveAction)) - (sethash arm gripper-stop-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/stop" - (string-downcase (string arm))) - franka_gripper::StopAction))) - )) + (&rest args) + (let ((all-arms (list :rarm :larm))) + (send-super* :init :robot dual_panda-robot + :joint-states-topic "dual_panda/joint_states" + :all-arms all-arms + :all-arm-aliases (mapcar #'(lambda (arm) nil) all-arms) + :error-topics (mapcar + #'(lambda (arm) + (format nil "/dual_panda/~a/has_error" + (string-downcase (string arm)))) + all-arms) + :error-topic-types (mapcar #'(lambda (arm) std_msgs::Bool) all-arms) + :error-recovery-action "/dual_panda/error_recovery" + :gripper-action-prefixes (mapcar + #'(lambda (arm) + (format nil "/dual_panda/~a" + (string-downcase (string arm)))) + all-arms) + args))) (:default-controller - () - (list + () (list - (cons :controller-action "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory") - (cons :controller-state "/dual_panda/dual_panda_effort_joint_trajectory_controller/state") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (send-all (send robot :joint-list) :name)) - ))) - (:set-joint-pd-gain - (joint-name pgain dgain) - "Set P gain and D gain of each joint" - (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) - (send req :config :doubles - (list (instance dynamic_reconfigure::DoubleParameter :init - :name "p" :value pgain) - (instance dynamic_reconfigure::DoubleParameter :init - :name "d" :value dgain))) - (ros::service-call - (format nil "/dual_panda/dual_panda_effort_joint_trajectory_controller/gains/~A/set_parameters" joint-name) - req) - )) - (:set-all-joint-pd-gain - (pgain dgain) - "Set P gain and D gain of all joints" - (dolist (j (send robot :joint-list)) - (send self :set-joint-pd-gain (send j :name) pgain dgain)) - ) - (:check-error - () - "Check if the robot has error. -If this method returns T, you must call :recover-error to move the robot. -" - (ros::spin-once "error_group") - (or r-error l-error) - ) - (:callback-rarm-error - (msg) - (setq r-error (send msg :data)) - ) - (:callback-larm-error - (msg) - (setq l-error (send msg :data)) - ) - (:wait-recover-error () (send error-recovery-act :wait-for-result)) - (:recover-error - (&key (wait t)) - "Recover from errors and reflexes. -Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-control -" - (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send error-recovery-act :send-goal goal) - (if wait (send self :wait-recover-error)) - )) - ;; gripper action for real-controller - (:send-gripper-grasp-action - (arm width speed force &key (wait t) (inner 0.005) (outer 0.07)) - (let ((goal (instance franka_gripper::GraspActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - (send goal :goal :force force) ;; [N] - (send goal :goal :epsilon :inner inner) ;; [m] - (send goal :goal :epsilon :outer outer) ;; [m] - ;; - (send (gethash arm gripper-grasp-actions) :send-goal goal) - (if wait (send (gethash arm gripper-grasp-actions) :wait-for-result)) - )) - (:send-gripper-homing-action - (arm &key (wait t)) - (let ((goal (instance franka_gripper::HomingActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send (gethash arm gripper-homing-actions) :send-goal goal) - (if wait (send (gethash arm gripper-homing-actions) :wait-for-result)) - )) - (:send-gripper-move-action - (arm width speed &key (wait t)) - (let ((goal (instance franka_gripper::MoveActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - ;; - (send (gethash arm gripper-move-actions) :send-goal goal) - (if wait (send (gethash arm gripper-move-actions) :wait-for-result)) - )) - (:send-gripper-stop-action - (arm &key (wait t)) - (let ((goal (instance franka_gripper::StopActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send (gethash arm gripper-stop-actions) :send-goal goal) - (if wait (send (gethash arm gripper-stop-actions) :wait-for-result)) - )) - (:arm2arms - (arm) - (case arm - ((:rarm :larm) (list arm)) - (:arms (list :rarm :larm)) - )) - (:gripper-method-helper - (action-sender actions arm wait) - (let ((arms (send self :arm2arms arm))) - (when arms - (dolist (a arms) - (send self action-sender a :wait nil)) - (if wait - (mapcar #'(lambda (a) (send (gethash a actions) :wait-for-result)) - arms)) - ))) - (:stop-gripper - (arm &key (wait nil)) - "Abort a running gripper action. This can be used to stop applying forces after grasping. -Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-helper - :send-gripper-stop-action gripper-stop-actions arm wait) - ) - (:homing-gripper - (arm &key (wait nil)) - "Home the gripper and update the maximum width given the mounted fingers (i.e., calibrate & initialize the gripper). -Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-helper - :send-gripper-homing-action gripper-homing-actions arm wait) - ) - (:gripper - (&rest args) - "Get information of gripper -Arguments: -- arm : :rarm, :larm, or :arms -- type : :position ([m]) -Example: (send self :gripper :rarm :position) => 0.00 -" - (when (eq (car args) :arms) - (return-from :gripper - (mapcar #'(lambda (x) - (send self :gripper x (cadr args))) - '(:larm :rarm)))) - (unless (memq (car args) '(:larm :rarm)) - (error "you must specify arm ~A from ~A" (car args) '(:larm :rarm)) - (return-from :gripper nil)) - (send self :update-robot-state) - (case (cadr args) - (:position (* 0.001 (v. (send robot (car args) :gripper :angle-vector) - #f(0 0 0 0 1 0 0 1)))) - )) - (:gripper-method-with-width-helper - (action-sender actions arm wait width tm &rest args) - (let ((arms (send self :arm2arms arm))) - (when arms - (dolist (a arms) - (send* self action-sender a width - (/ (abs (- (send self :gripper a :position) width)) - (/ tm 1000.0)) - (append args (list :wait nil)))) - (if wait - (mapcar #'(lambda (a) (send (gethash a actions) :wait-for-result)) - arms)) - ))) - (:start-grasp - (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) - "Try to grasp at the desired `width` with the desired `effort` while closing with the desired speed calculated from `tm`. -Arguments: -- arm : :rarm, :larm, or :arms -- width : target distance between the fingers [m] -- effort : target effort [N] -- tm : time to target [ms]. This will be converted to the movement speed -- wait : if this argument is T, this method waits until the movement finishes -- inner : lower admissible error of width. If this is violated, the gripper stops applying forces -- outer : upper admissible error of width. If this is violated, the gripper stops applying forces - Details: https://github.com/ykawamura96/jsk_robot/pull/1#issuecomment-860324988 -Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-with-width-helper - :send-gripper-grasp-action gripper-grasp-actions arm wait width tm - effort :inner inner :outer outer) - ) - (:stop-grasp - (arm &key (wait nil) (width 0.08)) - "Open the gripper to the target `width` [m]" - (unless (memq arm '(:larm :rarm :arms)) - (error "you must specify arm ~A from ~A" (car args) '(:larm :rarm :arms)) - (return-from :stop-grasp nil)) - (send self :move-gripper arm width :tm 500 :wait wait) - ) - (:move-gripper - (arm width &key (tm 500) (wait nil)) - "Move the gripper to the target `width` [m] while closing with the desired speed calculated from `tm` [ms]. -Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-with-width-helper - :send-gripper-move-action gripper-move-actions arm wait width tm) - ) - ) + (list + (cons :controller-action "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "/dual_panda/dual_panda_effort_joint_trajectory_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name)))))) (defun dual_panda-init () (setq *ri* (instance dual_panda-robot-interface :init)) - (setq *robot* (dual_panda)) - ) + (setq *robot* (dual_panda))) -#| -You can check current gains with rqt_reconfigure. -Be careful changing them with :set-joint-pd-gain or :set-all-joint-pd-gain -|# +(provide :dual_panda-interface) diff --git a/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l b/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l new file mode 100644 index 0000000000..4f8b9a1ab4 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l @@ -0,0 +1,29 @@ +(require :dual_panda "package://panda_eus/models/dual_panda.l") + +(defmethod dual_panda-robot + (:start-grasp + (arm &rest args &key (width 0.0) &allow-other-keys) + (send* self :move-gripper arm width args)) + (:stop-grasp + (arm &rest args &key (width 0.08) &allow-other-keys) + (send* self :move-gripper arm width args)) + (:move-gripper + (arm width &rest args) + "Move the gripper to the target `width`. +Arguments: +- arm : :rarm, :larm, or :arms +- width : target distance between the fingers [m] +" + (let ((arms (case arm + ((:rarm :larm) (list arm)) + (:arms (list :rarm :larm))))) + (dolist (am arms) + (send-all + (remove nil (mapcar + #'(lambda (jt) + (if (= (send jt :min-angle) (send jt :max-angle)) nil jt)) + (send self am :gripper :joint-list))) + ;; Get joint list of gripper excluding fixed joints + :joint-angle (* (/ width 2.0) 1000)))))) + +(provide :dual_panda-utils) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l new file mode 100644 index 0000000000..f7251cf696 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -0,0 +1,465 @@ +(require :robot-interface "package://pr2eus/robot-interface.l") + +(ros::roseus-add-msgs "actionlib_msgs") +(ros::roseus-add-msgs "franka_msgs") +(ros::roseus-add-msgs "franka_gripper") + +(defclass franka-common-interface + :super robot-interface + :slots (all-arms all-arm-aliases + arm-error-p error-recovery-act + gripper-grasp-actions gripper-move-actions gripper-homing-actions gripper-stop-actions + ) + ) +(defmethod franka-common-interface + (:init + (&rest args &key (robot nil) + (joint-states-topic nil) + ((:all-arms aa) nil) + ((:all-arm-aliases aaa) nil) + (error-topics nil) + (error-topic-types nil) + (error-recovery-action nil) + (gripper-action-prefixes nil) + &allow-other-keys) + "Create robot interface for franka robot +Arguments specific to franka robot: +- all-arms : names of all arms (e.g., (list :rarm)) +- all-arm-aliases : aliases of all arms (e.g., (list :arm)) +- error-topics : topics used for getting error states of arms (e.g., (list \"/franka_state_controller/franka_states\")) +- error-topic-types : message types of error-topics (e.g., (list franka_msgs::FrankaState)) +- error-recovery-action : action for error recovery (e.g., \"/franka_control/error_recovery\") +- gripper-action-prefixes : prefixes added to gripper action names (e.g., (list \"\")) +" + (setq all-arms aa) + (setq all-arm-aliases aaa) + (unless (= (length all-arms) (length all-arm-aliases) + (length error-topics) (length error-topic-types) + (length gripper-action-prefixes)) + (error + "all-arms, all-arm-aliases, error-topics, error-topic-types, and gripper-action-prefixes must have the same length") + (return-from :init nil)) + (prog1 + (send-super* :init :robot robot + :joint-states-topic joint-states-topic + args) + ;; for error recovery + (ros::create-nodehandle "error_group") + (setq arm-error-p (make-hash-table)) + (dotimes (i (length all-arms)) + (let ((arm (elt all-arms i)) (etype (elt error-topic-types i))) + (sethash arm arm-error-p nil) + (cond + ((eq etype std_msgs::Bool) + ;; franka_combined_control.launch (used in dual_panda) publishes has_error topic (800Hz) + (ros::subscribe + (elt error-topics i) std_msgs::Bool + #'send self :callback-error arm + 1 :groupname "error_group")) + ((eq etype franka_msgs::FrankaState) + ;; franka_control.launch (used in single panda) does not publish has_error topic. + ;; We use franka_states topic instead, but its frequency is lower (30Hz). + ;; We do not use franka_combined_control.launch in single panda for backward compatibility. + ;; position_joint_trajectory_controller, which we used in single panda so far, cannot be used on franka_combined_control.launch. + ;; This is because + ;; https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_hw/src/franka_hw.cpp#L528-L529 + ;; does not exist in + ;; https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_hw/src/franka_combinable_hw.cpp#L26-L39. + (ros::subscribe + (elt error-topics i) franka_msgs::FrankaState + #'send self :callback-franka-state arm + 1 :groupname "error_group")) + (t + (ros::ros-error "error-topic-types includes unsupported type") + (ros::ros-error ":check-error will return meaningless value"))))) + (setq error-recovery-act (instance ros::simple-action-client :init + error-recovery-action + franka_msgs::ErrorRecoveryAction + :groupname "error_group" + )) + ;; actions for gripper + (setq gripper-grasp-actions (make-hash-table)) + (setq gripper-homing-actions (make-hash-table)) + (setq gripper-move-actions (make-hash-table)) + (setq gripper-stop-actions (make-hash-table)) + (dotimes (i (length all-arms)) + (let ((arm (elt all-arms i)) (prefix (elt gripper-action-prefixes i))) + (sethash arm gripper-grasp-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/grasp" prefix) + franka_gripper::GraspAction + :groupname groupname)) + (sethash arm gripper-homing-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/homing" prefix) + franka_gripper::HomingAction + :groupname groupname)) + (sethash arm gripper-move-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/move" prefix) + franka_gripper::MoveAction + :groupname groupname)) + (sethash arm gripper-stop-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/stop"prefix) + franka_gripper::StopAction + :groupname groupname)))) + )) + (:set-joint-pd-gain + (joint-name pgain dgain &key (type :default-controller)) + "Set P gain and D gain of each joint. +This method works only when `type` includes a controller coming from effort_controllers/JointTrajectoryController and including `joint-name`. +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":set-joint-pd-gain is not implemented on simulation mode, always returns t") + (return-from :set-joint-pd-gain t))) + (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init)) (set-p nil)) + (send req :config :doubles + (list (instance dynamic_reconfigure::DoubleParameter :init + :name "p" :value pgain) + (instance dynamic_reconfigure::DoubleParameter :init + :name "d" :value dgain))) + (dolist (fjt-name (mapcar #'(lambda (joint-param) + (cdr (assoc :controller-action joint-param))) + (send self type))) + (let* ((cname (subseq fjt-name 0 (- (length fjt-name) + (position #\/ (reverse fjt-name)) + 1))) + (srv-name (format nil "~a/gains/~a/set_parameters" cname joint-name))) + ;; If fjt-name is "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory", + ;; cname will be "/dual_panda/dual_panda_effort_joint_trajectory_controller" + (if (ros::wait-for-service srv-name 0) + (progn + (ros::service-call srv-name req) + (setq set-p t))))) + (if (not set-p) + (progn + (ros::ros-error "Setting PD gains of ~a via ~a failed" joint-name type) + (ros::ros-error + "~a may not include a controller coming from effort_controllers/JointTrajectoryController and including ~a" + type joint-name))) + )) + (:set-all-joint-pd-gain + (pgain dgain &key (type :default-controller)) + "Set P gain and D gain of all joints. +This method fully works only when each joint is included by a controller in `type` coming from effort_controllers/JointTrajectoryController. +" + (dolist (j (send robot :joint-list)) + (send self :set-joint-pd-gain (send j :name) pgain dgain :type type)) + ) + (:check-error + () + "Check if the robot has error. +If this method returns T, you must call :recover-error to move the robot. +" + (ros::spin-once "error_group") + (reduce #'(lambda (x y) (or x y)) + (mapcar #'(lambda (arm) (gethash arm arm-error-p)) all-arms)) + ) + (:callback-error + (arm msg) + (sethash arm arm-error-p (send msg :data)) + ) + (:callback-franka-state + (arm msg) + (sethash arm arm-error-p + (not (= (send msg :robot_mode) + franka_msgs::FrankaState::*ROBOT_MODE_MOVE*))) + ) + (:wait-recover-error + () + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":wait-recover-error is not implemented on simulation mode, always returns t") + (return-from :wait-recover-error t))) + (send error-recovery-act :wait-for-result) + ) + (:recover-error + (&key (wait t)) + "Recover from errors and reflexes. +Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-control +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":recover-error is not implemented on simulation mode, always returns t") + (return-from :recover-error t))) + (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + (send error-recovery-act :send-goal goal) + (if wait (send self :wait-recover-error)) + )) + (:expand-arm-alias + (arm-alias) + (let ((expanded + (if arm-alias + (let ((i (position arm-alias all-arm-aliases))) + (if i + (elt all-arms i) + arm-alias)) + nil))) + (if (memq expanded all-arms) + expanded + (progn + (error "arm ~a does not exist" arm-alias) + nil)) + )) + ;; gripper action for real-controller + (:send-gripper-grasp-action + (arm width speed force &key (wait t) (inner 0.005) (outer 0.07)) + (let ((goal (instance franka_gripper::GraspActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-grasp-actions))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + (send goal :goal :force force) ;; [N] + (send goal :goal :epsilon :inner inner) ;; [m] + (send goal :goal :epsilon :outer outer) ;; [m] + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-homing-action + (arm &key (wait t)) + (let ((goal (instance franka_gripper::HomingActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-homing-actions))) + (send goal :header :stamp (ros::time-now)) + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-move-action + (arm width speed &key (wait t)) + (let ((goal (instance franka_gripper::MoveActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-move-actions))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-stop-action + (arm &key (wait t)) + (let ((goal (instance franka_gripper::StopActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-stop-actions))) + (send goal :header :stamp (ros::time-now)) + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:arm2arms + (arm) + (case arm + (:arms all-arms) + (t (list arm)) + )) + (:send-gripper-action-method + (arm actions method &rest args) + (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) + (let ((arms (send self :arm2arms arm)) (wait-res t)) + (if wait + (dolist (a arms) + (setq wait-res + (and (send self :send-gripper-action-method a actions :wait-for-result) + wait-res))) + (send self :spin-once)) + (if (not wait-res) + (progn + (setq wait-res t) + (dolist (a arms) + (if (eq (send self :send-gripper-action-method a actions :get-state) + actionlib_msgs::GoalStatus::*pending*) + (progn + (ros::ros-error + "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" + (send self :send-gripper-action-method a actions :name)) + (setq wait-res nil)))) + (if (not wait-res) + (return-from :gripper-action-postprocess nil)))) + (send self :send-gripper-action-method arm actions :get-result) + )) + (:gripper-method-helper + (action-sender actions arm wait) + (let ((arms (send self :arm2arms arm))) + (dolist (a arms) + (send self action-sender a :wait nil)) + (send self :gripper-action-postprocess arm actions wait) + )) + (:stop-gripper + (arm &key (wait nil)) + "Abort a running gripper action. This can be used to stop applying forces after grasping. +Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":stop-gripper is not implemented on simulation mode, always returns t") + (return-from :stop-gripper t))) + (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`)" + (send self :gripper-action-postprocess arm gripper-stop-actions wait) + ) + (:homing-gripper + (arm &key (wait nil)) + "Home the gripper and update the maximum width given the mounted fingers (i.e., calibrate & initialize the gripper). +Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send self :stop-grasp arm :wait wait) + (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`)" + (send self :gripper-action-postprocess arm gripper-homing-actions wait) + ) + (:gripper + (&rest args) + "Get information of gripper +Arguments: +- arm : :arms or any element of all-arms and all-arm-aliases (e.g., :rarm) +- type : :position ([m]) +Example: (send self :gripper :rarm :position) => 0.00 +" + (if (eq (car args) :arms) + (return-from :gripper + (mapcar #'(lambda (x) + (send self :gripper x (cadr args))) + all-arms))) + (let ((arm (send self :expand-arm-alias (car args)))) + (if arm + (progn + (send self :update-robot-state) + (case (cadr args) + (:position + (* 0.001 + (apply #'+ + (send-all + (remove nil (mapcar #'(lambda (jt) + (if (= (send jt :min-angle) + (send jt :max-angle)) + nil jt)) + (send robot arm :gripper :joint-list))) + ;; Get joint list of gripper excluding fixed joints + :joint-angle)))))))) + ) + (:gripper-method-with-width-helper + (action-sender actions arm wait width tm &rest args) + (let ((arms (send self :arm2arms arm))) + (dolist (a arms) + (send* self action-sender a width + (/ (abs (- (send self :gripper a :position) width)) + (/ tm 1000.0)) + (append args (list :wait nil)))) + (send self :gripper-action-postprocess arm actions wait) + )) + (:start-grasp + (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) + "Try to grasp at the desired `width` with the desired `effort` while closing with the desired speed calculated from `tm`. +Arguments: +- arm : :arms or any element of all-arms and all-arm-aliases (e.g., :rarm) +- width : target distance between the fingers [m] +- effort : target effort [N] +- tm : time to target [ms]. This will be converted to the movement speed +- wait : if this argument is T, this method waits until the movement finishes +- inner : lower admissible error of width. If this is violated, the gripper stops applying forces +- outer : upper admissible error of width. If this is violated, the gripper stops applying forces + Details: https://github.com/ykawamura96/jsk_robot/pull/1#issuecomment-860324988 +Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send robot :start-grasp arm :width width) + (send self :gripper-method-with-width-helper + :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`)" + (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-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-grasp (`franka_gripper::MoveActionResult`)" + (send self :get-move-gripper-result arm :wait wait) + ) + (:move-gripper + (arm width &key (tm 500) (wait nil)) + "Move the gripper to the target `width` [m] while closing with the desired speed calculated from `tm` [ms]. +Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send robot :move-gripper arm width) + (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`)" + (send self :gripper-action-postprocess arm gripper-move-actions wait) + ) + ) + +(provide :franka-common-interface) + +#| +You can check current joint gains with rqt_reconfigure when you use effort_controllers/JointTrajectoryController. +Be careful changing them with :set-joint-pd-gain or :set-all-joint-pd-gain +|# diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l new file mode 100644 index 0000000000..78cb96a842 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -0,0 +1,34 @@ +(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l") +(require :panda-utils "package://panda_eus/euslisp/panda-utils.l") + +(defclass panda-robot-interface + :super franka-common-interface + :slots ()) + +(defmethod panda-robot-interface + (:init + (&rest args) + (send-super* :init :robot panda-robot + :joint-states-topic "joint_states" + :all-arms (list :rarm) + :all-arm-aliases (list :arm) + :error-topics (list "/franka_state_controller/franka_states") + :error-topic-types (list franka_msgs::FrankaState) + :error-recovery-action "/franka_control/error_recovery" + :gripper-action-prefixes (list "") + args)) + (:default-controller + () + (list + (list + (cons :controller-action "/position_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "/position_joint_trajectory_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name))))) + ) + +(defun panda-init () + (setq *ri* (instance panda-robot-interface :init)) + (setq *robot* (panda))) + +(provide :panda-interface) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-utils.l b/jsk_panda_robot/panda_eus/euslisp/panda-utils.l new file mode 100644 index 0000000000..1657664f38 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/panda-utils.l @@ -0,0 +1,26 @@ +(require :panda "package://panda_eus/models/panda.l") + +(defmethod panda-robot + (:arm (&rest args) (send* self :rarm args)) ;; Enable to call (send *panda* :arm :angle-vector) + (:start-grasp + (arm &rest args &key (width 0.0) &allow-other-keys) + (send* self :move-gripper arm width args)) + (:stop-grasp + (arm &rest args &key (width 0.08) &allow-other-keys) + (send* self :move-gripper arm width args)) + (:move-gripper + (arm width &rest args) + "Move the gripper to the target `width`. +Arguments: +- arm : :arm, :rarm, or :arms (only for compatibility with panda-robot-interface) +- width : target distance between the fingers [m] +" + (send-all + (remove nil (mapcar + #'(lambda (jt) + (if (= (send jt :min-angle) (send jt :max-angle)) nil jt)) + (send self :rarm :gripper :joint-list))) + ;; Get joint list of gripper excluding fixed joints + :joint-angle (* (/ width 2.0) 1000)))) + +(provide :panda-utils) 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 d786546287..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 @@ - + - + - + - + diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml index 338af0958f..14c48cfbfe 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml +++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml @@ -20,13 +20,13 @@ larm: # - pan_tilt_JOINT1 : head-neck-p rarm-end-coords: - parent: rarm_link7 - translate: [0, 0, 0.2] - rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + 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_link7 - translate: [0, 0, 0.2] - rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + 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: reset-pose: [ 0.0, 15.0, 0.0, -135.0, 0.0, 150.0, 45.0, diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml new file mode 100644 index 0000000000..73b9e3f980 --- /dev/null +++ b/jsk_panda_robot/panda_eus/models/panda.yaml @@ -0,0 +1,19 @@ +rarm: + - panda_joint1 : rarm-collar-y + - panda_joint2 : rarm-shoulder-p + - panda_joint3 : rarm-shoulder-y + - panda_joint4 : rarm-elbow-p + - panda_joint5 : rarm-wrist-r + - panda_joint6 : rarm-wrist-p + - panda_joint7 : rarm-wrist-y + +rarm-end-coords: + 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: + reset-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0, + 0.0, -75.0 ] + reset-manip-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0, + 0.0, -75.0 ] diff --git a/jsk_panda_robot/panda_eus/package.xml b/jsk_panda_robot/panda_eus/package.xml index 8bc83b05dd..0867097679 100644 --- a/jsk_panda_robot/panda_eus/package.xml +++ b/jsk_panda_robot/panda_eus/package.xml @@ -22,5 +22,6 @@ franka_gripper franka_control pr2eus + actionlib_msgs