From d20092bb2e96ecef79afae88030504029fb35589 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 26 Oct 2019 00:57:05 +0900 Subject: [PATCH] add min abs velocity in :go-pos-unsafe to prevent wheel slipping --- jsk_fetch_robot/fetcheus/fetch-interface.l | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 028ba14c01b..9c8a9131b47 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -240,7 +240,8 @@ Example: (send self :gripper :position) => 0.00" t) (:go-pos-unsafe-wait (&key (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) - (translation-gain 1.0) (rotation-gain 1.0)) + (translation-gain 1.0) (rotation-gain 1.0) + (min-translation-abs-vel 0.3) (min-rotation-abs-vel 0.8)) (unless (send self :get :go-pos-unsafe-no-wait-goal) (ros::ros-error ":go-pos-unsafe-wait is called without goal") (return-from :go-pos-unsafe-wait nil)) @@ -279,7 +280,10 @@ Example: (send self :gripper :position) => 0.00" (when (send self :simulation-modep) (send self :robot :rotate (deg2rad (if (> err 0) 5 -5)) :z) (send self :draw-objects)) - (send self :send-cmd-vel-raw 0 (* rotation-gain err)) + (let ((d-vel (* rotation-gain err))) + (send self :send-cmd-vel-raw 0 + (if (> (abs d-vel) min-rotation-abs-vel) + d-vel (* (if (>= d-vel 0) 1.0 -1.0) min-rotation-abs-vel)))) (ros::sleep)) ;; x @@ -299,8 +303,10 @@ Example: (send self :gripper :position) => 0.00" :translate (float-vector (if forward-p 0.1 -0.1) 0 0))) (send self :draw-objects)) - (send self :send-cmd-vel-raw - (* translation-gain (if forward-p err (- err))) 0) + (let ((x-vel (* translation-gain (if forward-p err (- err))))) + (send self :send-cmd-vel-raw + (if (> (abs x-vel) min-translation-abs-vel) + x-vel (* (if (>= x-vel 0) 1.0 -1.0) min-translation-abs-vel)) 0)) (ros::sleep)) ;; yaw @@ -317,7 +323,10 @@ Example: (send self :gripper :position) => 0.00" (when (send self :simulation-modep) (send self :robot :rotate (deg2rad (if (> err 0) 5 -5)) :z) (send self :draw-objects)) - (send self :send-cmd-vel-raw 0 (* rotation-gain err)) + (let ((d-vel (* rotation-gain err))) + (send self :send-cmd-vel-raw 0 + (if (> (abs d-vel) min-rotation-abs-vel) + d-vel (* (if (>= d-vel 0) 1.0 -1.0) min-rotation-abs-vel)))) (ros::sleep)) t))) ) ;; fetch-interface (simple base actions)