diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index af62d869..c34b1024 100644 --- a/pr2eus_moveit/euslisp/collision-object-publisher.l +++ b/pr2eus_moveit/euslisp/collision-object-publisher.l @@ -81,12 +81,12 @@ (send colobj :primitive_poses (append pose (list - (ros::coords->tf-pose (let ((coords (if relative-pose - relative-pose - (send obj :worldcoords)))) - (let* ((hv (float-vector 0 0 (/ (height-of-cylinder obj) 2))) - (hv-t (send coords :transform-vector hv))) - (make-coords :pos hv-t :rot (send coords :rot)))))))))) + (ros::coords->tf-pose (let* ((coords (if relative-pose + relative-pose + (send obj :worldcoords))) + (hv (float-vector 0 0 (/ (height-of-cylinder obj) 2))) + (hv-t (send coords :transform-vector hv))) + (make-coords :pos hv-t :rot (send coords :rot))))))))) ((and (derivedp obj body) (eq (car (send obj :body-type)) :cube)) (let ((geom (send colobj :primitives)) (pose (send colobj :primitive_poses)))