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.

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

Add pinching with gripper-v6 #2203

Merged
merged 34 commits into from
Jul 5, 2017
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
34 commits
Select commit Hold shift + click to select a range
0707b25
Enable to get gripper sensor states
pazeshun Jul 2, 2017
06b647b
Add get func of gripper sensor states
pazeshun Jul 2, 2017
95481f2
Enable :start-grasp and :stop-grasp to move hand
pazeshun Jul 2, 2017
c67fe05
Erase one-shot-subscribe and consider pinching in :graspingp
pazeshun Jul 2, 2017
ffef86b
Erase one-shot-subscribe in pressure calib
pazeshun Jul 2, 2017
b46f06e
Add :wait-interpolation-until
pazeshun Jul 2, 2017
3d5f1b6
Enable to select no gripper controller
pazeshun Jul 2, 2017
f9de111
Enable to set palm endpoint as move-target in IK
pazeshun Jul 2, 2017
c6133e2
Don't back to fold-pose-back until 2nd failure in stow
pazeshun Jul 2, 2017
4aaf9ce
Add set-grasp-style state in stow
pazeshun Jul 2, 2017
8e278a6
Enable :pick-object-with-movable-region to get grasp-style
pazeshun Jul 2, 2017
103ae7b
Split try-to-pick-object to try-to-pick-object-v4 and try-to-suction-…
pazeshun Jul 3, 2017
2705c72
Use wait-interpolation-until in try-to-suction-object
pazeshun Jul 3, 2017
5f7c83c
Add :try-to-pinch-object and use it in stow
pazeshun Jul 3, 2017
d6745e2
Don't back to fold-pose-back until 2nd failure in pick
pazeshun Jul 3, 2017
9ee918f
Add pinching to pick
pazeshun Jul 3, 2017
7a2a839
Stop grasp in return-from-pick-object
pazeshun Jul 3, 2017
8591814
Add logging to try-to-pinch-object
pazeshun Jul 3, 2017
a971561
Avoid collision between fingers
pazeshun Jul 3, 2017
f2600cd
Re-calibrate finger tendon winder
pazeshun Jul 3, 2017
5969a9e
Calib finger init state of try-to-pick-object
pazeshun Jul 3, 2017
eb18c88
Don't use prismatic load for graspingp and calib thresholds
pazeshun Jul 3, 2017
bb755b0
Ignore unstable flex value and calib flex offset
pazeshun Jul 3, 2017
9ba6b28
Reset picking-fail-count for new target obj
pazeshun Jul 4, 2017
e15845e
Rewrite waiting for :interpolatingp
pazeshun Jul 4, 2017
d192d14
Add meta method :try-to-pick-object and :try-to-suction-object
pazeshun Jul 4, 2017
057e20c
Move pinch-yaw to key in try-to-pick-object
pazeshun Jul 5, 2017
e56f868
Fold fingers more tightly before suction-object
pazeshun Jul 5, 2017
11b4df9
Fix logging of wait-interpolation-until
pazeshun Jul 5, 2017
67e17a2
Don't turn gripper over in ik->cardboard-center
pazeshun Jul 4, 2017
356a958
Wait for opposite return-object in pick task
pazeshun Jul 5, 2017
dd85519
Ignore collision between fingers and other gripper parts
pazeshun Jul 5, 2017
f2e1ba9
Fix offset of place-object in pick for moveit
pazeshun Jul 5, 2017
682f82b
Avoid collision to shelf or tote in pick-object
pazeshun Jul 5, 2017
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
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,6 @@ finger_tendon_controller:
ignored_errors: ['DXL_OVERLOAD_ERROR']
motor:
id: 3
init: 3530
min: 3530
max: 1000
init: 3370
min: 3370
max: 800
4 changes: 2 additions & 2 deletions jsk_arc2017_baxter/config/right_gripper_v6/ros_control.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ flex_thresholds:
- 720
- 700
wind_offset_flex:
- 0.35
- 0.35
- 0.5
- 0.5

control_rate: 20
286 changes: 203 additions & 83 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l

Large diffs are not rendered by default.

230 changes: 197 additions & 33 deletions jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,11 @@
:super baxter-interface
:slots (rarm-pressure-threshold-
larm-pressure-threshold-
right-hand-action-))
right-hand-action-
pressure-
finger-flex-
finger-load-
prismatic-load-))

