diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index 7e41b7e428..eb353bde2b 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 `end-coords`, `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 index d306dfc5d3..be2638609b 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch @@ -7,14 +7,11 @@ - + - - - 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 e29a0217aa..6727ab33cd 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/jsk_panda_robot/panda_eus/CMakeLists.txt @@ -16,7 +16,7 @@ if(franka_description_FOUND AND (NOT ("${franka_description_VERSION}" VERSION_LESS "${_franka_description_min_ver}")) AND (NOT ("${xacro_VERSION}" VERSION_LESS "${_xacro_min_ver}"))) # Why franka_description >= 0.10.0: - # panda.urdf.xacro and dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. + # dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. # See https://github.com/frankaemika/franka_ros/compare/0.9.1...0.10.0 for details. # Why xacro >= 1.13.14: # xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14. @@ -40,14 +40,17 @@ if(franka_description_FOUND ### ### 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.urdf.xacro > panda.urdf - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models - DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro) + 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) 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..818cbb3fae --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -0,0 +1,415 @@ +(require :robot-interface "package://pr2eus/robot-interface.l") + +(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)) + (sethash arm gripper-homing-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/homing" prefix) + franka_gripper::HomingAction)) + (sethash arm gripper-move-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/move" prefix) + franka_gripper::MoveAction)) + (sethash arm gripper-stop-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/stop"prefix) + franka_gripper::StopAction)))) + )) + (: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) + (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))) + (if wait + (dolist (a arms) + (send self :send-gripper-action-method a actions :wait-for-result))) + (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))) + )) + (: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-result + (arm) + "Return result of :stop-gripper (`franka_gripper::StopActionResult`)" + (send self :gripper-action-postprocess arm gripper-stop-actions t) + ) + (: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-result + (arm) + "Return result of :homing-gripper (`franka_gripper::HomingActionResult`)" + (send self :gripper-action-postprocess arm gripper-homing-actions t) + ) + (: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-result + (arm) + "Return result of :start-grasp (`franka_gripper::GraspActionResult`)" + (send self :gripper-action-postprocess arm gripper-grasp-actions t) + ) + (: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 + (arm) + "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)" + (send self :get-move-gripper-result arm) + ) + (: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-result + (arm) + "Return result of :move-gripper (`franka_gripper::MoveActionResult`)" + (send self :gripper-action-postprocess arm gripper-move-actions t) + ) + ) + +(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 index d7f5c5dbb3..78cb96a842 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -1,217 +1,34 @@ -(require :robot-interface "package://pr2eus/robot-interface.l") -(require :franka "package://panda_eus/models/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 :panda-utils "package://panda_eus/euslisp/panda-utils.l") (defclass panda-robot-interface - :super robot-interface - :slots (gripper-action - error-recovery-act error - gripper-grasp-action gripper-move-action gripper-homing-action gripper-stop-action - ) - ) + :super franka-common-interface + :slots ()) + (defmethod panda-robot-interface (:init - (&rest args &key ((:controller-timeout ct) nil)) - (prog1 - (send-super* :init :robot panda-robot - :joint-states-topic "joint_states" - :controller-timeout ct args) - ;; for error recovery - (ros::create-nodehandle "error_group") - (ros::subscribe "/panda/arm/has_error" std_msgs::Bool - #'send self :callback-arm-error 1 :groupname "error_group") - (setq error-recovery-act (instance ros::simple-action-client :init - "/franka_control/error_recovery" - franka_msgs::ErrorRecoveryAction - :groupname "error_group" - )) - ;; actions for gripper - (setq gripper-grasp-action - (instance ros::simple-action-client :init - "/franka_gripper/grasp" - franka_gripper::GraspAction)) - (setq gripper-homing-action - (instance ros::simple-action-client :init - "/franka_gripper/homing" - franka_gripper::HomingAction)) - (setq gripper-move-action - (instance ros::simple-action-client :init - "/franka_gripper/move" - franka_gripper::MoveAction)) - (setq gripper-stop-action - (instance ros::simple-action-client :init - "/franka_gripper/stop" - franka_gripper::StopAction)) - )) + (&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)) - ))) - (:rarm-controller - () - (list - (list - (cons :controller-action "/rarm_controller/follow_joint_trajectory") - (cons :controller-state "/position_joint_trajectory_controller/follow_joint_trajectory") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names - (remove-if #'(lambda (jn) (substringp "finger" jn)) - (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) - (send-all (send robot :rarm :joint-list) :name))))))) - (:hand-controller - () - (list - (list - (cons :controller-action "/hand_controller/follow_joint_trajectory") - (cons :controller-state "/hand_controller/state") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (list "left_finger_joint1" "left_finger_joint2") )))) - (:set-joint-pd-gain - (joint-name pgain dgain) - (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 "/position_joint_trajectory_controller/gains/~A/set_parameters" joint-name) - req) - )) - (:set-all-joint-pd-gain - (pgain dgain) - (dolist (j (send robot :joint-list)) - (send self :set-joint-pd-gain (send j :name) pgain dgain)) - ) - (:check-error () - (ros::spin-once "error_group") - (or error) - ) - (:callback-arm-error (msg) - (setq error (send msg :data)) - ) - (:wait-recover-error () (send error-recovery-act :wait-for-result)) - (:recover-error (&key (wait 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)) - )) - ;; gripper action for real-controller - (:send-gripper-grasp-action - (act width speed force &key (wait t) (inner 0.005) (outer 0.005)) - (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 act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-homing-action - (act &key (wait t)) - (let ((goal (instance franka_gripper::HomingActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-move-action - (act 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 act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-stop-action - (act &key (wait t)) - (let ((goal (instance franka_gripper::StopActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:stop-gripper - (arm &key (wait nil)) - (let (acts) - (case - arm - (:arm (setq acts (list gripper-stop-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-stop-action act - pos (/ (* 1000 0.08) tm) :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:homing-gripper - (arm &key (wait nil)) - (let (acts) - (case - arm - (:arm (setq acts (list gripper-homing-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-homing-action act :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:start-grasp - (arm &optional (pos 0.0) &key (effort 10.0) (tm 500) (wait nil)) ;; TODO :arms is not implemented - (let (acts) - (case - arm - (:arm (setq acts (list gripper-grasp-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-grasp-action act - pos (/ (* 1000 0.08) tm) effort :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:stop-grasp - (arm &key (wait nil)) ;; TODO :arms is not implemented - (unless (memq arm '(:arm :arms)) - (error "you must specify arm ~A from ~A" (car args) '(:arm :arms)) - (return-from :stop-grasp nil)) - (send self :move-gripper arm 0.08 :tm 500 :wait wait) - ) - (:move-gripper - (arm pos &key (tm 500) (timeout 5000) (wait nil)) ;; TODO :arms is not implemented - (let (acts) - (case - arm - (:arm (setq acts (list gripper-move-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-move-action act - pos (/ (* 1000 0.08) tm) :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) + (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))))) ) -;; grasp controller ... (defun panda-init () (setq *ri* (instance panda-robot-interface :init)) - (setq *robot* (panda)) - ) + (setq *robot* (panda))) -#| -(send *ri* :set-all-joint-pd-gain 1000.0 5.0) ;; default -(send *ri* :set-all-joint-pd-gain 300.0 5.0) ;; hard -(send *ri* :set-all-joint-pd-gain 30.0 0.5) ;; soft -|# +(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/panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/panda.urdf.xacro deleted file mode 100644 index 0f0058ab53..0000000000 --- a/jsk_panda_robot/panda_eus/models/panda.urdf.xacro +++ /dev/null @@ -1,5 +0,0 @@ - - - - -