Fix sending angle-vector

jsk-ros-pkg · Nov 12, 2019 · 1be9ba6 · 1be9ba6
1 parent ff04eb4
commit 1be9ba6
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/elevator_move_base_pr2/sample/fetch-sandwich.l b/elevator_move_base_pr2/sample/fetch-sandwich.l
@@ -21,7 +21,8 @@
         *ri* (instance pr2-interface :init
                        :move-base-action-name "elevator_move_base"))
   (pr2-tuckarm-pose)
-  (send *ri* :angle-vector (send *pr2* :head :angle-vector #f(0 0)) 1000)
+  (send *pr2* :head :angle-vector #f(0 0))
+  (send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
   (send *ri* :wait-interpolation)
   (when create-viewer (objects (list *pr2*))))
 
@@ -165,7 +166,8 @@
     (send *ri* :angle-vector-sequence (list av0 av1) (list 3000 1000))
     (send *ri* :wait-interpolation)
     (pr2-tuckarm-pose)
-    (send *ri* :angle-vector (send *pr2* :head :angle-vector #f(0 0)) 1000)
+    (send *pr2* :head :angle-vector #f(0 0))
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
     (send *ri* :wait-interpolation)
     (update-eus-robot-model)
   ))
@@ -213,7 +215,8 @@
   (send *ri* :wait-interpolation)
 
   (pr2-tuckarm-pose :rarm :outside)
-  (send *ri* :angle-vector (send *pr2* :head :angle-vector #f(0 0)) 1000)
+  (send *pr2* :head :angle-vector #f(0 0))
+  (send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
   (send *ri* :wait-interpolation)
 
   (send *ri* :go-pos-unsafe 0 0 180)