From 2558300d10f385c12ecb0e6eb002ef105a3927d6 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Mon, 21 Jun 2021 22:17:59 -0400 Subject: [PATCH] Fix goal position tolerances from box primitive Converting a box primitive dimension in absolute tolerances results in doubling the tolerances wrt. the pose of the BOX. --- stomp_moveit/src/utils/kinematics.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/stomp_moveit/src/utils/kinematics.cpp b/stomp_moveit/src/utils/kinematics.cpp index 275207d..6304cf3 100644 --- a/stomp_moveit/src/utils/kinematics.cpp +++ b/stomp_moveit/src/utils/kinematics.cpp @@ -366,9 +366,9 @@ bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model,const move } using SP = shape_msgs::SolidPrimitive; - tolerance[0] = bv.dimensions[SP::BOX_X]; - tolerance[1] = bv.dimensions[SP::BOX_Y]; - tolerance[2] = bv.dimensions[SP::BOX_Z]; + tolerance[0] = bv.dimensions[SP::BOX_X] / 2.0; + tolerance[1] = bv.dimensions[SP::BOX_Y] / 2.0; + tolerance[2] = bv.dimensions[SP::BOX_Z] / 2.0; } break; @@ -497,9 +497,9 @@ EigenSTL::vector_Affine3d sampleCartesianPoses(const moveit_msgs::Constraints& c } using SP = shape_msgs::SolidPrimitive; - tolerances[0] = bv.dimensions[SP::BOX_X]; - tolerances[1] = bv.dimensions[SP::BOX_Y]; - tolerances[2] = bv.dimensions[SP::BOX_Z]; + tolerances[0] = bv.dimensions[SP::BOX_X] / 2.0; + tolerances[1] = bv.dimensions[SP::BOX_Y] / 2.0; + tolerances[2] = bv.dimensions[SP::BOX_Z] / 2.0; } break;