(defmethod jsk_arc2017_baxter::baxter-interface
(:init
Expand Down Expand Up @@ -62,6 +66,34 @@
(and joint-action-enable (send right-hand-action- :wait-for-server 3))
(ros::ros-warn "~A is not respond" right-hand-action-)
(ros::ros-info "*** if you do not have hand, you can ignore this message ***"))
;; gripper sensors
(setq pressure- (make-hash-table))
(setq finger-load- (make-hash-table))
(setq finger-flex- (make-hash-table))
(setq prismatic-load- (make-hash-table))
(setq prismatic-vel- (make-hash-table))
(dolist (arm (list :rarm :larm))
(ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
std_msgs::Float64
#'send self :set-pressure arm
:groupname groupname)
(ros::subscribe (format nil "/gripper_front/limb/~a/dxl/finger_tendon_controller/state"
(arm2str arm))
dynamixel_msgs::JointState
#'send self :set-finger-load arm
:groupname groupname)
(ros::subscribe (format nil "/gripper_front/limb/~a/dxl/prismatic_joint_controller/state"
(arm2str arm))
dynamixel_msgs::JointState
#'send self :set-prismatic-state arm
:groupname groupname)
(sethash arm finger-flex- (make-hash-table))
(dolist (side (list :right :left))
(ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state"
(arm2str arm) (symbol2str side))
std_msgs::UInt16
#'send self :set-finger-flex arm side
:groupname groupname)))
(if mvit-rb (setq moveit-robot mvit-rb))
(if mvit-env (send self :set-moveit-environment (send mvit-env :init :robot moveit-robot))))
;; Overwrite super class's :rarm-controller
Expand Down Expand Up @@ -108,6 +140,19 @@
(cons :controller-state "/gripper_front/limb/left/state")
(cons :action-type control_msgs::FollowJointTrajectoryAction)
(cons :joint-names (list "left_gripper_vacuum_pad_joint")))))
(:set-pressure
(arm msg)
(sethash arm pressure- (send msg :data)))
(:set-finger-load
(arm msg)
(sethash arm finger-load- (send msg :load)))
(:set-prismatic-state
(arm msg)
(sethash arm prismatic-load- (send msg :load))
(sethash arm prismatic-vel- (send msg :velocity)))
(:set-finger-flex
(arm side msg)
(sethash side (gethash arm finger-flex-) (send msg :data)))

;; Hand interface
;; based on naoqi-interface and fetch-interface
Expand Down Expand Up @@ -139,31 +184,88 @@
res)
nil)
nil))
(:get-finger-flex (arm side)
(send self :spin-once)
(gethash side (gethash arm finger-flex-)))
(:get-finger-load (arm)
(send self :spin-once)
(gethash arm finger-load-))
(:get-prismatic-load (arm)
(send self :spin-once)
(gethash arm prismatic-load-))
(:get-prismatic-vel (arm)
(send self :spin-once)
(gethash arm prismatic-vel-))
(:get-real-finger-av (arm)
(send self :update-robot-state :wait-until-update t)
(float-vector
(send robot (str2symbol (format nil "~a_gripper_finger_yaw_joint" (arm2str arm)))
:joint-angle)
(send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
:joint-angle)))
(:finger-closep (arm)
(if (eq arm :rarm)
(> (aref (send self :get-real-finger-av arm) 1) 30) nil))

