From a4a4051eeda0d013a7e4a66f9e7a481c6ddfa870 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Dec 2019 13:55:30 +0900 Subject: [PATCH 01/13] Add test for #422 --- pr2eus/test/pr2-ri-test-arm.l | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/pr2eus/test/pr2-ri-test-arm.l b/pr2eus/test/pr2-ri-test-arm.l index 244c863ad..08c856f93 100644 --- a/pr2eus/test/pr2-ri-test-arm.l +++ b/pr2eus/test/pr2-ri-test-arm.l @@ -229,6 +229,19 @@ (assert (< tm-diff 2) (format nil "end-coords-interpolation takes too long time ~A" tm-diff)) )) +(deftest test-angle-vector-fast + (send *pr2* :angle-vector (send *ri* :state :reference-vector)) + (send *pr2* :r_wrist_roll_joint :joint-angle + (- (send *pr2* :r_wrist_roll_joint :joint-angle) 200)) + ;; make sure :check-continuous-joint-move-over-180 is true + (assert (send *ri* :check-continuous-joint-move-over-180 + (v- (send *ri* :state :reference-vector) (send *pr2* :angle-vector))) + "Prerequisites did not meet at test-angle-vector-fast") + ;; check for integer-expected errors when time is `:fast` and + ;; `:check-continuous-joint-move-over-180' is true. + (send *ri* :angle-vector (send *pr2* :angle-vector) :fast nil 0 :min-time 500) + (send *ri* :wait-interpolation)) + (run-all-tests) (exit) From c292d0541ffe0ad2810adc2c2ff2ed0e41e49821 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 19 Dec 2019 11:36:58 +0900 Subject: [PATCH 02/13] Remove :min-time at test-angle-vector-fast --- pr2eus/test/pr2-ri-test-arm.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pr2eus/test/pr2-ri-test-arm.l b/pr2eus/test/pr2-ri-test-arm.l index 08c856f93..ca856c711 100644 --- a/pr2eus/test/pr2-ri-test-arm.l +++ b/pr2eus/test/pr2-ri-test-arm.l @@ -239,7 +239,7 @@ "Prerequisites did not meet at test-angle-vector-fast") ;; check for integer-expected errors when time is `:fast` and ;; `:check-continuous-joint-move-over-180' is true. - (send *ri* :angle-vector (send *pr2* :angle-vector) :fast nil 0 :min-time 500) + (send *ri* :angle-vector (send *pr2* :angle-vector) :fast) (send *ri* :wait-interpolation)) (run-all-tests) From 7e4563a399e7d3968e199b41dae4e6a23b8fcc5c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Nov 2019 17:03:13 +0900 Subject: [PATCH 03/13] Fix bug on joint-move-over-180 with tm :fast --- pr2eus/pr2-interface.l | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l index 303735aae..ecb53f113 100644 --- a/pr2eus/pr2-interface.l +++ b/pr2eus/pr2-interface.l @@ -439,13 +439,16 @@ Example: (send self :gripper :rarm :position) => 0.00" (ros::ros-warn " : ~A ~A" avs tms) (ros::ros-warn " current angle vector : ~A" prev-av) (ros::ros-warn "new trajectory command : dvi = ~A" div) - (dotimes (i div) - (ros::ros-warn " : ~A ~A" (midpoint (/ (+ i 1.0) div) prev-av av) (/ (elt tms l) div))) - (dotimes (i (1- div)) - (setq avs (list-insert (midpoint (/ (+ i 1.0) div) prev-av av) (+ l i) avs))) - (setf (elt tms l) (/ (elt tms l) div)) - (dotimes (i (1- div)) - (setq tms (list-insert (elt tms l) l tms))) + (let ((midtm (if (equal (elt tms l) :fast) + :fast + (/ (elt tms l) div)))) + (setf (elt tms l) midtm) + (dotimes (i (1- div)) + (let ((midpt (midpoint (/ (+ i 1.0) div) prev-av av))) + (ros::ros-warn " : ~A ~A" midpt midtm) + (setq avs (list-insert midpt (+ l i) avs)) + (setq tms (list-insert midtm l tms)))) + (ros::ros-warn " : ~A ~A" av midtm)) (incf l (1- div)) (ros::ros-warn "new trajectory command :") (ros::ros-warn " : ~A ~A" avs tms) From ccef1f4b3bf4ecf7a662dd33a12c1dc87bf85053 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 20 Nov 2019 17:11:57 +0900 Subject: [PATCH 04/13] Fix bug on angle-vector-sequence with end-coords-interpolation t and tm :fast --- pr2eus/robot-interface.l | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 35ae8138f..0f27f696c 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -481,6 +481,9 @@ (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev)) (setq target-limbs nil) (setq tm (elt tms i)) + (if (equal tm :fast) + (setq tm (* 1000 (send self :angle-vector-duration + av-prev av scale min-time ctype)))) (setq pass-tm (/ tm (float end-coords-interpolation-steps))) (dotimes (l (length limbs)) (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) From 4dda0114ea02097804e9d5edd39ba481f74790ed Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 21 Dec 2019 06:58:58 +0000 Subject: [PATCH 05/13] fix indent --- pr2eus/robot-interface.l | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 0f27f696c..31f506d1d 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -478,13 +478,13 @@ (dolist (av avs) (send robot :angle-vector av) (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) - (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev)) + (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev)) (setq target-limbs nil) - (setq tm (elt tms i)) + (setq tm (elt tms i)) (if (equal tm :fast) (setq tm (* 1000 (send self :angle-vector-duration av-prev av scale min-time ctype)))) - (setq pass-tm (/ tm (float end-coords-interpolation-steps))) + (setq pass-tm (/ tm (float end-coords-interpolation-steps))) (dotimes (l (length limbs)) (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) (when (and ec-prev ec-current From afe3df8ce1eaaf45b005347e6f5a0fa33897c8b8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:02:20 +0900 Subject: [PATCH 06/13] add pr2-ri-test-simple.l to test/pr2-ri-test.launch need to add static_transform_publisher 0 0 0 0 0 0 /world /map 100 to provide '/world' --- pr2eus/test/pr2-ri-test.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch index f2564d6d7..25c4016b2 100644 --- a/pr2eus/test/pr2-ri-test.launch +++ b/pr2eus/test/pr2-ri-test.launch @@ -137,6 +137,8 @@ observation_sources: base_scan + @@ -146,4 +148,6 @@ args="$(find pr2eus)/test/pr2-ri-test-arm.l" time-limit="800" /> + From 6fc83c4f25a941f952221c1595aa34f572f9a43e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:11:08 +0900 Subject: [PATCH 07/13] pr2-ri-test-simple.l: add eps= with 1.0 degree buffer --- pr2eus/test/pr2-ri-test-simple.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l index 6e156f20c..66d9b2376 100644 --- a/pr2eus/test/pr2-ri-test-simple.l +++ b/pr2eus/test/pr2-ri-test-simple.l @@ -131,9 +131,9 @@ (format nil "*ri* ~A, *pr2* ~A ~A~%" (elt (send *ri* :state :potentio-vector) 5) (elt (send *pr2* :angle-vector) 5) - (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0))) + (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0 1.0))) (warning-message 2 msg) - (assert (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0) msg) + (assert (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0 1.0) msg) ) )) @@ -149,9 +149,9 @@ (format nil "*ri* ~A, *pr2* ~A ~A~%" (elt (send *ri* :state :potentio-vector) 5) (elt (send *pr2* :angle-vector) 5) - (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0))) + (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0 1.0))) (warning-message 2 msg) - (assert (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0) msg) + (assert (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0 1.0) msg) ) )) From 7264b64fbcf30291b025028e4959ca2a5d48fac7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:11:50 +0900 Subject: [PATCH 08/13] pr2-ri-test-simple.l: wait 1 sec before :go-waitp --- pr2eus/test/pr2-ri-test-simple.l | 1 + 1 file changed, 1 insertion(+) diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l index 66d9b2376..d30e0a662 100644 --- a/pr2eus/test/pr2-ri-test-simple.l +++ b/pr2eus/test/pr2-ri-test-simple.l @@ -55,6 +55,7 @@ (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)") (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)") + (ros::sleep 1) ;; wait for 1 sec to activate goal status... (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)") (assert (send *ri* :go-wait) "(send *ri* :go-wait)") (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0))) From baf3a0c1d82fbb897a7fb4bd166811cb4946b7e8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:13:08 +0900 Subject: [PATCH 09/13] check if move-base-trajectory-action exists and invalid joint-names --- pr2eus/robot-interface.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 35ae8138f..787192af9 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -728,6 +728,10 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (if (and warningp (not (yes-or-no-p (format nil "~C[3~CmAre you sure to move the real robot? (~A) ~C[0m" #x1b 49 (send action :name) #x1b)))) (return-from :send-ros-controller nil)) + (dolist (name joint-names) + (unless (send robot :joint name) + (warning-message 1 "[robot-interface.l] (send-ros-controller) could not find joint-name '~A' (~A)~%" name (send robot :joint name)) + (return-from :send-ros-controller nil))) (let* ((goal (send action :make-goal-instance)) (goal-points nil) (st (if (numberp starttime) @@ -1487,7 +1491,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (cond ((or (equal (send move-base-action :get-state) ros::*simple-goal-state-active*) - (equal (send move-base-trajectory-action :get-state) ros::*simple-goal-state-active*)) + (and move-base-trajectory-action (equal (send move-base-trajectory-action :get-state) ros::*simple-goal-state-active*))) (return-from :go-waitp t)) (t (return-from :go-waitp nil)))) From 5a496e7cacb6f8ddac4b29b8bc5db911413204cc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:13:23 +0900 Subject: [PATCH 10/13] check if tms and avs are same length --- pr2eus/pr2-interface.l | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l index 303735aae..a8dac55a5 100644 --- a/pr2eus/pr2-interface.l +++ b/pr2eus/pr2-interface.l @@ -429,6 +429,13 @@ Example: (send self :gripper :rarm :position) => 0.00" (max-av (fill (instantiate float-vector len-av) 180)) (min-av (fill (instantiate float-vector len-av) -180)) diff-av (l 0) dist div) + (if (atom tms) (setq tms (make-list (length avs) :initial-element tms))) + (cond + ((< (length tms) (length avs)) + (nconc tms (make-list (- (length avs) (length tms)) :initial-element (car (last tms))))) + ((> (length tms) (length avs)) + (ros::ros-warn "length of tms should be the same or smaller than avs") + (setq tms (subseq tms 0 (length avs))))) (dolist (av avs) ;; check if joint move more than 180 degree which has -inf/inf limit (setq diff-av (v- av prev-av)) From b99d8ec585369f64aca9576a8e677047e1b7c9d7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 8 Apr 2020 22:16:04 +0900 Subject: [PATCH 11/13] add test to check :go-pos-unsafe-no-wait --- pr2eus/test/pr2-ri-test-simple.l | 1 + 1 file changed, 1 insertion(+) diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l index d30e0a662..1305687b8 100644 --- a/pr2eus/test/pr2-ri-test-simple.l +++ b/pr2eus/test/pr2-ri-test-simple.l @@ -59,6 +59,7 @@ (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)") (assert (send *ri* :go-wait) "(send *ri* :go-wait)") (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0))) + (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)") )) (deftest test-move-to From 1909302b640d88ce65170bf03435ff8aea445719 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Tue, 31 Mar 2020 22:39:45 +0900 Subject: [PATCH 12/13] move shortest-angle from pr2-ri-test-base.l to robot-interface.l --- pr2eus/robot-interface.l | 3 +++ pr2eus/test/pr2-ri-test-base.l | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 787192af9..04d3a4f2a 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -10,6 +10,9 @@ (ros::roseus-add-msgs "nav_msgs") (require :speak "package://pr2eus/speak.l") +(defun shortest-angle (d0 d1) + (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) + ;; add ros-joint-angle method using meter/radian (defmethod rotational-joint (:ros-joint-angle diff --git a/pr2eus/test/pr2-ri-test-base.l b/pr2eus/test/pr2-ri-test-base.l index adf027bce..b1a95d4fa 100644 --- a/pr2eus/test/pr2-ri-test-base.l +++ b/pr2eus/test/pr2-ri-test-base.l @@ -9,9 +9,6 @@ (substringp "violate max-angle" format)) (apply #'warning-message-org color format mesg))) -(defun shortest-angle (d0 d1) - (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) - ;; initialize *pr2* (setq *pr2* (pr2)) From d104b8996aa33c7c5f63379df700b45059a57a71 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 12 Apr 2020 19:23:31 +0900 Subject: [PATCH 13/13] [pr2eus_moveit] Add test for collision-object-publisher --- pr2eus_moveit/test/test-pr2eus-moveit.l | 32 +++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/pr2eus_moveit/test/test-pr2eus-moveit.l b/pr2eus_moveit/test/test-pr2eus-moveit.l index f44ca5a79..9578c2905 100755 --- a/pr2eus_moveit/test/test-pr2eus-moveit.l +++ b/pr2eus_moveit/test/test-pr2eus-moveit.l @@ -263,6 +263,38 @@ (assert (< tm-diff 7) (format nil "start-offset-time is considered multiple times. Traj finishes at ~A" tm-diff)) )) +;; send target coords between blocks +(deftest test-collision-object-publisher () + (let ((l (make-cube 500 100 500)) + (r (make-cube 500 100 500)) + (co (instance collision-object-publisher :init)) + (collision-check-result nil)) + (send *ri* :angle-vector (send *pr2* :reset-pose)) + (send *ri* :wait-interpolation) + (send l :locate #f(800 -200 700) :world) + (send r :locate #f(800 200 700) :world) + (send co :wipe-all) + (send co :add-object l) + (send co :add-object r) + ;; move left arm between blocks + (send *ri* :move-end-coords-plan (make-coords :pos #f(700 0 700)) :move-arm :larm) + (ros::rate 10) + (while (not (send *ri* :interpolatingp)) + (send *ri* :spin-once) + (ros::sleep)) + ;; check collision during interpolation + (while (send *ri* :interpolatingp) + (send *ri* :spin-once) + (send *pr2* :angle-vector + (send *ri* :state :potentio-vector :wait-until-update t)) + (setq collision-check-result + (or collision-check-result + (pqp-collision-check-objects (send *pr2* :links) (list l r)))) + (ros::sleep)) + (ros::ros-info "collision occurred? -> ~A~%" collision-check-result) + (assert (not collision-check-result) "Collision occurred between pr2 and cubes") + )) + (run-all-tests) (exit)