Update the OMPL validators to the parallel-array transform setter

Gather the active set into thread-local buffers and call the id-typed
parallel-array setter, restoring active-only and fail-loud semantics while
keeping the Coal/FCL batching. Replaces the whole-map call.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Author: rjoomen

motion_planners/ompl/src/continuous_motion_validator.cpp

@@ -149,7 +149,19 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State
   TESSERACT_THREAD_LOCAL tesseract::common::LinkIdTransformMap state1;
   manip_->calcFwdKin(state1, finish_joints);
 
-  cm->setCollisionObjectsTransform(state0, state1);
+  TESSERACT_THREAD_LOCAL tesseract::common::VectorIsometry3d poses0;
+  TESSERACT_THREAD_LOCAL tesseract::common::VectorIsometry3d poses1;
+  poses0.clear();
+  poses1.clear();
+  poses0.reserve(link_ids_.size());
+  poses1.reserve(link_ids_.size());
+  for (const auto& id : link_ids_)
+  {
+    poses0.push_back(state0.at(id));
+    poses1.push_back(state1.at(id));
+  }
+
+  cm->setCollisionObjectsTransform(link_ids_, poses0, poses1);
 
   tesseract::collision::ContactResultMap contact_map;
   cm->contactTest(contact_map, tesseract::collision::ContactTestType::FIRST);

motion_planners/ompl/src/state_collision_validator.cpp

@@ -77,7 +77,13 @@ bool StateCollisionValidator::isValid(const ompl::base::State* state) const
   TESSERACT_THREAD_LOCAL tesseract::common::LinkIdTransformMap state1;
   manip_->calcFwdKin(state1, finish_joints);
 
-  cm->setCollisionObjectsTransform(state1);
+  TESSERACT_THREAD_LOCAL tesseract::common::VectorIsometry3d poses;
+  poses.clear();
+  poses.reserve(link_ids_.size());
+  for (const auto& id : link_ids_)
+    poses.push_back(state1.at(id));
+
+  cm->setCollisionObjectsTransform(link_ids_, poses);
 
   tesseract::collision::ContactResultMap contact_map;
   cm->contactTest(contact_map, tesseract::collision::ContactTestType::FIRST);