commit 708 wrong code

Author: k-okada

Diff for: jsk_arc2017_baxter/euslisp/lib/arc-interface.l

@@ -260,13 +260,16 @@
   (:pick-object-with-movable-region
     (arm movable-region &key (n-trial 1) (n-trial-same-pos 1)
          (do-stop-grasp nil) (grasp-style :suction))
+    (send *ri* :calib-proximity-threshold arm)
     (let (graspingp avs object-index obj-pos obj-cube pinch-yaw)
       ;; TODO: object-index is set randomly
       (setq object-index (random (length (gethash arm object-boxes-))))
       (setq obj-pos
             (send self :get-object-position arm movable-region :object-index object-index))
       (setq obj-cube (send self :bbox->cube (elt (gethash arm object-boxes-) object-index)))
-      (if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) (setq pinch-yaw pi/2) (setq pinch-yaw 0))
+      (if (> (x-of-cube obj-cube) (y-of-cube obj-cube))
+        (setq pinch-yaw (+ pi/2 (caar (send (send obj-cube :worldcoords) :rpy-angle))))
+        (setq pinch-yaw (caar (send (send obj-cube :worldcoords) :rpy-angle))))
       (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm)
       (send *ri* :gripper-servo-on arm)
       ;; Setup arm for picking

Diff for: jsk_arc2017_baxter/euslisp/lib/baxter-interface.l

@@ -15,11 +15,14 @@
   :super baxter-interface
   :slots (rarm-pressure-threshold-
           larm-pressure-threshold-
+          proximity-threshold-
           right-hand-action-
           pressure-
           finger-flex-
           finger-load-
-          prismatic-load-))
+          prismatic-load-
+          proximities-
+          proximity-init-values-))
 
 (defmethod jsk_arc2017_baxter::baxter-interface
   (:init
@@ -72,6 +75,8 @@
     (setq finger-flex- (make-hash-table))
     (setq prismatic-load- (make-hash-table))
     (setq prismatic-vel- (make-hash-table))
+    (setq proximities- (make-hash-table))
+    (setq proximity-init-values- (make-hash-table))
     (dolist (arm (list :rarm :larm))
       (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
                       std_msgs::Float64
@@ -87,7 +92,13 @@
                       dynamixel_msgs::JointState
                       #'send self :set-prismatic-state arm
                       :groupname groupname)
+      (ros::subscribe (format nil "/gripper_front/limb/~a/proximity_array"
+                              (arm2str arm))
+                      force_proximity_ros::ProximityArray
+                      #'send self :set-proximities arm
+                      :groupname groupname)
       (sethash arm finger-flex- (make-hash-table))
+      (sethash arm proximity-init-values- (make-hash-table))
       (dolist (side (list :right :left))
         (ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state"
                                 (arm2str arm) (symbol2str side))
@@ -153,6 +164,9 @@
   (:set-finger-flex
     (arm side msg)
     (sethash side (gethash arm finger-flex-) (send msg :data)))
+  (:set-proximities
+    (arm msg)
+    (sethash arm proximities- (send msg :proximities)))
 
   ;; Hand interface
   ;; based on naoqi-interface and fetch-interface
@@ -184,6 +198,10 @@
           res)
         nil)
       nil))
+  (:cancel-move-hand()
+    (send right-hand-action- :cancel-goal))
+  (:hand-interpolatingp ()
+    (eq (send right-hand-action- :get-state) ros::*simple-goal-state-active*))
   (:get-finger-flex (arm side)
     (send self :spin-once)
     (gethash side (gethash arm finger-flex-)))
@@ -196,6 +214,22 @@
   (:get-prismatic-vel (arm)
     (send self :spin-once)
     (gethash arm prismatic-vel-))
