Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Revert #2235 'Grasp using proximity' #2241

Merged
merged 1 commit into from
Jul 9, 2017
Merged

Revert #2235 'Grasp using proximity' #2241

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 1 addition & 4 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -260,16 +260,13 @@
(: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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ここで左手がエラーになりますね。
すみません、review不足でした。

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

エラーになる理由は教えてくれると嬉しいで@pazeshun @708yamaguchi
#2497 で知りたい

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

この時代には左手には近接センサがなかったからだと思います

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

うーん、なるほど．それはtravisでは引っ掛けられないなぁ．
となると今出来るのは
euslisp/EusLisp#245
ぐらいかなぁ．．．

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

  (: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 -> ???

あれ？近接なくてもエラーにはならない気がするんだけど．．．．

Copy link
Collaborator

@pazeshun pazeshun Aug 1, 2017 •
edited by 708yamaguchi
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

:get-proximityでエラーが出ていたと思います。

https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l#L257-L260
で、左手のtopicが来ていないと全てnilが入ります。
すると、
https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l#L265
で、expected numberのエラーが出ていたと思います。

Copy link
Member

@708yamaguchi 708yamaguchi Aug 1, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

:calib-proximity-thresholdの中で、

(: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))))))

を呼んでいますが、近接センサがないとletの変数のproximitiesがnilになり、
(elt proximities 0)がエラーになりました。

(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 (caar (send (send obj-cube :worldcoords) :rpy-angle))))
(setq pinch-yaw (caar (send (send obj-cube :worldcoords) :rpy-angle))))
(if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) (setq pinch-yaw pi/2) (setq pinch-yaw 0))
(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: 5 additions & 95 deletions jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,11 @@
:super baxter-interface
:slots (rarm-pressure-threshold-
larm-pressure-threshold-
proximity-threshold-
right-hand-action-
pressure-
finger-flex-
finger-load-
prismatic-load-
proximities-
proximity-init-values-))
prismatic-load-))

(defmethod jsk_arc2017_baxter::baxter-interface
(:init
Expand Down Expand Up @@ -75,8 +72,6 @@
(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 @@ -92,13 +87,7 @@
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 @@ -164,9 +153,6 @@
(: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 @@ -198,10 +184,6 @@
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 @@ -214,22 +196,6 @@
(: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 @@ -259,55 +225,9 @@
((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 av avs hand-interpolatingp)
;; rotate arm using proximity sensor
(prev-av (send robot :angle-vector)) avs)
(setf (aref finger-av 1) 180)
(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)))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

typo

(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))
(send self :move-hand l/r finger-av 1000)
(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 @@ -316,7 +236,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 @@ -474,17 +394,7 @@
(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-))
(: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 -> ???
rarm-pressure-threshold- larm-pressure-threshold-)))


(defclass jsk_arc2017_baxter::baxter-moveit-environment
Expand Down
15 changes: 7 additions & 8 deletions jsk_arc2017_baxter/euslisp/lib/baxter.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,32 +109,31 @@
(case (car args)
((:angle-vector)
(let ((av (cadr args)) (joints (gethash arm hand-joints-)))
(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)))
(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))))
(mapcar
#'(lambda (j) (send self j :joint-angle))
(subseq joints 0 (/ (length joints) 2)))
))
(t (ros::ros-error ":hand first arg is invalid. args: ~A~%" args) (exit))
(t (error ":hand first arg is invalid. args: ~A~%" args))
))
(: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 (ros::ros-error ":hand-grasp-pre-pose no such style ~A~%" style) (exit))
(t (error ":hand-grasp-pre-pose no such style ~A~%" style))
))
(: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 (ros::ros-error ":hand-grasp-pose no such style ~A~%" style) (exit))
(t (error ":hand-grasp-pose no such style ~A~%" style))
))
(:avoid-shelf-pose
(arm bin)
Expand Down