diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 21516083a..a86d3a0cb 100644 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -23,6 +23,8 @@ orders- postponed-objects- rack-leg-cubes- + robot- + robot-interface- tote-cubes- tote-movable-regions- unfinished-objects- @@ -47,22 +49,29 @@ (setq rack-leg-cubes- (make-hash-table)) (setq finished-objects- (make-hash-table)) (setq unfinished-objects- (make-hash-table))) + (:set-robot-interface (ri) (setq robot-interface- ri)) + (:get-robot-interface () + (if robot-interface- robot-interface- (if (boundp '*ri*) *ri* nil))) + (:set-robot (rb) (setq robot- rb)) + (:get-robot () (if robot- robot- (if (boundp '*baxter*) *baxter* nil))) (:arc-reset-pose (&optional (arm :arms) &rest args) (dolist (tmp-arm (if (eq arm :arms) (list :rarm :larm) (list arm))) - (send *baxter* :arc-reset-pose tmp-arm)) - (send* *ri* :angle-vector (send *baxter* :angle-vector) args)) + (send (send self :get-robot) :arc-reset-pose tmp-arm)) + (send* (send self :get-robot-interface) :angle-vector + (send (send self :get-robot) :angle-vector) args)) (:fold-pose-back (&optional (arm :arms) &rest args) (dolist (tmp-arm (if (eq arm :arms) (list :rarm :larm) (list arm))) - (send *baxter* :fold-pose-back tmp-arm)) - (send* *ri* :angle-vector (send *baxter* :angle-vector) args)) + (send (send self :get-robot) :fold-pose-back tmp-arm)) + (send* (send self :get-robot-interface) :angle-vector + (send (send self :get-robot) :angle-vector) args)) (:bbox->cube (bbox) (let* ((dims (ros::tf-point->pos (send bbox :dimensions))) (bx (make-cube (elt dims 0) (elt dims 1) (elt dims 2)))) (send bx :newcoords - (send *ri* :tf-pose->coords + (send (send self :get-robot-interface) :tf-pose->coords (send bbox :header :frame_id) (send bbox :pose))) bx)) @@ -372,7 +381,7 @@ (setq obj-box (send box-msg :boxes)) (setq obj-coords (mapcar #'(lambda (obj-pose) - (send *ri* :tf-pose->coords + (send (send self :get-robot-interface) :tf-pose->coords (send com-msg :header :frame_id) obj-pose)) (send com-msg :poses))) (if (and (> (length obj-box) 0) (> (length obj-coords) 0)) @@ -431,19 +440,19 @@ (return-from :get-largest-object-index target-index))))) nil)) (:resolve-collision-between-fingers (arm) - (send *ri* :angle-vector-raw - (send *baxter* :slide-gripper arm 120 :relative nil) - 1000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000 :wait nil) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :slide-gripper arm 120 :relative nil) + 1000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :cylindrical) 1000 :wait nil) + (send (send self :get-robot-interface) :wait-interpolation) (unix::sleep 1) - (send *ri* :angle-vector-raw - (send *baxter* :slide-gripper arm 0 :relative nil) - 1000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :slide-gripper arm 0 :relative nil) + 1000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) + (send (send self :get-robot-interface) :wait-interpolation)) (:pick-object-in-bin (arm bin &rest args) (let (pick-result movable-region) @@ -466,8 +475,8 @@ (arm movable-region &key (n-trial 1) (n-trial-same-pos 1) (do-stop-grasp nil) (grasp-style :suction) (object-index 0)) "Return value: :grasp-succeeded, :grasp-failed or :ik-failed" - (send *ri* :stop-grasp arm :pinch) - (send *ri* :calib-proximity-threshold arm) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) + (send (send self :get-robot-interface) :calib-proximity-threshold arm) (let (pick-result avs obj-pos obj-cube pinch-yaw suction-yaw near-walls) ;; pick largest bounding box when object-index is 0 (setq obj-pos @@ -502,11 +511,11 @@ (t nil))) (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm) (ros::ros-info "[:pick-object-with-movable-region] arm:~a obj-pos: ~a" arm obj-pos) - (send *ri* :gripper-servo-on arm) + (send (send self :get-robot-interface) :gripper-servo-on arm) ;; Setup arm for picking (let (next-avs target-poss) - (send *baxter* :rotate-gripper arm 0 :relative nil) - (send *baxter* :slide-gripper arm 0 :relative nil) + (send (send self :get-robot) :rotate-gripper arm 0 :relative nil) + (send (send self :get-robot) :slide-gripper arm 0 :relative nil) (let ((target-pos (copy-object obj-pos))) ;; movable-region: ((min-of-x max-of-x) (min-of-y max-of-y) (min-of-z max-of-z)) (setf (elt target-pos 2) (+ (cadr (elt movable-region 2)) 100)) @@ -520,7 +529,7 @@ (progn (dolist (target-pos target-poss) (pushback - (send *baxter* :rotate-wrist-ik arm + (send (send self :get-robot) :rotate-wrist-ik arm (make-coords :pos target-pos :rpy (float-vector (or suction-yaw 0) 0 0)) :move-palm-end nil @@ -528,34 +537,36 @@ next-avs) ) ;; Fold fingers to avoid collision to the bin walls - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :spherical) 1000 + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :spherical) 1000 :wait nil) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :spherical :angle 90) 1000 + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pose arm :spherical :angle 90) 1000 :wait t)) ;; pinch (progn (dolist (target-pos target-poss) (pushback - (send *baxter* :rotate-wrist-ik arm + (send (send self :get-robot) :rotate-wrist-ik arm (make-coords :pos target-pos :rpy (float-vector pinch-yaw 0 0)) :move-palm-end t :rotation-axis t) next-avs)) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :cylindrical :angle 90) 1000))) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pose arm :cylindrical :angle 90) 1000))) (dolist (av next-avs) (unless av (ros::ros-error "[:pick-object-with-movable-region] arm:~a IK before trying grasp fails. Abort picking" arm) (return-from :pick-object-with-movable-region :ik-failed))) ;; Move whole arm to location above of the target pose - (send *ri* :angle-vector-sequence-raw next-avs - :fast (send *ri* :get-arm-controller arm :head t) 0 :scale 10.0) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-sequence-raw next-avs + :fast (send (send self :get-robot-interface) :get-arm-controller arm :head t) + 0 :scale 10.0) + (send (send self :get-robot-interface) :wait-interpolation)) ;; Now hand is inside of the bin, so we can use cylindrical - (send *ri* :move-hand arm (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :cylindrical) 1000) ;; Picking trials in the bin (dotimes (i n-trial) (dotimes (j n-trial-same-pos) @@ -568,15 +579,15 @@ :pinch-yaw pinch-yaw))))) (when do-stop-grasp (unless (eq pick-result :grasp-succeeded) - (send *ri* :stop-grasp arm) + (send (send self :get-robot-interface) :stop-grasp arm) (if (eq grasp-style :pinch) - (send *ri* :stop-grasp arm :pinch)))) + (send (send self :get-robot-interface) :stop-grasp arm :pinch)))) (if (eq grasp-style :suction) (progn ;; Move underactuated fingers to initial pose - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000 :wait nil))) - (send *ri* :gripper-servo-on arm) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :opposed) 1000 :wait nil))) + (send (send self :get-robot-interface) :gripper-servo-on arm) pick-result)) (:get-object-position (arm movable-region &key (object-index 0)) @@ -628,122 +639,140 @@ (arm obj-pos &key (offset #f(0 0 0))) "Return value: :grasp-succeeded or :grasp-failed" (let (graspingp) - (send *ri* :angle-vector-raw - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) :rpy #f(0 0 0)) :use-gripper t :rotation-axis :z) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) ;; start the vacuum gripper after approaching to the object (ros::ros-info "[:try-to-suction-object-with-gripper-v4] arm:~a start vacuum gripper" arm) - (send *ri* :start-grasp arm) + (send (send self :get-robot-interface) :start-grasp arm) (unix::sleep 1) - (send *ri* :angle-vector-raw - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos obj-pos :rpy #f(0 0 0)) :use-gripper t :rotation-axis :z) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation-until-grasp arm) - (setq graspingp (send *ri* :graspingp arm :suction)) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until-grasp arm) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :suction)) (ros::ros-info "[:try-to-suction-object-with-gripper-v4] arm:~a graspingp: ~a" arm graspingp) (unless graspingp (ros::ros-info "[:try-to-suction-object-with-gripper-v4] arm:~a again approach to the object" arm) - (let ((temp-av (send *baxter* :angle-vector))) + (let ((temp-av (send (send self :get-robot) :angle-vector))) ;; only if robot can solve IK - (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0)) - (send *ri* :wait-interpolation-until-grasp arm) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) - 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter - (send *ri* :wait-interpolation-until-grasp arm))) + (if (send (send self :get-robot) arm :move-end-pos #f(0 0 -50) :local) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0)) + (send (send self :get-robot-interface) :wait-interpolation-until-grasp arm) + ;; revert baxter + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector temp-av) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until-grasp arm))) ;; lift object (ros::ros-info "[:try-to-suction-object-with-gripper-v4] arm:~a lift the object" arm) - (send *ri* :gripper-servo-off arm) - (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :gripper-servo-off arm) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :move-end-pos #f(0 0 200) :world) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) (unix::sleep 1) ;; wait for arm to follow - (setq graspingp (send *ri* :graspingp arm :suction)) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :suction)) (ros::ros-info "[:try-to-suction-object-with-gripper-v4] arm:~a graspingp: ~a" arm graspingp) (if graspingp :grasp-succeeded :grasp-failed))) (:try-to-suction-object-with-gripper-v6 (arm obj-pos suction-yaw &key (offset #f(0 0 0))) "Return value: :grasp-succeeded or :grasp-failed" (let (graspingp av - (coords-before-approach (send *baxter* arm :end-coords :copy-worldcoords))) + (coords-before-approach (send (send self :get-robot) arm :end-coords :copy-worldcoords))) (send coords-before-approach :locate (v+ obj-pos #f(0 0 300)) :world) (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a suction-yaw: ~a" arm suction-yaw) ;; start the vacuum gripper before approaching to the object (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a start vacuum gripper" arm) - (send *ri* :start-grasp arm) + (send (send self :get-robot-interface) :start-grasp arm) ;; suction: prismatic-based approach - (send *baxter* :slide-gripper arm 120 :relative nil) - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot) :slide-gripper arm 120 :relative nil) + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) :rpy (float-vector (or suction-yaw 0) 0 0)) :use-gripper t :rotation-axis (if suction-yaw t :z)) - (let ((prismatic-angle (send *baxter* arm :gripper-x :joint-angle))) - (send *baxter* :slide-gripper arm 0 :relative nil) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) ;; move down only the hand palm + (let ((prismatic-angle (send (send self :get-robot) arm :gripper-x :joint-angle))) + (send (send self :get-robot) :slide-gripper arm 0 :relative nil) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) ;; move down only the hand palm - (send *baxter* :slide-gripper arm prismatic-angle :relative nil) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0) + (send (send self :get-robot) :slide-gripper arm prismatic-angle :relative nil) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) ;; FIXME: :wait-interpolation-until using :prismatic-loaded sometimes ends too fast, ;; so currently we only check :grasp (suction pressure). - (send *ri* :wait-interpolation-until arm :grasp) + (send (send self :get-robot-interface) :wait-interpolation-until arm :grasp) - (send *baxter* :slide-gripper arm 120 :relative nil) ;; maximum angle of prismatic joint - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded) + ;; maximum angle of prismatic joint + (send (send self :get-robot) :slide-gripper arm 120 :relative nil) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until arm + :grasp :prismatic-loaded) ) - (setq graspingp (send *ri* :graspingp arm :suction)) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :suction)) (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a graspingp: ~a" arm graspingp) (unless graspingp (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a again approach to the object" arm) - (let ((temp-av (send *baxter* :angle-vector))) + (let ((temp-av (send (send self :get-robot) :angle-vector))) ;; only if robot can solve IK - (if (send *baxter* arm :move-end-pos #f(0 0 -100) :local) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0)) - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) - 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))) + (if (send (send self :get-robot) arm :move-end-pos #f(0 0 -100) :local) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0)) + (send (send self :get-robot-interface) :wait-interpolation-until arm + :grasp :prismatic-loaded) + ;; revert baxter + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector temp-av) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until arm + :grasp :prismatic-loaded))) ;; Open fingers in bin - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :spherical) 1000 :wait nil) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) - (send *ri* :angle-vector-raw (send *baxter* :rotate-gripper arm 30 :relative nil) - 1000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :spherical) 1000 :wait nil) + (send (send self :get-robot) :angle-vector + (send (send self :get-robot-interface) :state :potentio-vector :wait-until-update t)) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :rotate-gripper arm 30 :relative nil) + 1000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) ;; suction: prismatic-based approach - (send *baxter* :slide-gripper arm 0 :relative nil) - (send *baxter* :rotate-gripper arm 0 :relative nil) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0) + (send (send self :get-robot) :slide-gripper arm 0 :relative nil) + (send (send self :get-robot) :rotate-gripper arm 0 :relative nil) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) ;; suction: prismatic-based approach ;; lift object (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a lift the object" arm) - (send *ri* :gripper-servo-off arm) - (let ((tc (send *baxter* arm :end-coords :copy-worldcoords))) + (send (send self :get-robot-interface) :gripper-servo-off arm) + (let ((tc (send (send self :get-robot) arm :end-coords :copy-worldcoords))) ;; overwrite only world-z (setf (aref (send tc :worldpos) 2) (elt (send coords-before-approach :worldpos) 2)) - (send *baxter* arm :inverse-kinematics tc :rotation-axis :z) + (send (send self :get-robot) arm :inverse-kinematics tc :rotation-axis :z) ) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) - (setq graspingp (send *ri* :graspingp arm :suction)) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :suction)) (ros::ros-info "[:try-to-suction-object-with-gripper-v6] arm:~a graspingp: ~a" arm graspingp) (if graspingp :grasp-succeeded :grasp-failed))) (:try-to-pinch-object @@ -752,20 +781,20 @@ (let (av graspingp pre-coords) (ros::ros-info "[:try-to-pinch-object] arm:~a pinch-yaw: ~a" arm pinch-yaw) ;; Initialize finger - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :cylindrical :angle 40) 1000) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pose arm :cylindrical :angle 40) 1000) ;; start the vacuum gripper before approaching to the object (ros::ros-info "[:try-to-pinch-object] arm:~a start vacuum gripper" arm) - (send *ri* :start-grasp arm) + (send (send self :get-robot-interface) :start-grasp arm) (setq av - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) :rpy (float-vector pinch-yaw 0 0)) :move-palm-end t :rotation-axis t)) (if (null av) (setq av - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) :rpy (float-vector pinch-yaw 0 0)) :move-palm-end t @@ -773,22 +802,24 @@ (unless av (ros::ros-error "[:try-to-pinch-object] arm:~a IK to object fails. Abort picking" arm) (return-from :try-to-pinch-object :ik-failed)) - (send *ri* :update-robot-state :wait-until-update t) - (unless (send *ri* :angle-vector-raw av - 3000 (send *ri* :get-arm-controller arm) 0 :end-coords-interpolation t) - (send *ri* :angle-vector-raw av 3000 (send *ri* :get-arm-controller arm) 0)) + (send (send self :get-robot-interface) :update-robot-state :wait-until-update t) + (unless (send (send self :get-robot-interface) :angle-vector-raw av + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) + 0 :end-coords-interpolation t) + (send (send self :get-robot-interface) :angle-vector-raw av 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0)) ;; Wait until grasp or finger touch - (send *ri* :wait-interpolation-until arm + (send (send self :get-robot-interface) :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded :prismatic-loaded :finger-proximity) (setq av - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos obj-pos :rpy (float-vector pinch-yaw 0 0)) :move-palm-end t :rotation-axis t)) (if (null av) (setq av - (send *baxter* arm :inverse-kinematics + (send (send self :get-robot) arm :inverse-kinematics (make-coords :pos obj-pos :rpy (float-vector pinch-yaw 0 0)) :move-palm-end t @@ -796,40 +827,48 @@ (unless av (ros::ros-error "[:try-to-pinch-object] arm:~a IK to object fails. Abort picking" arm) (return-from :try-to-pinch-object :ik-failed)) - (send *ri* :angle-vector-raw av 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation-until arm + (send (send self :get-robot-interface) :angle-vector-raw av 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded :prismatic-loaded :finger-proximity) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) - (send *baxter* arm :move-end-pos #f(0 0 -20) :world) - (send *ri* :angle-vector-raw - (send *baxter* :slide-gripper arm 120 :relative nil) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) - (send *ri* :stop-grasp arm) - (send *ri* :start-grasp arm :pinch) - (send *ri* :start-grasp arm) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) - (setq graspingp (send *ri* :graspingp arm :pinch)) + (send (send self :get-robot) :angle-vector + (send (send self :get-robot-interface) :state :potentio-vector + :wait-until-update t)) + (send (send self :get-robot) arm :move-end-pos #f(0 0 -20) :world) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :slide-gripper arm 120 :relative nil) + 3000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation-until arm :grasp :prismatic-loaded) + (send (send self :get-robot) :angle-vector + (send (send self :get-robot-interface) :state :potentio-vector :wait-until-update t)) + (send (send self :get-robot-interface) :stop-grasp arm) + (send (send self :get-robot-interface) :start-grasp arm :pinch) + (send (send self :get-robot-interface) :start-grasp arm) + (send (send self :get-robot) :angle-vector + (send (send self :get-robot-interface) :state :potentio-vector :wait-until-update t)) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :pinch)) (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp) ;; lift object (ros::ros-info "[:try-to-pinch-object] arm:~a lift the object" arm) - (setq pre-coords (send (send *baxter* arm :end-coords) :copy-worldcoords)) + (setq pre-coords (send (send (send self :get-robot) arm :end-coords) :copy-worldcoords)) (send pre-coords :translate #f(0 0 200) :world) - (setq av (send *baxter* arm :inverse-kinematics pre-coords :rotation-axis t)) + (setq av (send (send self :get-robot) arm :inverse-kinematics pre-coords :rotation-axis t)) (if (null av) (setq av - (send *baxter* arm :inverse-kinematics pre-coords :rotation-axis :z))) - (send *ri* :angle-vector-raw av 3000 (send *ri* :get-arm-controller arm :gripper nil) 0) - (send *ri* :wait-interpolation) + (send (send self :get-robot) arm :inverse-kinematics pre-coords :rotation-axis :z))) + (send (send self :get-robot-interface) :angle-vector-raw av 3000 + (send (send self :get-robot-interface) :get-arm-controller arm :gripper nil) 0) + (send (send self :get-robot-interface) :wait-interpolation) ;; slide gripper to grasp tightly - (send *baxter* :rotate-gripper arm -90 :relative nil) - (send *baxter* :slide-gripper arm 0 :relative nil) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - :fast (send *ri* :get-arm-controller arm) 0 :scale 3.0) - (send *ri* :wait-interpolation) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) - (setq graspingp (send *ri* :graspingp arm :pinch)) + (send (send self :get-robot) :rotate-gripper arm -90 :relative nil) + (send (send self :get-robot) :slide-gripper arm 0 :relative nil) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 3.0) + (send (send self :get-robot-interface) :wait-interpolation) + (send (send self :get-robot) :angle-vector + (send (send self :get-robot-interface) :state :potentio-vector :wait-until-update t)) + (setq graspingp (send (send self :get-robot-interface) :graspingp arm :pinch)) (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp) (if graspingp :grasp-succeeded :grasp-failed))) (:ik->bin-center @@ -845,7 +884,7 @@ (send bin-coords :rotate (aref rpy 0) :z) (send bin-coords :rotate (aref rpy 1) :y) (send bin-coords :rotate (aref rpy 2) :x) - (send *baxter* arm :inverse-kinematics bin-coords + (send (send self :get-robot) arm :inverse-kinematics bin-coords :rotation-axis rotation-axis :use-gripper use-gripper :move-palm-end move-palm-end))) @@ -864,12 +903,12 @@ (send cardboard-coords :rotate (aref rpy 2) :x) ;; use prismatic joint as much as possible - (let ((jnt (send *baxter* :joint (format nil "~a_gripper_prismatic_joint" (arm2str arm))))) + (let ((jnt (send (send self :get-robot) :joint (format nil "~a_gripper_prismatic_joint" (arm2str arm))))) (send jnt :joint-angle (send jnt :max-angle)) ) - (send *baxter* arm :inverse-kinematics cardboard-coords + (send (send self :get-robot) arm :inverse-kinematics cardboard-coords :rotation-axis nil :use-gripper nil) - (send *baxter* arm :inverse-kinematics cardboard-coords + (send (send self :get-robot) arm :inverse-kinematics cardboard-coords :rotation-axis rotation-axis :use-gripper use-gripper :move-palm-end move-palm-end))) @@ -886,7 +925,7 @@ (send tote-coords :rotate (aref rpy 0) :z) (send tote-coords :rotate (aref rpy 1) :y) (send tote-coords :rotate (aref rpy 2) :x) - (send *baxter* arm :inverse-kinematics tote-coords + (send (send self :get-robot) arm :inverse-kinematics tote-coords :rotation-axis rotation-axis :use-gripper use-gripper :move-palm-end move-palm-end))) @@ -895,32 +934,36 @@ (let (avs offset rpy (offset-x (if (eq arm :larm) 30 -50)) (offset-y (if (eq arm :larm) 280 -250))) - (setq avs (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) - (send *baxter* :reset-pose arm) - (send *baxter* :rotate-gripper arm gripper-angle :relative nil) + (setq avs + (list (send (send self :get-robot) :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) + (send (send self :get-robot) :reset-pose arm) + (send (send self :get-robot) :rotate-gripper arm gripper-angle :relative nil) (setq offset (float-vector offset-x offset-y 250)) (setq rpy (float-vector 0 pi/2 (if (eq arm :larm) pi/2 -pi/2))) (send self :ik->bin-center arm bin :offset offset :rpy rpy :use-gripper nil) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 - (send *ri* :get-arm-controller arm) 0))) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0))) (:move-arm-body->tote-overlook-pose (arm &key (gripper-angle 90)) (let (avs offset rpy (offset-x (if (eq arm :larm) 0 -20)) (offset-y (if (eq arm :larm) 390 -320))) - (setq avs (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) - (send *baxter* :reset-pose arm) - (send *baxter* :rotate-gripper arm gripper-angle :relative nil) + (setq avs + (list (send (send self :get-robot) :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) + (send (send self :get-robot) :reset-pose arm) + (send (send self :get-robot) :rotate-gripper arm gripper-angle :relative nil) (setq offset (float-vector offset-x offset-y 250)) (setq rpy (float-vector 0 pi/2 (if (eq arm :larm) pi/2 -pi/2))) (pushback (send self :ik->tote-center arm :offset offset :rpy rpy :use-gripper nil) avs) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 - (send *ri* :get-arm-controller arm) 0))) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0))) (:wait-for-user-input-to-start (arm) (let (can-start) (ros::ros-info "[:wait-for-user-input-to-start] wait for user input to start: ~a" arm) @@ -951,7 +994,7 @@ (format nil "/bin_contents/~A" (symbol-string bin)))) (:add-workspace-scene () - (let ((base-name (send (send *baxter* :base_lk) :name)) + (let ((base-name (send (send (send self :get-robot) :base_lk) :name)) (dimensions (list '(2500 10 2000) '(2500 10 2000))) (positions (list (float-vector 500 1200 0) (float-vector 500 -1200 0))) (names (list "workspace_left" "workspace_right"))) @@ -968,7 +1011,7 @@ )) (:add-bin-scene (bin) (let ((cube (gethash bin bin-cubes-)) - (base-name (send (send *baxter* :base_lk) :name))) + (base-name (send (send (send self :get-robot) :base_lk) :name))) (send *co* :add-object cube :frame-id base-name :relative-pose (send cube :copy-worldcoords) :object-id (format nil "bin_~A" (symbol-string bin))))) @@ -981,7 +1024,7 @@ (dolist (bin (list :a :b :c)) (send self :delete-bin-scene bin))) (:add-cardboard-scene (cardboard) - (let (cardboard-cube (base-name (send (send *baxter* :base_lk) :name))) + (let (cardboard-cube (base-name (send (send (send self :get-robot) :base_lk) :name))) (setq cardboard-cube (gethash cardboard cardboard-cubes-)) (send *co* :add-object cardboard-cube :frame-id base-name :relative-pose (send cardboard-cube :copy-worldcoords) @@ -993,7 +1036,7 @@ (:add-cardboad-rack-leg-scene (side) (let* ((cardboard-c-cube (gethash :c cardboard-cubes-)) (cardboard-c-coords (send cardboard-c-cube :copy-worldcoords)) - (base-name (send (send *baxter* :base_lk) :name)) + (base-name (send (send (send self :get-robot) :base_lk) :name)) rack-leg-cube offset) (setq offset (float-vector @@ -1021,7 +1064,7 @@ (:add-tote-scene (arm) (dolist (tmp-arm (if (eq arm :arms) (list :rarm :larm) (list arm))) (let ((cube (gethash tmp-arm tote-cubes-)) - (base-name (send (send *baxter* :base_lk) :name))) + (base-name (send (send (send self :get-robot) :base_lk) :name))) (send *co* :add-object cube :frame-id base-name :relative-pose (send cube :copy-worldcoords) :object-id (format nil "~A_tote" (arm2str tmp-arm)))))) @@ -1057,12 +1100,13 @@ (send *co* :wipe-all)) (:spin-off-by-wrist (arm &key (times 10)) - (send *ri* :angle-vector-sequence-raw - (send *baxter* :spin-off-by-wrist arm :times times) - :fast (send *ri* :get-arm-controller arm) 0)) + (send (send self :get-robot-interface) :angle-vector-sequence-raw + (send (send self :get-robot) :spin-off-by-wrist arm :times times) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0)) (:send-av (&optional (tm 3000) (ctype nil)) - (send *ri* :angle-vector (send *baxter* :angle-vector) tm ctype)) + (send (send self :get-robot-interface) :angle-vector + (send (send self :get-robot) :angle-vector) tm ctype)) (:check-can-start (arm start-state wait-state) (let ((service-name (format nil "/state_server/~a_hand/check_can_start" (arm2str arm))) @@ -1313,6 +1357,8 @@ (unless (boundp '*co*) (setq *co* (when moveit (instance collision-object-publisher :init)))) (unless (boundp '*ti*) - (setq *ti* (instance jsk_arc2017_baxter::arc-interface :init))) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :calib-grasp :arms))) + (setq *ti* (instance jsk_arc2017_baxter::arc-interface :init)) + (send *ti* :set-robot-interface *ri*)) + (send (send *ti* :get-robot) :angle-vector + (send (send *ti* :get-robot-interface) :state :potentio-vector)) + (send (send *ti* :get-robot-interface) :calib-grasp :arms))) diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l index c1424ac19..5450e146d 100644 --- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l @@ -50,7 +50,7 @@ (send self :add-shelf-scene) ;; (send self :add-cardboard-rack-scene) ) - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm is waiting for user input to start the picking task." (arm2str arm))) (ros::ros-info "[:wait-for-user-input] wait for user input to start: ~a" arm) @@ -66,13 +66,13 @@ (send self :set-objects-param arm bin :object-type :all)) can-start)) (:calib-prismatic-joint (arm &key (timeout 10)) - (send *ri* :start-prismatic-calib arm) + (send (send self :get-robot-interface) :start-prismatic-calib arm) (let ((time-count 1)) - (while (and (send *ri* :prismatic-calibratingp arm) + (while (and (send (send self :get-robot-interface) :prismatic-calibratingp arm) (> (* timeout 10) time-count)) (unix::usleep (* 100 1000)) (incf time-count)) - (if (send *ri* :prismatic-calibratingp arm) nil t))) + (if (send (send self :get-robot-interface) :prismatic-calibratingp arm) nil t))) (:set-target (arm) (setq label-names- (ros::get-param (format nil "/~a_hand_camera/label_names" @@ -96,13 +96,13 @@ (let (is-recognized recognition-count) (setq trial-fail-count- 0) (setq grasp-style- nil) - (send *ri* :move-hand arm + (send (send self :get-robot-interface) :move-hand arm (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) (ros::ros-info "[main] Recognizing objects in bin ~a" target-bin-) (unless (> start-picking-fail-count- 0) - (send *ri* :speak (format nil "~a arm is going to bin ~a." (arm2str arm) target-bin-)) + (send (send self :get-robot-interface) :speak (format nil "~a arm is going to bin ~a." (arm2str arm) target-bin-)) (send self :move-arm-body->bin-overlook-pose arm target-bin-) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :wait-interpolation)) (setq recognition-count 1) (let ((stamp (ros::time-now)) order) (while (null (or (> recognition-count trial-times) is-recognized)) @@ -148,11 +148,11 @@ (send self :add-postponed-object arm target-obj- target-bin-) (setq start-picking-fail-count- 0) (ros::ros-info "[main] arm: ~a, failed to recognize object ~a" arm target-obj-) - (send *ri* :angle-vector-sequence-raw + (send (send self :get-robot-interface) :angle-vector-sequence-raw (list (send *baxter* :fold-to-keep-object arm) (send *baxter* :arc-reset-pose arm)) - :fast (send *ri* :get-arm-controller arm) 0 :scale 3.0) - (send *ri* :wait-interpolation)) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 3.0) + (send (send self :get-robot-interface) :wait-interpolation)) (:check-recognize-fail-count (arm &key (count-limit 2)) (incf recognize-fail-count-) (ros::ros-info "[main] arm: ~a, recognize fail count: ~a" arm recognize-fail-count-) @@ -186,7 +186,7 @@ start-picking-failp)) (:change-grasp-style (arm) (incf trial-fail-count-) - (send *ri* :stop-grasp arm) + (send (send self :get-robot-interface) :stop-grasp arm) (let (next-style) ;; try another grasp style (setq next-style (send self :get-grasp-style arm target-obj- (+ trial-fail-count- 1))) @@ -201,7 +201,7 @@ nil)))) (:pick-object (arm) - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm is picking ~a, which is~a an ordered object." (arm2str arm) (underscore-to-space target-obj-) @@ -225,7 +225,7 @@ (when moveit-p- (send self :add-bin-scene target-bin-)) ;; Don't trust pressure sensor ;; (unless (eq pick-result :ik-failed) - ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (setq graspingp (send (send self :get-robot-interface) :graspingp arm grasp-style-)) ;; (unless graspingp (return-from :pick-object nil)) ;; ) (let (avs end-pos) @@ -249,12 +249,12 @@ :rpy (float-vector (if (eq arm :larm) -pi/2 pi/2) 0 0)) :move-palm-end t :rotate-axis t))) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 - (send *ri* :get-arm-controller arm :gripper nil) 0) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-raw (send *baxter* :angle-vector) 3000 + (send (send self :get-robot-interface) :get-arm-controller arm :gripper nil) 0) + (send (send self :get-robot-interface) :wait-interpolation)) (if (eq pick-result :ik-failed) (return-from :pick-object nil)) ;; Don't trust pressure sensor - ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (setq graspingp (send (send self :get-robot-interface) :graspingp arm grasp-style-)) ;; (ros::ros-info "[main] arm: ~a graspingp: ~a" arm graspingp) ;; graspingp)) (setq scale-candidates- @@ -268,12 +268,13 @@ (:return-from-pick-object (arm) (send self :add-postponed-object arm target-obj- target-bin-) (setq start-picking-fail-count- 0) - (send *ri* :angle-vector-sequence-raw + (send (send self :get-robot-interface) :angle-vector-sequence-raw (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)) (send *baxter* :arc-reset-pose arm)) - :fast (send *ri* :get-arm-controller arm :gripper (eq grasp-style- :suction)) + :fast (send (send self :get-robot-interface) :get-arm-controller arm + :gripper (eq grasp-style- :suction)) 0 :scale 3.0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm))) (:verify-object (arm) @@ -337,31 +338,32 @@ (send self :add-postponed-object arm target-obj- target-bin-) (when moveit-p- (send self :delete-bin-scene target-bin-)) (if (eq grasp-style- :suction) - (send *ri* :angle-vector-raw + (send (send self :get-robot-interface) :angle-vector-raw (send self :ik->bin-center arm target-bin- :offset (send self :decide-bin-return-area arm target-bin-) :rotation-axis :z :use-gripper t) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :angle-vector-raw + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 5.0) + (send (send self :get-robot-interface) :angle-vector-raw (send self :ik->bin-center arm target-bin- :offset (send self :decide-bin-return-area arm target-bin-) :rotation-axis :z :move-palm-end t) - :fast (send *ri* :get-arm-controller arm :gripper nil) 0 :scale 5.0)) - (send *ri* :wait-interpolation) - (send *ri* :stop-grasp arm) + :fast (send (send self :get-robot-interface) :get-arm-controller arm :gripper nil) + 0 :scale 5.0)) + (send (send self :get-robot-interface) :wait-interpolation) + (send (send self :get-robot-interface) :stop-grasp arm) (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) (send *baxter* :rotate-gripper arm 90 :relative nil)) (send self :spin-off-by-wrist arm :times 3) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (ros::ros-info "[main] ~a, return object in shelf" arm) - (send *ri* :angle-vector-raw + (send (send self :get-robot-interface) :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world :rotation-axis :z) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :wait-interpolation) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 5.0) + (send (send self :get-robot-interface) :wait-interpolation) (when moveit-p- (send self :add-bin-scene target-bin-)) (send self :arc-reset-pose arm) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm))) (:place-object (arm) @@ -379,17 +381,17 @@ ;; (setq path-constraints (instance moveit_msgs::Constraints :init ;; :orientation_constraints (list orientation-constraint))) - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm is placing ~a into box ~a." (arm2str arm) (underscore-to-space target-obj-) target-cardboard-)) (ros::ros-info "[main] ~a, place object in bin ~a" arm target-cardboard-) ;; (when moveit-p- (send self :add-object-in-hand-scene arm target-obj-)) ;; FIXME: moveit cannot solve path - ;; (send *ri* :angle-vector + ;; (send (send self :get-robot-interface) :angle-vector ;; (send self :ik->cardboard-center arm target-cardboard- ;; :offset #f(0 0 200) :rotation-axis :z :use-gripper t) - ;; 4000 (send *ri* :get-arm-controller arm) 0 + ;; 4000 (send (send self :get-robot-interface) :get-arm-controller arm) 0 ;; :path-constraints path-constraints) (let (avs (offsets (list #f(-50 0 500) #f(-50 0 200) #f(0 0 200) #f(30 0 200)))) (dolist (offset offsets) @@ -403,26 +405,28 @@ (setq avs (remove nil avs)) (setq place-z 50) (if (eq grasp-style- :suction) - (send *ri* :angle-vector-sequence-raw avs :fast - (send *ri* :get-arm-controller arm :gripper t :head t) 0 :scale 7.0) - (send *ri* :angle-vector-sequence-raw avs :fast - (send *ri* :get-arm-controller arm :gripper nil :head t) 0 :scale 7.0)) - (send *ri* :wait-interpolation)) - (setq dropped (not (send *ri* :graspingp arm grasp-style-))) + (send (send self :get-robot-interface) :angle-vector-sequence-raw avs :fast + (send (send self :get-robot-interface) :get-arm-controller arm + :gripper t :head t) 0 :scale 7.0) + (send (send self :get-robot-interface) :angle-vector-sequence-raw avs :fast + (send (send self :get-robot-interface) :get-arm-controller arm + :gripper nil :head t) 0 :scale 7.0)) + (send (send self :get-robot-interface) :wait-interpolation)) + (setq dropped (not (send (send self :get-robot-interface) :graspingp arm grasp-style-))) ;; (when moveit-p- (send self :delete-object-in-hand-scene arm)) (if dropped (progn - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm dropped ~a to somewhere." (arm2str arm) (underscore-to-space target-obj-))) (ros::ros-error "[main] arm ~a: dropped object" arm) - (send *ri* :stop-grasp arm) + (send (send self :get-robot-interface) :stop-grasp arm) (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) (send *baxter* :rotate-gripper arm 90 :relative nil)) ;; we assume object is dropped, but it can be just a mis-detection of grasp (send self :spin-off-by-wrist arm :times 3) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (send self :add-postponed-object arm target-obj- target-bin-)) (let (av) (ros::ros-info-green "[main] arm ~a: place object ~a in cardboard ~a" arm target-obj- target-cardboard-) @@ -435,31 +439,32 @@ (send *baxter* arm :move-end-pos (float-vector 0 0 (- place-z)) :world :rotation-axis nil))) (when av - (send *ri* :angle-vector-raw av - 2000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation)) - (send *ri* :stop-grasp arm) ;; release object + (send (send self :get-robot-interface) :angle-vector-raw av + 2000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation)) + (send (send self :get-robot-interface) :stop-grasp arm) ;; release object (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) (send *baxter* :rotate-gripper arm 90 :relative nil)) ;; release object (send self :spin-off-by-wrist arm :times 5) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (when av (setq av (if (eq grasp-style- :suction) (send *baxter* arm :move-end-pos (float-vector 0 0 place-z) :world :rotation-axis :z :use-gripper t) (send *baxter* arm :move-end-pos (float-vector 0 0 place-z) :world :rotation-axis nil))) - (send *ri* :angle-vector-raw av - 1000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-raw av + 1000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation)) (when moveit-p- (send self :add-cardboard-scene target-cardboard-)))))) (:return-from-place-object (arm) (let (avs) (setq avs (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) (pushback (send *baxter* :arc-reset-pose arm) avs) - (send *ri* :angle-vector-sequence avs 5000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :angle-vector-sequence avs 5000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm)) (setq grasp-style- nil)))) @@ -479,6 +484,7 @@ (unless (boundp '*co*) (setq *co* (when moveit (instance collision-object-publisher :init)))) (unless (boundp '*ti*) - (setq *ti* (instance jsk_arc2017_baxter::pick-interface :init :moveit moveit))) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :calib-grasp :arms))) + (setq *ti* (instance jsk_arc2017_baxter::pick-interface :init :moveit moveit)) + (send *ti* :set-robot-interface *ri*)) + (send *baxter* :angle-vector (send (send *ti* :get-robot-interface) :state :potentio-vector)) + (send (send *ti* :get-robot-interface) :calib-grasp :arms))) diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l index 42229f467..9a2db2cd4 100644 --- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l @@ -57,13 +57,13 @@ (send self :set-objects-param arm :tote :object-type :all) can-start)) (:calib-prismatic-joint (arm &key (timeout 10)) - (send *ri* :start-prismatic-calib arm) + (send (send self :get-robot-interface) :start-prismatic-calib arm) (let ((time-count 1)) - (while (and (send *ri* :prismatic-calibratingp arm) + (while (and (send (send self :get-robot-interface) :prismatic-calibratingp arm) (> (* timeout 10) time-count)) (unix::usleep (* 100 1000)) (incf time-count)) - (if (send *ri* :prismatic-calibratingp arm) nil t))) + (if (send (send self :get-robot-interface) :prismatic-calibratingp arm) nil t))) (:recognize-object (arm &key (trial-times 10)) (let (is-recognized recognition-count label-names) (setq trial-fail-count- 0) @@ -72,12 +72,12 @@ (ros::get-param (format nil "/~a_hand_camera/label_names" (arm2str arm)))) (send self :set-target-location arm :tote) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) + (send (send self :get-robot-interface) :move-hand arm + (send (send self :get-robot) :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) (ros::ros-info "[main] Recognizing objects in tote") (unless (> start-picking-fail-count- 0) (send self :move-arm-body->tote-overlook-pose arm) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :wait-interpolation)) (setq recognition-count 1) (let ((stamp (ros::time-now)) target-index) (while (null (or (> recognition-count trial-times) is-recognized)) @@ -104,11 +104,11 @@ (send self :add-postponed-object arm target-obj- :tote) (setq start-picking-fail-count- 0) (ros::ros-info "[main] arm: ~a, failed to recognize object in tote" arm) - (send *ri* :angle-vector-sequence-raw - (list (send *baxter* :fold-to-keep-object arm) - (send *baxter* :arc-reset-pose arm)) - :fast (send *ri* :get-arm-controller arm) 0 :scale 3.0) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-sequence-raw + (list (send (send self :get-robot) :fold-to-keep-object arm) + (send (send self :get-robot) :arc-reset-pose arm)) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 3.0) + (send (send self :get-robot-interface) :wait-interpolation)) (:check-recognize-fail-count (arm &key (count-limit 2)) (incf recognize-fail-count-) (ros::ros-info "[main] arm: ~a, recognize fail count: ~a" arm recognize-fail-count-) @@ -143,7 +143,7 @@ start-picking-failp)) (:change-grasp-style (arm) (incf trial-fail-count-) - (send *ri* :stop-grasp arm) + (send (send self :get-robot-interface) :stop-grasp arm) (let (next-style) ;; try another grasp style (setq next-style (send self :get-grasp-style arm target-obj- (+ trial-fail-count- 1))) @@ -158,7 +158,7 @@ nil)))) (:pick-object (arm &key (use-scale t) (move-head t) (avoid-tote t)) - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm is picking ~a." (arm2str arm) (underscore-to-space target-obj-))) (let (pick-result graspingp) (setq recognize-fail-count- 0) @@ -167,7 +167,8 @@ (send self :set-movable-region-for-tote arm :offset (list 50 50 0)) (send self :set-movable-region-for-tote arm :offset (list 50 50 0))) (when use-scale (send self :reset-scale arm)) - (when move-head (send *baxter* :head_pan :joint-angle (if (eq arm :larm) -70 70))) + (when move-head + (send (send self :get-robot) :head_pan :joint-angle (if (eq arm :larm) -70 70))) (setq pick-result (send self :pick-object-in-tote arm :n-trial (if (eq grasp-style- :suction) 2 1) @@ -175,10 +176,11 @@ :do-stop-grasp nil :grasp-style grasp-style-)) (when moveit-p- (send self :add-tote-scene arm)) - (unless avoid-tote (return-from :pick-object (send *ri* :graspingp arm))) + (unless avoid-tote + (return-from :pick-object (send (send self :get-robot-interface) :graspingp arm))) ;; Don't trust pressure sensor ;; (unless (eq pick-result :ik-failed) - ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (setq graspingp (send (send self :get-robot-interface) :graspingp arm grasp-style-)) ;; (unless graspingp (return-from :pick-object nil)) ;; ) (let (end-pos) @@ -188,8 +190,10 @@ :offset #f(0 0 400) :rpy (float-vector (if (eq arm :larm) -pi/2 pi/2) 0 0) :rotation-axis t :use-gripper t) - (setq end-pos (send (send *baxter* arm :end-coords :copy-worldcoords) :worldpos)) - (send *baxter* :rotate-wrist-ik arm + (setq end-pos + (send (send (send self :get-robot) arm :end-coords :copy-worldcoords) + :worldpos)) + (send (send self :get-robot) :rotate-wrist-ik arm (make-coords :pos end-pos :rpy (float-vector (if (eq arm :larm) -pi/2 pi/2) 0 0)) :rotate-axis t)) @@ -198,19 +202,23 @@ :offset #f(0 0 400) :rpy (float-vector (if (eq arm :larm) -pi/2 pi/2) 0 0) :rotation-axis t :move-palm-end t) - (setq end-pos (send (send *baxter* arm :palm-endpoint :copy-worldcoords) :worldpos)) - (send *baxter* :rotate-wrist-ik arm + (setq end-pos + (send (send (send self :get-robot) arm :palm-endpoint :copy-worldcoords) + :worldpos)) + (send (send self :get-robot) :rotate-wrist-ik arm (make-coords :pos end-pos :rpy (float-vector (if (eq arm :larm) -pi/2 pi/2) 0 0)) :move-palm-end t :rotate-axis t))) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 - (send *ri* :get-arm-controller arm :gripper nil) 0) - (send *ri* :wait-interpolation)) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) :angle-vector) 3000 + (send (send self :get-robot-interface) :get-arm-controller arm :gripper nil) 0) + (send (send self :get-robot-interface) :wait-interpolation)) (if (eq pick-result :ik-failed) (return-from :pick-object nil)) - (unless use-scale (return-from :pick-object (send *ri* :graspingp arm))) + (unless use-scale + (return-from :pick-object (send (send self :get-robot-interface) :graspingp arm))) ;; Don't trust pressure sensor - ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (setq graspingp (send (send self :get-robot-interface) :graspingp arm grasp-style-)) ;; (ros::ros-info "[main] arm: ~a graspingp: ~a" arm graspingp) ;; graspingp)) (setq scale-candidates- @@ -224,12 +232,13 @@ (:return-from-pick-object (arm) (send self :add-postponed-object arm target-obj- :tote) (ros::ros-info "[main] arm: ~a return from pick-object to arc-reset-pose" arm) - (send *ri* :angle-vector-sequence-raw - (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)) - (send *baxter* :arc-reset-pose arm)) - :fast (send *ri* :get-arm-controller arm :gripper (eq grasp-style- :suction)) + (send (send self :get-robot-interface) :angle-vector-sequence-raw + (list (send (send self :get-robot) :avoid-shelf-pose arm (if (eq arm :larm) :d :f)) + (send (send self :get-robot) :arc-reset-pose arm)) + :fast (send (send self :get-robot-interface) :get-arm-controller arm + :gripper (eq grasp-style- :suction)) 0 :scale 3.0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm))) (:verify-object (arm) @@ -291,72 +300,75 @@ ;; (setq offset (send self :decide-tote-return-area arm)) ;; (ros::ros-info "return object to tote: arm, ~A, offset ~A" (arm2str arm) offset) (if (eq grasp-style- :suction) - (send *ri* :angle-vector-raw + (send (send self :get-robot-interface) :angle-vector-raw (send self :ik->tote-center arm :offset offset :rotation-axis :z :use-gripper t) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :angle-vector-raw + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 5.0) + (send (send self :get-robot-interface) :angle-vector-raw (send self :ik->tote-center arm :offset offset :rotation-axis :z :move-palm-end t) - :fast (send *ri* :get-arm-controller arm :gripper nil) 0 :scale 5.0))) - (send *ri* :wait-interpolation) - (send *ri* :stop-grasp arm) + :fast (send (send self :get-robot-interface) :get-arm-controller arm :gripper nil) + 0 :scale 5.0))) + (send (send self :get-robot-interface) :wait-interpolation) + (send (send self :get-robot-interface) :stop-grasp arm) (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) - (send *baxter* :rotate-gripper arm 90 :relative nil)) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) + (send (send self :get-robot) :rotate-gripper arm 90 :relative nil)) (send self :spin-off-by-wrist arm :times 3) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (ros::ros-info "[main] ~a, return object in tote" arm) - (send *ri* :angle-vector-raw - (send *baxter* arm :move-end-pos #f(0 0 200) :world :rotation-axis :z) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :move-end-pos #f(0 0 200) :world :rotation-axis :z) + :fast (send (send self :get-robot-interface) :get-arm-controller arm) 0 :scale 5.0) + (send (send self :get-robot-interface) :wait-interpolation) (when moveit-p- (send self :add-tote-scene arm)) - (send self :arc-reset-pose arm 3000 (send *ri* :get-arm-controller arm) 0 :move-arm arm) - (send *ri* :wait-interpolation) + (send self :arc-reset-pose arm 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0 :move-arm arm) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm))) (:place-object (arm) (let (dropped place-av) - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm is placing ~a into bin ~a." (arm2str arm) (underscore-to-space target-obj-) target-bin-)) (ros::ros-info "[main] ~a, place object in bin ~a" arm target-bin-) (when moveit-p- (send self :add-object-in-hand-scene arm target-obj-)) - (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 70 -70)) + (send (send self :get-robot) :head_pan :joint-angle (if (eq arm :larm) 70 -70)) (if (eq grasp-style- :suction) - (send *ri* :angle-vector + (send (send self :get-robot-interface) :angle-vector (send self :ik->bin-center arm target-bin- :offset (v+ #f(0 0 300) (send self :decide-bin-place-area arm target-bin- grasp-style-)) :rotation-axis :z :use-gripper t) - 4000 (send *ri* :get-arm-controller arm :head t) 0 - :move-arm arm) - (send *ri* :angle-vector + 4000 (send (send self :get-robot-interface) :get-arm-controller arm :head t) + 0 :move-arm arm) + (send (send self :get-robot-interface) :angle-vector (send self :ik->bin-center arm target-bin- :offset (v+ #f(0 0 300) (send self :decide-bin-place-area arm target-bin- grasp-style-)) :rotation-axis nil :move-palm-end t) - 4000 (send *ri* :get-arm-controller arm :gripper nil :head t) 0 - :move-arm arm)) - (send *ri* :wait-interpolation) - (setq dropped (not (send *ri* :graspingp arm grasp-style-))) + 4000 (send (send self :get-robot-interface) + :get-arm-controller arm :gripper nil :head t) + 0 :move-arm arm)) + (send (send self :get-robot-interface) :wait-interpolation) + (setq dropped (not (send (send self :get-robot-interface) :graspingp arm grasp-style-))) (when moveit-p- (send self :delete-object-in-hand-scene arm)) (if dropped (progn - (send *ri* :speak + (send (send self :get-robot-interface) :speak (format nil "~a arm dropped ~a to somewhere." (arm2str arm) (underscore-to-space target-obj-))) (ros::ros-error "[main] arm ~a: dropped object" arm) - (send *ri* :stop-grasp arm) + (send (send self :get-robot-interface) :stop-grasp arm) (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) - (send *baxter* :rotate-gripper arm 90 :relative nil)) + (send (send self :get-robot-interface) :stop-grasp arm :pinch) + (send (send self :get-robot) :rotate-gripper arm 90 :relative nil)) ;; we assume object is dropped, but it can be just a mis-detection of grasp (send self :spin-off-by-wrist arm :times 3) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (send self :add-postponed-object arm target-obj- :tote)) (progn (send self :update-json target-obj- :src :tote :dst (cons :bin target-bin-)) @@ -364,39 +376,42 @@ (ros::ros-info-green "[main] arm ~a: place object ~a in bin ~a" arm target-obj- target-bin-) (when moveit-p- (send self :delete-bin-scene target-bin-)) (if (eq grasp-style- :suction) - (send *ri* :angle-vector-raw - (send *baxter* arm :move-end-pos #f(0 0 -300) :world + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :move-end-pos #f(0 0 -300) :world :rotation-axis :z :use-gripper t) - 2000 (send *ri* :get-arm-controller arm) 0) + 2000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) (progn (setq place-av - (send *baxter* arm :move-end-pos #f(0 0 -250) :world :rotation-axis nil)) + (send (send self :get-robot) arm :move-end-pos #f(0 0 -250) + :world :rotation-axis nil)) (if place-av - (send *ri* :angle-vector-raw place-av 2000 (send *ri* :get-arm-controller arm) 0)))) - (send *ri* :wait-interpolation) - (send *ri* :stop-grasp arm) ;; release object + (send (send self :get-robot-interface) :angle-vector-raw place-av 2000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0)))) + (send (send self :get-robot-interface) :wait-interpolation) + (send (send self :get-robot-interface) :stop-grasp arm) ;; release object (when (eq grasp-style- :pinch) - (send *ri* :stop-grasp arm :pinch) - (send *baxter* :rotate-gripper arm 90 :relative nil)) ;; release object + (send (send self :get-robot-interface) :stop-grasp arm :pinch) + (send (send self :get-robot) :rotate-gripper arm 90 :relative nil)) ;; release object (send self :spin-off-by-wrist arm :times 5) - (send *ri* :wait-interpolation) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :suction) - (send *ri* :angle-vector-raw - (send *baxter* arm :move-end-pos #f(0 0 300) :world + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :move-end-pos #f(0 0 300) :world :rotation-axis :z :use-gripper t) - 2000 (send *ri* :get-arm-controller arm) 0) + 2000 (send (send self :get-robot-interface) :get-arm-controller arm) 0) (if place-av - (send *ri* :angle-vector-raw - (send *baxter* arm :move-end-pos #f(0 0 250) :world + (send (send self :get-robot-interface) :angle-vector-raw + (send (send self :get-robot) arm :move-end-pos #f(0 0 250) :world :rotation-axis nil) - 2000 (send *ri* :get-arm-controller arm) 0))) - (send *ri* :wait-interpolation) + 2000 (send (send self :get-robot-interface) :get-arm-controller arm) 0))) + (send (send self :get-robot-interface) :wait-interpolation) (when moveit-p- (send self :add-bin-scene target-bin-)))))) (:return-from-place-object (arm) - (send self :arc-reset-pose arm 3000 (send *ri* :get-arm-controller arm) 0 :move-arm arm) - (send *ri* :wait-interpolation) + (send self :arc-reset-pose arm 3000 + (send (send self :get-robot-interface) :get-arm-controller arm) 0 :move-arm arm) + (send (send self :get-robot-interface) :wait-interpolation) (if (eq grasp-style- :pinch) (send self :resolve-collision-between-fingers arm)) (setq grasp-style- nil))) @@ -416,6 +431,8 @@ (unless (boundp '*co*) (setq *co* (when moveit (instance collision-object-publisher :init)))) (unless (boundp '*ti*) - (setq *ti* (instance jsk_arc2017_baxter::stow-interface :init :moveit moveit))) - (send *baxter* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :calib-grasp :arms))) + (setq *ti* (instance jsk_arc2017_baxter::stow-interface :init :moveit moveit)) + (send *ti* :set-robot-interface *ri*)) + (send (send *ti* :get-robot) :angle-vector + (send (send *ti* :get-robot-interface) :state :potentio-vector)) + (send (send *ti* :get-robot-interface) :calib-grasp :arms)))