+  (:get-proximity (arm side &key (raw nil))
+    (send self :spin-once)
+    (let ((rl (gethash :left (gethash arm proximity-init-values-)))
+          (rr (gethash :right (gethash arm proximity-init-values-)))
+          ;;(rm (gethash :middle (gethash arm proximity-init-values-)))
+          (proximities (gethash arm proximities-)))
+      (if (null raw)
+        (cond
+          ;; for sparkfun proximity sensor
+          ((eq side :left)
+           (/ (- (send (elt proximities 0) :average) rl) (expt (/ rl 2500.0) 1.5)))
+          ((eq side :right)
+           (/ (- (send (elt proximities 1) :average) rr) (expt (/ rr 2500.0) 1.5))))
+        (cond
+          ((eq side :left) (send (elt proximities 0) :average))
+          ((eq side :right) (send (elt proximities 1) :average))))))
   (:get-real-finger-av (arm)
     (send self :update-robot-state :wait-until-update t)
     (float-vector
@@ -225,9 +259,55 @@
             ((eq type :pinch)
              (send self :update-robot-state :wait-until-update t)
              (let ((finger-av (send self :get-real-finger-av l/r))
-                   (prev-av (send robot :angle-vector)) avs)
+                   prev-av av avs hand-interpolatingp)
+               ;; rotate arm using proximity sensor
                (setf (aref finger-av 1) 180)
-               (send self :move-hand l/r finger-av 1000)
+               (send self :move-hand l/r finger-av 2000 :wait nil)
+               (dotimes (x 100)
+                 (if (send self :interpolatingp) (return))
+                 (unix::usleep 1000))
+               (while (and
+                        (< (max (send self :get-proximity l/r :right)
+                                (send self :get-proximity l/r :left))
+                           proximity-threshold-)
+                        (setq hand-interpolatingp (send self :hand-interpolatingp)))
+                 (unix::usleep 1000))
+               (when hand-interpolatingp
+                 (send self :cancel-move-hand)
+                 (send self :update-robot-state :wait-until-update t)
+                 (setq prev-av (send robot :angle-vector))
+                 (if (< (send self :get-proximity l/r :right) (send self :get-proximity l/r :left))
+                   (progn
+                     (send robot l/r :wrist-r :joint-angle 45 :relative t)
+                     (setq av (send robot :angle-vector))
+                     (send robot :angle-vector prev-av)
+                     (send self :angle-vector-raw av 2000)
+                     (send self :move-hand l/r finger-av 4000 :wait nil)
+                     (dotimes (x 100)
+                       (if (send self :interpolatingp) (return))
+                       (unix::usleep 1000))
+                     (while (and (< (send self :get-proximity l/r :right)
+                                    (send self :get-proximity l/r :left))
+                                 (send self :interpolatingp))
+                       (unix::usleep 1000)))
+                   (progn
+                     (send robot l/r :wrist-r :joint-angle -45 :relative t)
+                     (setq av (send robot :angle-vector))
+                     (send robot :angle-vector prev-av)
+                     (send self :angle-vector-raw av 2000)
+                     (send self :move-hand l/r finger-av 4000 :wait nil)
+                     (dotimes (x 100)
+                       (if (send self :interpolatingp) (return))
+                       (unix::usleep 1000))
+                     (while (and (> (send self :get-proximity l/r :right)
+                                    (send self :get-proximity l/r :left))
+                                 (send self :interpolatingp))
+                       (unix::uleep 1000)))
+                 (send self :cancel-move-hand)
+                 (send self :cancel-angle-vector)
+                 (send self :update-robot-state :wait-until-update t)
+                 (send self :move-hand l/r finger-av 1000))
+               (setq prev-av (send robot :angle-vector))
                (when (> (aref finger-av 0) 45)
                  ;; if cylindrical and spherical grasp, move other gripper joints
                  (send robot l/r :gripper-p :joint-angle -90)
@@ -236,7 +316,7 @@
                  (pushback (send robot :angle-vector) avs)
                  (send robot :angle-vector prev-av)
                  (send self :angle-vector-sequence-raw avs)
-                 (send self :wait-interpolation)))))))
+                 (send self :wait-interpolation))))))))
   (:stop-grasp
     (&optional (arm :arms) (type :suction))
     (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
@@ -392,7 +472,17 @@
                (setq rarm-pressure-threshold- (- min-pressure 15))))))
     (send self :stop-grasp arm)
     (ros::ros-info "[:calib-pressure-threshold] Threshold r: ~a l: ~a"
-                   rarm-pressure-threshold- larm-pressure-threshold-)))
+                   rarm-pressure-threshold- larm-pressure-threshold-))
+  (:calib-proximity-threshold
+    (&optional (arm :arms))
+    (send self :spin-once)
+    (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
+      (let ((hash-table (make-hash-table)))
+        (sethash :left hash-table (send self :get-proximity l/r :left :raw t))
+        (sethash :right hash-table (send self :get-proximity l/r :right :raw t))
+        (sethash :middle hash-table (send self :get-proximity l/r :middle :raw t))
+        (sethash l/r proximity-init-values- hash-table))
+      (setq proximity-threshold- 100)))) ;; TODO decide proper threshold 100 -> ???
 
 
 (defclass jsk_arc2017_baxter::baxter-moveit-environment

Diff for: jsk_arc2017_baxter/euslisp/lib/baxter.l

@@ -109,31 +109,32 @@
     (case (car args)
       ((:angle-vector)
        (let ((av (cadr args)) (joints (gethash arm hand-joints-)))
-         (when av
-           (dotimes (i (length av))
-             (send self (elt joints i) :joint-angle (elt av i))
-             (send self (elt joints (+ i (length av))) :joint-angle (elt av i))))
+         (if (and (null (eq (length av) 0)) (null (eq (length av) 2)))
+           (progn (ros::ros-error "length of angle-vector must be 0 or 2.~%") (exit)))
+         (dotimes (i (length av))
+           (send self (elt joints i) :joint-angle (elt av i))
+           (send self (elt joints (+ i (length av))) :joint-angle (elt av i)))
          (mapcar
            #'(lambda (j) (send self j :joint-angle))
            (subseq joints 0 (/ (length joints) 2)))
          ))
-      (t (error ":hand first arg is invalid. args: ~A~%" args))
+      (t (ros::ros-error ":hand first arg is invalid. args: ~A~%" args) (exit))
       ))
   (:hand-grasp-pre-pose
     (arm style)
     (case style
       (:opposed (send self :hand arm :angle-vector #f(0 0)))
       (:spherical (send self :hand arm :angle-vector #f(30 0)))
       (:cylindrical (send self :hand arm :angle-vector #f(90 0)))
-      (t (error ":hand-grasp-pre-pose no such style ~A~%" style))
+      (t (ros::ros-error ":hand-grasp-pre-pose no such style ~A~%" style) (exit))
     ))
   (:hand-grasp-pose
     (arm style &key (angle 180))
     (case style
       (:opposed (send self :hand arm :angle-vector (float-vector 0 angle)))
       (:spherical (send self :hand arm :angle-vector (float-vector 30 angle)))
       (:cylindrical (send self :hand arm :angle-vector (float-vector 90 angle)))
-      (t (error ":hand-grasp-pose no such style ~A~%" style))
+      (t (ros::ros-error ":hand-grasp-pose no such style ~A~%" style) (exit))
     ))
   (:avoid-shelf-pose
     (arm bin)