fix baxter head end-coords

knorth55 · Apr 23, 2023 · 768a8eb · 768a8eb
1 parent d17dd26
commit 768a8eb
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/euslisp/lib/baxter-vr-interface.l b/euslisp/lib/baxter-vr-interface.l
@@ -70,7 +70,9 @@
     (let (coords)
       (send tfl :wait-for-transform base-frame-id head-frame-id (ros::time 0) 0.1)
       (setq coords (send tfl :lookup-transform base-frame-id head-frame-id (ros::time 0)))
-      (if coords (send coords :rotate pi/2 :y :world))
+      (if coords
+        (setq coords (make-coords :pos (send coords :worldpos)
+                                  :rpy (float-vector 0 pi/2 0))))
       coords))
   (:angle-vector (&rest args)
     (send* robot-interface :angle-vector-raw args))