Skip to content

Commit

Permalink
commit 708 wrong code to find why this passed test
Browse files Browse the repository at this point in the history
  • Loading branch information
@k-okada
k-okada committed Jul 31, 2017
1 parent b9b3599 commit 68787d5
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 13 deletions.
5 changes: 4 additions & 1 deletion jsk_arc2017_baxter/euslisp/lib/arc-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
100 changes: 95 additions & 5 deletions jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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))
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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-)))
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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)))
Expand Down Expand Up @@ -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
Expand Down
15 changes: 8 additions & 7 deletions jsk_arc2017_baxter/euslisp/lib/baxter.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 68787d5

Please sign in to comment.