matched the center of primitive cylinder between eus and moveit

jsk-ros-pkg · Dec 5, 2024 · 061c15a · 061c15a
1 parent 16cb8fa
commit 061c15a
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l
@@ -81,7 +81,12 @@
          (send colobj :primitive_poses
                (append pose
                        (list
-                        (ros::coords->tf-pose (if relative-pose relative-pose (send obj :worldcoords))))))))
+                        (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))))))))))
       ((and (derivedp obj body) (eq (car (send obj :body-type)) :cube))
        (let ((geom (send colobj :primitives))
              (pose (send colobj :primitive_poses)))