From 934b47af36198f9fbeae670191b1ea13fabf824e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 22 Nov 2022 18:18:30 +0900 Subject: [PATCH 1/2] :go-pos-unsafe for kinematic simulation --- jsk_fetch_robot/fetcheus/fetch-interface.l | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 55a354d772..5dd33840df 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -415,11 +415,15 @@ Example: (send self :gripper :position) => 0.00" t) (:go-pos-unsafe (x y d &rest args) ;; [m] [m] [degree] + (if (send self :simulation-modep) + (return-from :go-pos-unsafe (send-super* :go-pos-unsafe x y d args))) (send self :put :go-pos-unsafe-no-wait-goal (float-vector x y d)) (send* self :go-pos-unsafe-wait args) t) (:go-pos-unsafe-no-wait (x y &optional (d 0)) ;; [m] [m] [degree] + (if (send self :simulation-modep) + (return-from :go-pos-unsafe-no-wait (send-super :go-pos-unsafe-no-wait x y d))) (ros::ros-warn ":go-pos-unsafe-no-wait is not supported for this robot.") (send self :put :go-pos-unsafe-no-wait-goal (float-vector x y d)) t) @@ -427,6 +431,8 @@ Example: (send self :gripper :position) => 0.00" (&key (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (min-translation-abs-vel 0.3) (min-rotation-abs-vel 0.8)) + (if (send self :simulation-modep) + (return-from :go-pos-unsafe-wait (send-super :go-pos-unsafe-wait))) (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)) From 545d0a33892d98bf11bdf4d9ce19c2cd988c57a3 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 22 Nov 2022 20:24:55 +0900 Subject: [PATCH 2/2] go-pos and go-pos-unsafe test for kinematics simulation --- jsk_fetch_robot/fetcheus/test/test-fetcheus.l | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l index 39cae6c05c..b4cb5a9e84 100755 --- a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l +++ b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l @@ -126,6 +126,23 @@ (format nil ":go-velocity returns t with argument ~A" go-vel-arg)) )) +;; test for kinematics simulation +(deftest fetch-go-pos + (let (go-pos-arg) + (setq go-pos-arg (list 0.1 0.1 0.5)) + (assert (send* *ri* :go-pos go-pos-arg) + (format nil ":go-pos doesn't return t with argument ~A" go-pos-arg)) + )) + +;; test for kinematics simulation +(deftest fetch-go-pos-unsafe + (let (go-pos-arg) + (setq go-pos-arg (list 0.1 0.1 0.5)) + ;; :go-pos-unsafe returns nil + (assert (null (send* *ri* :go-pos-unsafe go-pos-arg)) + (format nil ":go-pos-unsafe doesn't return t with argument ~A" go-pos-arg)) + )) + (deftest fetch-use-torso (let () (setq *ri* (instance fetch-interface :init))