Update motion planners post check to only use continuous contact checking

Author: Levi-Armstrong

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/ompl/impl/ompl_freespace_planner.hpp

@@ -128,21 +128,7 @@ tesseract_common::StatusCode OMPLFreespacePlanner<PlannerType>::solve(PlannerRes
                                                       tesseract_collision::ContactTestType::FIRST,
                                                       verbose);
 
-  // Do a discrete check until continuous collision checking is updated to do dynamic-dynamic checking
-  discrete_contact_manager_->setContactDistanceThreshold(0);
-  collisions.clear();
-
-  found = found || tesseract_environment::checkTrajectory(collisions,
-                                                          *discrete_contact_manager_,
-                                                          *state_solver,
-                                                          kin_->getJointNames(),
-                                                          traj,
-                                                          config_->longest_valid_segment_length,
-                                                          tesseract_collision::ContactTestType::FIRST,
-                                                          verbose);
-
   // Set the contact distance back to original incase solve was called again.
-  discrete_contact_manager_->setContactDistanceThreshold(config_->collision_safety_margin);
   continuous_contact_manager_->setContactDistanceThreshold(config_->collision_safety_margin);
 
   // Send response

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp

@@ -174,22 +174,6 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(PlannerResponse& respon
                                tesseract_collision::ContactTestType::FIRST,
                                verbose);
 
-  // Do a discrete check until continuous collision checking is updated to do dynamic-dynamic checking
-  tesseract_collision::DiscreteContactManager::Ptr discrete_manager =
-      config_->prob->GetEnv()->getDiscreteContactManager();
-  discrete_manager->setActiveCollisionObjects(adjacency_map->getActiveLinkNames());
-  discrete_manager->setContactDistanceThreshold(0);
-  collisions.clear();
-
-  found = found || checkTrajectory(collisions,
-                                   *discrete_manager,
-                                   *state_solver,
-                                   config_->prob->GetKin()->getJointNames(),
-                                   getTraj(opt.x(), config_->prob->GetVars()),
-                                   length,
-                                   tesseract_collision::ContactTestType::FIRST,
-                                   verbose);
-
   // Send response
   response.joint_trajectory.trajectory = getTraj(opt.x(), config_->prob->GetVars());
   response.joint_trajectory.joint_names = config_->prob->GetKin()->getJointNames();