diff --git a/elevator_move_base_pr2/sample/fetch-sandwich.l b/elevator_move_base_pr2/sample/fetch-sandwich.l index 4c9a605647..61088ef794 100755 --- 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)