From c00ee30543d93bcb0133bda75526cec61f5ae35a Mon Sep 17 00:00:00 2001 From: ban-masa Date: Fri, 24 Jun 2016 17:14:06 +0900 Subject: [PATCH] added calib-pressure-threshold --- .../jsk_2016_01_baxter_apc/baxter-interface.l | 37 ++++++++++++++++++- jsk_2016_01_baxter_apc/euslisp/main.l | 1 + 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l index db422cf2d..217446e05 100644 --- a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l +++ b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l @@ -50,7 +50,8 @@ (setq _objects-sib-coords (make-hash-table)) ;; set hard-coded variables (setq _hard-coded-variables (make-hash-table)) - (sethash :pressure-threshold _hard-coded-variables 810) + (sethash :r-pressure-threshold _hard-coded-variables 1000) + (sethash :l-pressure-threshold _hard-coded-variables 1000) (sethash :offset-avoid-bin-top _hard-coded-variables -20) (sethash :pad-link-l _hard-coded-variables 85) (sethash :gripper-tube-t _hard-coded-variables 40) @@ -140,7 +141,13 @@ (progn (setq topic (format nil "gripper_front/limb/~a/pressure/state" (arm-to-str arm))) (< (send (one-shot-subscribe topic std_msgs::Float64) :data) - (gethash :pressure-threshold _hard-coded-variables)) + (cond + ((eq arm :rarm) + (gethash :r-pressure-threshold _hard-coded-variables)) + ((eq arm :larm) + (gethash :l-pressure-threshold _hard-coded-variables)) + ) + ) ) ) ) @@ -962,6 +969,32 @@ ) (ros::ros-info "[:wait-for-user-input-to-start] received user input: ~a" arm) )) + (:update-pressure-threshold + (arm) + (let (pressure hashkey topic) + (cond + ((eq arm :rarm) + (setq hashkey :r-pressure-threshold)) + ((eq arm :larm) + (setq hashkey :l-pressure-threshold))) + (setq topic (format nil "/gripper_front/limb/~a/pressure/state" (arm-to-str arm))) + (setf pressure (- (send (one-shot-subscribe topic std_msgs::Float64) :data) 7)) + (if (< pressure (gethash hashkey _hard-coded-variables)) + (setf (gethash hashkey _hard-coded-variables) pressure)))) + + (:calib-pressure-threshold + (&optional (arm :arms)) + (send self :start-grasp arm) + (dotimes (i 7) + (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm))) + (send self :update-pressure-threshold l/r) + ) + (unix::sleep 1)) + (send self :stop-grasp arm) + (ros::ros-info "[:calib-pressure-threshold] Threshold r:~a l:~a" + (gethash :r-pressure-threshold _hard-coded-variables) + (gethash :l-pressure-threshold _hard-coded-variables))) + ) (defun jsk_2016_01_baxter_apc::baxter-init (&key (ctype :default-controller)) diff --git a/jsk_2016_01_baxter_apc/euslisp/main.l b/jsk_2016_01_baxter_apc/euslisp/main.l index 62cd69250..8ce98f60c 100755 --- a/jsk_2016_01_baxter_apc/euslisp/main.l +++ b/jsk_2016_01_baxter_apc/euslisp/main.l @@ -12,6 +12,7 @@ (send *ri* :gripper-servo-on) (send *ri* :angle-vector (send *baxter* :fold-pose-back)) (send *ri* :wait-interpolation) + (send *ri* :calib-pressure-threshold) (objects (list *baxter*)) t)