diff --git a/jsk_arc2017_baxter/euslisp/check-suction-and-cylindrical.l b/jsk_arc2017_baxter/euslisp/check-suction-and-cylindrical.l new file mode 100644 index 000000000..da60cc469 --- /dev/null +++ b/jsk_arc2017_baxter/euslisp/check-suction-and-cylindrical.l @@ -0,0 +1,101 @@ +#!/usr/bin/env roseus + +(require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l") + +(ros::load-ros-manifest "jsk_arc2017_baxter") + +(unless (boundp '*baxter*) (jsk_arc2017_baxter::arc-init :ctype :rarm-controller)) + +(defun test () + (let (prev-av next-av gripper-x pre-end-coords) + (setq prev-av (send *baxter* :angle-vector)) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 0) 1000) + ;; :pick-object-with-movable-region + (setq next-av + ;; :rotate-gripper :rarm 0 + ;; :slide-gripper :rarm 80 + ;; end-coords #f(600 -700 0) + (send *baxter* :rarm :angle-vector + #f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 80.0 0.0))) + (setq gripper-x (send *baxter* :rarm :gripper-x :joint-angle)) + (send *baxter* :angle-vector prev-av) + ;; First, move prismatic joint to target position + (send *ri* :angle-vector-raw + (send *baxter* :slide-gripper :rarm gripper-x :relative nil) + :fast (send *ri* :get-arm-controller :rarm) 0 :scale 5.0) + (send *ri* :wait-interpolation) + ;; Fold fingers to avoid collision + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000) + ;; Move whole arm to target pose + (send *baxter* :rarm :angle-vector next-av) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000) + + (send *ri* :wait-interpolation) + ;; :try-to-suction-and-pinch-object + (send *ri* :move-hand :rarm (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000) + + (send *ri* :start-grasp :rarm) + + ;;(break) + + ;; :rotate-gripper :rarm 0 + ;; :slide-gripper :rarm 80 + ;; end-coords #f(600 -700 -300) + (send *baxter* :rarm :angle-vector + #f(-4.48961 -32.8098 21.798 65.4342 -20.9014 61.0237 -26.78 80.0 0.0)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 6000) + ;; Wait until finger touch + (send *ri* :wait-interpolation-until :rarm + :grasp :finger-flexion :finger-loaded :prismatic-loaded) + (unix::sleep 3) + (send *baxter* :angle-vector + (send *ri* :state :potentio-vector :wait-until-update t)) + (setq pre-end-coords (send *baxter* :rarm :end-coords :copy-worldcoords)) + ;; cylindrically grasp object + (send *ri* :move-hand :rarm + (send *baxter* + :hand-grasp-pose :rarm :cylindrical :angle 40) 1000) + (send *ri* :angle-vector-raw + (send *baxter* :slide-gripper :rarm 0 :relative nil) + 3000 (send *ri* :get-arm-controller :rarm) 0) + (send *ri* :wait-interpolation) + (send *ri* :move-hand :rarm + (send *baxter* + :hand-grasp-pose :rarm :cylindrical) 1000 :wait nil) + (send *ri* :wait-interpolation-until :rarm :finger-flexion :hand) + (send pre-end-coords :translate #f(0 0 30)) + (send *ri* :angle-vector-raw + (send *baxter* :rarm :inverse-kinematics pre-end-coords :rotation-axis t :use-gripper nil) + 3000) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :rarm :suction) + (unix::sleep 1) + (send *ri* :move-hand :rarm + (send *baxter* + :hand-grasp-pose :rarm :cylindrical :angle 120) 1000) + (send *baxter* :rarm :move-end-pos #f(0 0 10)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *baxter* :rotate-gripper :rarm -90 :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *ri* :start-grasp :rarm) + (unix::sleep 1) + (send *baxter* :rotate-gripper :rarm 0 :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000) + (send *ri* :move-hand :rarm + (send *baxter* + :hand-grasp-pose :rarm :cylindrical :angle 180) 4000) + (unix::sleep 1) + (send *baxter* :rarm :angle-vector + #f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 80.0 0.0)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 + (send *ri* :get-arm-controller :rarm :gripper nil) 0) + (send *ri* :wait-interpolation) + ;; (unix::sleep 3) + ;; (send *ri* :stop-grasp :rarm :suction) + ;; (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(90 0)) 1000))) diff --git a/jsk_arc2017_baxter/euslisp/check-suction-and-opposed.l b/jsk_arc2017_baxter/euslisp/check-suction-and-opposed.l new file mode 100644 index 000000000..964de5816 --- /dev/null +++ b/jsk_arc2017_baxter/euslisp/check-suction-and-opposed.l @@ -0,0 +1,79 @@ +#!/usr/bin/env roseus + +(require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l") + +(ros::load-ros-manifest "jsk_arc2017_baxter") + +(unless (boundp '*baxter*) (jsk_arc2017_baxter::arc-init :ctype :rarm-controller)) + +(defun test () + (let (prev-av next-av gripper-x end-coords (ik t) (avs nil) tms (count 0)) + (setq prev-av (send *baxter* :angle-vector)) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 0) 1000) + ;; :pick-object-with-movable-region + (setq next-av + ;; :rotate-gripper :rarm 0 + ;; :slide-gripper :rarm 80 + ;; end-coords #f(600 -700 0) + (send *baxter* :rarm :angle-vector + #f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 0.0 0.0))) + (setq gripper-x (send *baxter* :rarm :gripper-x :joint-angle)) + (send *baxter* :angle-vector prev-av) + ;; First, move prismatic joint to target position + (send *ri* :angle-vector-raw + (send *baxter* :slide-gripper :rarm gripper-x :relative nil) + :fast (send *ri* :get-arm-controller :rarm) 0 :scale 5.0) + (send *ri* :wait-interpolation) + ;; Fold fingers to avoid collision + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000) + ;; Move whole arm to target pose + (send *baxter* :rarm :angle-vector next-av) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000) + + (send *ri* :wait-interpolation) + ;; :try-to-suction-and-pinch-object + ;;(send *ri* :move-hand :rarm (send *baxter* :hand-grasp-pose :rarm :opposed :angle 50) 1000) + (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(30 30)) 1000) + + (send *ri* :start-grasp :rarm) + + ;;(break) + + ;; :rotate-gripper :rarm 0 + ;; :slide-gripper :rarm 80 + ;; end-coords #f(600 -700 -300) + (send *baxter* :rarm :angle-vector + #f(-4.48961 -32.8098 21.798 65.4342 -20.9014 61.0237 -26.78 0.0 0.0)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 6000) + ;; Wait until finger touch + (send *ri* :wait-interpolation-until :rarm + :grasp :finger-flexion :finger-loaded :prismatic-loaded) + (unix::sleep 1) + (send *baxter* :angle-vector + (send *ri* :state :potentio-vector :wait-until-update t)) + ;; opposedly grasp object + (unix::sleep 1) + + (setq end-coords (send (send *baxter* :rarm :end-coords) :copy-worldcoords)) + (setq ik (send *baxter* :angle-vector)) + (while (and ik (< count 30)) + (setq count (+ count 1)) + (setq avs (cons ik avs)) + (send *baxter* :rotate-gripper :rarm count :relative nil) + (setq ik (send *baxter* :rarm :inverse-kinematics end-coords))) + (setq avs (reverse avs)) + (setq tms (make-list (- count 1) :initial-element (/ 5000.0 count))) + (send *ri* :angle-vector-sequence-raw avs tms) + + (send *baxter* :rarm :angle-vector + #f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 0.0 0.0)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 5000 + (send *ri* :get-arm-controller :rarm :gripper nil) 0) + (send *ri* :wait-interpolation) + ;; (unix::sleep 3) + ;; (send *ri* :stop-grasp :rarm :suction) + ;; (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(90 0)) 1000))) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 820daf037..f0802cd18 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -356,7 +356,8 @@ ;; Setup arm for picking (let (prev-av next-av gripper-x) (setq prev-av (send *baxter* :angle-vector)) - (if (eq grasp-style :suction) + (cond + ((eq grasp-style :suction) (setq next-av (send *baxter* arm :inverse-kinematics (make-coords :pos (v+ obj-pos #f(0 0 150)) @@ -368,35 +369,41 @@ #'(lambda () (and (< (send *baxter* arm :gripper-x :joint-angle) 110) - (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))) - (progn - (send *baxter* :rotate-gripper arm 0 :relative nil) - (send *baxter* :slide-gripper arm 60 :relative nil) - (setq next-av - (send *baxter* arm :inverse-kinematics - (make-coords :pos (v+ obj-pos #f(0 0 200)) - :rpy (float-vector pinch-yaw 0 0)) - :move-palm-end t - :rotation-axis t)))) - (setq gripper-x (send *baxter* arm :gripper-x :joint-angle)) - (send *baxter* :angle-vector prev-av) - ;; First, move prismatic joint to target position - (send *ri* :angle-vector-raw - (send *baxter* :slide-gripper arm gripper-x :relative nil) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - ;; Fold fingers to avoid collision - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000 - :wait nil) - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000 - :wait nil) - (send *ri* :wait-interpolation) - ;; Move whole arm to target pose - (send *ri* :angle-vector-raw - (send *baxter* :angle-vector next-av) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :wait-interpolation)) + (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))) + ((eq grasp-style :pinch) + (send *baxter* :rotate-gripper :rarm 0 :relative nil) + (send *baxter* :slide-gripper :rarm 60 :relative nil) + (setq next-av + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 200)) + :rpy (float-vector pinch-yaw 0 0)) + :move-palm-end t + :rotation-axis t))) + ((eq grasp-style :suction-and-pinch) + (send *baxter* :rotate-gripper :rarm 0 :relative nil) + (send *baxter* :slide-gripper :rarm 80 :relative nil) + (setq next-av + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 150)) + :rpy (float-vector pinch-yaw 0 0)) + :rotation-axis t)))) + (setq gripper-x (send *baxter* arm :gripper-x :joint-angle)) + (send *baxter* :angle-vector prev-av) + ;; First, move prismatic joint to target position + (send *ri* :angle-vector-raw + (send *baxter* :slide-gripper arm gripper-x :relative nil) + :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) + (send *ri* :wait-interpolation) + ;; Fold fingers to avoid collision + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000) + ;; Move whole arm to target pose + (send *ri* :angle-vector-raw + (send *baxter* :angle-vector next-av) + :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) + (send *ri* :wait-interpolation)) (dotimes (i n-trial) (dotimes (j n-trial-same-pos) (unless graspingp @@ -442,9 +449,13 @@ (:try-to-pick-object (arm obj-pos &key (offset #f(0 0 0)) (grasp-style :suction) (pinch-yaw (if (eq grasp-style :pinch) 0))) - (if (eq grasp-style :suction) - (send self :try-to-suction-object arm obj-pos :offset offset) - (send self :try-to-pinch-object arm obj-pos pinch-yaw :offset offset))) + (cond + ((eq grasp-style :suction) + (send self :try-to-suction-object arm obj-pos :offset offset)) + ((eq grasp-style :pinch) + (send self :try-to-pinch-object arm obj-pos pinch-yaw :offset offset)) + ((eq grasp-style :suction-and-pinch) + (send self :try-to-suction-and-pinch-object arm obj-pos pinch-yaw :offset offset)))) (:try-to-suction-object (arm obj-pos &key (offset #f(0 0 0))) (send self :try-to-suction-object-with-gripper-v6 arm obj-pos :offset offset)) @@ -600,6 +611,67 @@ (setq graspingp (send *ri* :graspingp arm :pinch)) (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp) graspingp)) + (:try-to-suction-and-pinch-object + (arm obj-pos pinch-yaw &key (offset #f(0 0 0))) + (let (graspingp pre-end-coords) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000) + (ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a pinch-yaw: ~a" arm pinch-yaw) + ;; start the vacuum gripper before approaching to the object + (ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a start vacuum gripper" arm) + (send *ri* :start-grasp arm) + (send *ri* :angle-vector-raw + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos offset) + :rpy (float-vector pinch-yaw 0 0)) + :rotation-axis t + :use-gripper nil) + 3000 (send *ri* :get-arm-controller arm) 0) + ;; Wait until finger touch + (send *ri* :wait-interpolation-until arm + :grasp :finger-flexion :finger-loaded :prismatic-loaded) + (send *ri* :angle-vector-raw + (send *baxter* arm :inverse-kinematics + (make-coords :pos obj-pos + :rpy (float-vector pinch-yaw 0 0)) + :rotation-axis t + :use-gripper 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)) + (setq pre-end-coords (send (send *baxter* :rarm :end-coords) :worldcoords)) + ;; cylindrically grasp object + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 40) 1000) + (send *ri* :angle-vector-raw + (send *baxter* :slide-gripper arm 10 :relative nil) + 3000 (send *ri* :get-arm-controller arm) 0) + (send *ri* :wait-interpolation) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical) 1000 :wait nil) + (send *ri* :wait-interpolation-until arm :finger-flextion :hand) + (unix::sleep 3) + (send *ri* :angle-vector-raw + (send *baxter* :rarm :inverse-kinematics + pre-end-coords :rotation-axis t :use-gripper nil) + 3000) + (send *ri* :wait-interpolation-until arm :prismatic-load :finger-loaded) + (send *ri* :stop-grasp arm :suction) + (unix::sleep 1) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical) 1000) + (send *ri* :start-grasp arm :suction) + (setq graspingp (send *ri* :graspingp arm)) + (ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a graspingp: ~a" arm graspingp) + ;; lift object + (ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a lift the object" arm) + (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world) + 3000 (send *ri* :get-arm-controller arm :gripper nil) 0) + (send *ri* :wait-interpolation) + (unix::sleep 1) ;; wait for arm to follow + (setq graspingp (send *ri* :graspingp arm)) + (ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a graspingp: ~a" arm graspingp) + graspingp)) (:ik->bin-center (arm bin &key (offset #f(0 0 0)) (rpy #f(0 0 0)) (rotation-axis t) (use-gripper nil) (move-palm-end nil))