Merge pull request #35 from captain-yoshi/fix-goal-tolerance

Fix goal position tolerances from box primitive
ros-industrial · Oct 20, 2021 · 30bacd1 · 30bacd1
2 parents 599ccf8 + 2558300
commit 30bacd1
Showing 1 changed file with 6 additions and 6 deletions.
diff --git 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;