From 061c15aaaed33b45ecbe1db8c298016578e2a614 Mon Sep 17 00:00:00 2001 From: soonhyo Date: Thu, 5 Dec 2024 15:25:06 +0900 Subject: [PATCH 1/4] matched the center of primitive cylinder between eus and moveit --- pr2eus_moveit/euslisp/collision-object-publisher.l | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index 21c5e3ba..af62d869 100644 --- 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))) From 7a26e6b410bdcc19905332344c3a0c6f229b0e33 Mon Sep 17 00:00:00 2001 From: soonhyo Date: Fri, 6 Dec 2024 12:29:24 +0900 Subject: [PATCH 2/4] removed overused let --- pr2eus_moveit/euslisp/collision-object-publisher.l | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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))) From 79435541312e2b637f4326a18ecd17769d4a8757 Mon Sep 17 00:00:00 2001 From: soonhyo Date: Fri, 6 Dec 2024 12:37:41 +0900 Subject: [PATCH 3/4] add comment --- pr2eus_moveit/euslisp/collision-object-publisher.l | 1 + 1 file changed, 1 insertion(+) diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index c34b1024..b55c3253 100644 --- a/pr2eus_moveit/euslisp/collision-object-publisher.l +++ b/pr2eus_moveit/euslisp/collision-object-publisher.l @@ -78,6 +78,7 @@ :dimensions (float-vector (/ (height-of-cylinder obj) 1000.0) (/ (radius-of-cylinder obj) 1000.0)))))) + ;; Adjust the pose in the message to align EusLisp cylinder (origin at base) with MoveIt cylinder (origin at center). (send colobj :primitive_poses (append pose (list From d562110fdcd73f69333849e36551e9baf41368e3 Mon Sep 17 00:00:00 2001 From: soonhyo Date: Fri, 6 Dec 2024 12:41:51 +0900 Subject: [PATCH 4/4] changed indent and position of comment --- pr2eus_moveit/euslisp/collision-object-publisher.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index b55c3253..fe11bd81 100644 --- a/pr2eus_moveit/euslisp/collision-object-publisher.l +++ b/pr2eus_moveit/euslisp/collision-object-publisher.l @@ -78,7 +78,6 @@ :dimensions (float-vector (/ (height-of-cylinder obj) 1000.0) (/ (radius-of-cylinder obj) 1000.0)))))) - ;; Adjust the pose in the message to align EusLisp cylinder (origin at base) with MoveIt cylinder (origin at center). (send colobj :primitive_poses (append pose (list @@ -87,7 +86,8 @@ (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))))))))) + ;; Adjust the pose in the message to align EusLisp cylinder (origin at base) with MoveIt cylinder (origin at center). + (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)))