(:get-arm-controller (arm)
(cond ((eq arm :rarm) :rarm-controller)
((eq arm :larm) :larm-controller)
(:get-arm-controller (arm &key (gripper t))
(cond ((eq arm :rarm) (if gripper :rarm-controller :rarm-no-gripper-controller))
((eq arm :larm) (if gripper :larm-controller :larm-no-gripper-controller))
(t nil)))
(:start-grasp
(&optional (arm :arms))
(dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))
(if (ros::get-param "/apc_on_gazebo" nil)
(ros::service-call
(format nil "/robot/~a_vacuum_gripper/on" l/r)
(instance std_srvs::EmptyRequest :init))
(ros::publish
(format nil "/vacuum_gripper/limb/~a" l/r)
(instance std_msgs::Bool :init :data t)))))
(&optional (arm :arms) (type :suction))
(dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
(cond ((eq type :suction)
(if (ros::get-param "/apc_on_gazebo" nil)
(ros::service-call
(format nil "/robot/~a_vacuum_gripper/on" (arm2str l/r))
(instance std_srvs::EmptyRequest :init))
(ros::publish
(format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
(instance std_msgs::Bool :init :data t))))
((eq type :pinch)
(send self :update-robot-state :wait-until-update t)
(let ((finger-av (send self :get-real-finger-av l/r))
(prev-av (send robot :angle-vector)) avs)
(setf (aref finger-av 1) 180)
(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)
(pushback (send robot :angle-vector) avs)
(send robot l/r :gripper-x :joint-angle 0)
(pushback (send robot :angle-vector) avs)
(send robot :angle-vector prev-av)
(send self :angle-vector-sequence-raw avs)
(send self :wait-interpolation)))))))
(:stop-grasp
(&optional (arm :arms))
(dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))
(if (ros::get-param "/apc_on_gazebo" nil)
(ros::service-call
(format nil "/robot/~a_vacuum_gripper/off" l/r)
(instance std_srvs::EmptyRequest :init))
(ros::publish
(format nil "/vacuum_gripper/limb/~a" l/r)
(instance std_msgs::Bool :init :data nil)))))
(&optional (arm :arms) (type :suction))
(dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
(cond ((eq type :suction)
(if (ros::get-param "/apc_on_gazebo" nil)
(ros::service-call
(format nil "/robot/~a_vacuum_gripper/off" (arm2str l/r))
(instance std_srvs::EmptyRequest :init))
(ros::publish
(format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
(instance std_msgs::Bool :init :data nil))))
((eq type :pinch)
(send self :update-robot-state :wait-until-update t)
(let ((finger-av (send self :get-real-finger-av l/r))
(prev-av (send robot :angle-vector)) avs)
(setf (aref finger-av 1) 0)
(send self :move-hand l/r finger-av 1000)
(when (> (aref finger-av 0) 45)
;; if cylindrical and spherical grasp, move other gripper joints
(pushback (send robot :angle-vector) avs)
(send robot l/r :gripper-x :joint-angle 120)
(pushback (send robot :angle-vector) avs)
(send robot l/r :gripper-p :joint-angle 0)
(pushback (send robot :angle-vector) avs)
(send robot l/r :gripper-x :joint-angle 0)
(pushback (send robot :angle-vector) avs)
(send robot :angle-vector prev-av)
(send self :angle-vector-sequence-raw avs)
(send self :wait-interpolation)))))))
(:gripper-servo-on
(&optional (arm :arms))
(dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))
Expand All @@ -177,19 +279,29 @@
(format nil "/gripper_front/limb/~a/servo/torque" l/r)
(instance std_msgs::Bool :init :data nil))))
(:graspingp
(arm)
(let (topic)
(arm &optional type)
(let (topic finger-av fingerp)
(if (ros::get-param "/apc_on_gazebo" nil)
(progn
(setq topic (format nil "/robot/~a_vacuum_gripper/grasping" (arm-to-str arm)))
(send (one-shot-subscribe topic std_msgs::Bool) :data)
)
(progn
(setq topic (format nil "gripper_front/limb/~a/pressure/state" (arm-to-str arm)))
(< (send (one-shot-subscribe topic std_msgs::Float64) :data)
(cond
((eq arm :rarm) rarm-pressure-threshold-)
((eq arm :larm) larm-pressure-threshold-)))))))
(when (eq arm :rarm)
(setq finger-av (send self :get-real-finger-av arm))
(setq fingerp
(and (or
(and (< (aref finger-av 0) 45) (< (aref finger-av 1) 80))
(and (>= (aref finger-av 0) 45) (< (aref finger-av 1) 155)))
(> (send self :get-finger-load arm) 0.55))))
(send self :spin-once)
(or (if (not (eq type :suction)) fingerp nil)
(if (not (eq type :pinch))
(< (gethash arm pressure-)
(cond
((eq arm :rarm) rarm-pressure-threshold-)
((eq arm :larm) larm-pressure-threshold-)))
nil))))))
(:arm-potentio-vector
(arm)
(case arm
Expand All @@ -212,15 +324,67 @@
(progn
(ros::ros-info "[:wait-interpolation-until-grasp] Grasping detected. Cancel angle vector: ~a" arm)
(send self :cancel-angle-vector)))))
(:wait-interpolation-until
(arm &rest args)
(when (send self :simulation-modep)
(return-from :wait-interpolation-until (send self :wait-interpolation)))
(unless (or (eq arm :rarm) (eq arm :larm))
(error ":wait-interpolation-until set arm for first arg~%"))
(let (conds r-init-flex l-init-flex init-load)
(setq r-init-flex (send self :get-finger-flex arm :right))
(setq l-init-flex (send self :get-finger-flex arm :left))
(setq init-load (send self :get-finger-load arm))
(ros::ros-info
"[:wait-interpolation-until] Init flex: r: ~a l: ~a" r-init-flex l-init-flex)
(ros::ros-info "[:wait-interpolation-until] Init load: ~a" init-load)
(when (member :grasp args)
(ros::ros-info "[:wait-interpolation-until] Prepare for grasp")
(pushback #'(lambda () (send self :graspingp arm)) conds))
(when (member :finger-flexion args)
(ros::ros-info "[:wait-interpolation-until] Prepare for finger flexion")
(pushback
#'(lambda () (or (> (send self :get-finger-flex arm :right) (+ r-init-flex 20))
(> (send self :get-finger-flex arm :left) (+ l-init-flex 20))))
conds))
(when (member :finger-extension args)
(ros::ros-info "[:wait-interpolation-until] Prepare for finger extension")
(pushback
#'(lambda () (or (< (send self :get-finger-flex arm :right) (- r-init-flex 30))
(< (send self :get-finger-flex arm :left) (- l-init-flex 30))))
conds))
(when (member :finger-loaded args)
(ros::ros-info "[:wait-interpolation-until] Prepare for finger loaded")
(pushback
#'(lambda () (> (send self :get-finger-load arm) (+ init-load 0.01)))
conds))
(when (member :finger-unloaded args)
(ros::ros-info "[:wait-interpolation-until] Prepare for finger unloaded")
(pushback
#'(lambda () (< (send self :get-finger-load arm) (- init-load 0.01)))
conds))
(when (member :prismatic-loaded args)
(ros::ros-info "[:wait-interpolation-until] Prepare for prismatic joint loaded")
(pushback
#'(lambda () (and (< (send self :get-prismatic-load arm) -0.07)
(< (send self :get-prismatic-vel arm) 0.01)))
conds))
;; wait for :interpolatingp
(dotimes (x 100)
(if (send self :interpolatingp) (return))
(unix::usleep 1000))
(while (send self :interpolatingp)
(when (reduce #'(lambda (x y) (or x y)) (mapcar #'funcall conds))
(ros::ros-info "[:wait-interpolation-until] Cancel angle vector: ~a" arm)
(send self :cancel-angle-vector)))))
(:calib-pressure-threshold
(&optional (arm :arms))
(send self :start-grasp arm)
(dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
(let ((min-pressure)
(topic (format nil "/gripper_front/limb/~a/pressure/state" (arm-to-str l/r))))
(let (min-pressure)
(dotimes (i 7)
(let ((pressure (send (one-shot-subscribe topic std_msgs::Float64) :data)))
(when (or (null min-pressure) (< pressure min-pressure)) (setq min-pressure pressure)))
(send self :spin-once)
(when (or (null min-pressure) (< (gethash l/r pressure-) min-pressure))
(setq min-pressure (gethash l/r pressure-)))
(unix::sleep 1))
(cond ((eq l/r :larm)
(setq larm-pressure-threshold- (- min-pressure 5)))
Expand Down
14 changes: 13 additions & 1 deletion jsk_arc2017_baxter/euslisp/lib/baxter.l
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,12 @@
;; We have to move mimic joints, too
:right_gripper_r_finger_yaw_joint
:right_gripper_r_finger_roll_joint))
;; translate palm end for good cylindrical grasp
(send self :rarm-palm-endpoint :translate #f(30 0 0) :local)
)
(:rarm-palm-endpoint
(&rest args)
(send* self :right_gripper_palm_endpoint_lk args))
(:set-ik-prepared-poses
(poses)
(if (listp poses)
Expand All @@ -66,8 +71,15 @@
(if (atom move-target)
(send self :link-list (send move-target :parent))
(mapcar #'(lambda (mt) (send self :link-list (send mt :parent))) move-target)))
(use-gripper nil) (rthre (deg2rad 10))
(use-gripper nil) (move-palm-end nil) (rthre (deg2rad 10))
&allow-other-keys)
;; currently works only if move-target is not list
;; set endpoint of palm as move-target
(if move-palm-end
(dolist (limb (list :rarm))
(when (eq move-target (send self limb :end-coords))
(setq move-target (send self limb :palm-endpoint))
(setq link-list (send self :link-list (send move-target :parent))))))
;; if the last link of link-list is in gripper, remove gripper links
(if (null use-gripper)
(cond ((equal (send (car (last link-list)) :name) "right_gripper_pad_with_base")
Expand Down
Loading