Skip to content

Commit

Permalink
Use prismatic-loaded to detect pushing object
Browse files Browse the repository at this point in the history
  • Loading branch information
pazeshun committed Jun 20, 2017
1 parent 93c1420 commit 8a489ff
Showing 1 changed file with 16 additions and 8 deletions.
24 changes: 16 additions & 8 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l
Original file line number Diff line number Diff line change
Expand Up @@ -351,16 +351,18 @@
(send *ri* :start-grasp arm)
(send *ri* :angle-vector-raw (send *baxter* :angle-vector target-av)
3000 (send *ri* :get-arm-controller arm) 0)
;; Wait until grasp to prevent tooth flying
(send *ri* :wait-interpolation-until-grasp arm)))
;; Wait until grasp or push object to prevent tooth flying
(send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)))
(send *ri* :angle-vector-raw
(send *baxter* 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)
(if (eq arm :larm)
(send *ri* :wait-interpolation-until-grasp arm)
(send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))
(setq graspingp (send *ri* :graspingp arm))
(ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
(unless graspingp
Expand All @@ -370,10 +372,14 @@
(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)
(if (eq arm :larm)
(send *ri* :wait-interpolation-until-grasp arm)
(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-grasp arm)))
(if (eq arm :larm)
(send *ri* :wait-interpolation-until-grasp arm)
(send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))))
(when (eq arm :rarm)
;; Open fingers in bin
(send *ri* :move-hand arm
Expand Down Expand Up @@ -416,20 +422,22 @@
:rotation-axis t)
3000 (send *ri* :get-arm-controller arm) 0)
;; Wait until grasp or finger touch
(send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
(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 pi/2 0 0))
:move-palm-end t
:rotation-axis t)
3000 (send *ri* :get-arm-controller arm) 0)
(send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
(send *ri* :wait-interpolation-until arm
:grasp :finger-flexion :finger-loaded :prismatic-loaded)
(send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper :rarm 120 :relative nil)
3000 (send *ri* :get-arm-controller arm) 0)
(send *ri* :wait-interpolation-until arm :grasp)
(send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)
(send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
(send *ri* :start-grasp arm :hand)
(send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
Expand Down

0 comments on commit 8a489ff

Please sign in to comment.