From 768a8ebf21cf350896383b1c8b5e3c75b6174b52 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sun, 23 Apr 2023 18:28:17 +0900 Subject: [PATCH] fix baxter head end-coords --- euslisp/lib/baxter-vr-interface.l | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/euslisp/lib/baxter-vr-interface.l b/euslisp/lib/baxter-vr-interface.l index af3744d..525bc3c 100644 --- 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))