Fixed issue checking wrong size for contact check results

tesseract-robotics · Mar 7, 2024 · 515db21 · 515db21
1 parent 0fae365
commit 515db21
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp
@@ -318,7 +318,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
     // Grab the first waypoint to get the joint names
     const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
     traj_contacts =
-        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(program.size()));
+        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(mi.size()));
   }
 
   contacts.clear();
@@ -625,7 +625,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
     // Grab the first waypoint to get the joint names
     const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
     traj_contacts =
-        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(program.size()));
+        std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names, static_cast<int>(mi.size()));
   }
 
   contacts.clear();