Add MoveWaypointFromCollisionRandomSampler to FixStateCollisionProcess (#426)

* Add MoveWaypointFromCollisionRandomSampler to FixStateCollisionProcess

* Add more generalized way of specifying correction methods

* Bug fix

* Add assert to catch mismatched sizes

* Rebase fixes and a bug fix

Author: mpowelson

tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_collision_process_generator.h

@@ -48,22 +48,39 @@ struct FixStateCollisionProfile
     DISABLED
   };
 
+  /** @brief Used to specify method used to correct states in collision */
+  enum class CorrectionMethod
+  {
+    NONE,
+    TRAJOPT,
+    RANDOM_SAMPLER
+  };
+
   FixStateCollisionProfile(Settings mode = Settings::ALL) : mode(mode) {}
 
   /** @brief Sets which terms will be corrected  */
   Settings mode;
 
+  /** @brief Order that correction methods will be applied. These will be attempted in order until one succeeds or all
+   * have been tried */
+  std::vector<CorrectionMethod> correction_workflow{ CorrectionMethod::TRAJOPT, CorrectionMethod::RANDOM_SAMPLER };
+
   /** @brief Percent of the total joint range that a joint will be allowed to be adjusted */
   double jiggle_factor{ 0.02 };
 
   /** @brief Safety margin applied to collision costs/cnts when using trajopt to correct collisions */
   double safety_margin{ 0.025 };
+
+  /** @brief Number of sampling attempts if TrajOpt correction fails*/
+  int sampling_attempts{ 100 };
 };
 using FixStateCollisionProfileMap = std::unordered_map<std::string, FixStateCollisionProfile::Ptr>;
 
 /**
  * @brief This generator modifies the const input instructions in order to push waypoints that are in collision out of
- * collision
+ * collision.
+ *
+ * First it uses TrajOpt to correct the waypoint. If that fails, it reverts to random sampling
  */
 class FixStateCollisionProcessGenerator : public ProcessGenerator
 {
@@ -100,5 +117,47 @@ class FixStateCollisionProcessGenerator : public ProcessGenerator
 
   void process(ProcessInput input) const;
 };
+
+/**
+ * @brief Checks if a joint state is in collision
+ * @param start_pos Vector that represents a joint state
+ * @param input Process Input associated with waypoint. Needed for kinematics, etc.
+ * @return True if in collision
+ */
+bool StateInCollision(const Eigen::Ref<const Eigen::VectorXd>& start_pos,
+                      const ProcessInput& input,
+                      const FixStateCollisionProfile& profile);
+
+/**
+ * @brief Checks if a waypoint is in collision
+ * @param waypoint Must be a waypoint for which getJointPosition will return a position
+ * @param input Process Input associated with waypoint. Needed for kinematics, etc.
+ * @return True if in collision
+ */
+bool WaypointInCollision(const Waypoint& waypoint, const ProcessInput& input, const FixStateCollisionProfile& profile);
+
+/**
+ * @brief Takes a waypoint and uses a small trajopt problem to push it out of collision if necessary
+ * @param waypoint Must be a waypoint for which getJointPosition will return a position
+ * @param input Process Input associated with waypoint. Needed for kinematics, etc.
+ * @param profile Profile containing needed params
+ * @return True if successful
+ */
+bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint,
+                                      const ProcessInput& input,
+                                      const FixStateCollisionProfile& profile);
+
+/**
+ * @brief Takes a waypoint and uses random sampling to find a position that is out of collision
+ * @param waypoint Must be a waypoint for which getJointPosition will return a position
+ * @param input Process Input associated with waypoint. Needed for kinematics, etc.
+ * @param profile Profile containing needed params
+ * @return True if successful
+ */
+bool MoveWaypointFromCollisionRandomSampler(Waypoint& waypoint,
+                                            const ProcessInput& input,
+                                            const FixStateCollisionProfile& profile);
+
+bool ApplyCorrectionWorkflow(Waypoint& waypoint, const ProcessInput& input, const FixStateCollisionProfile& profile);
 }  // namespace tesseract_planning
 #endif  // TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_PROCESS_GENERATOR_H

tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_collision_process_generator.cpp

@@ -37,13 +37,50 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-/**
- * @brief Checks if a waypoint is in collision
- * @param waypoint Must be a waypoint for which getJointPosition will return a position
- * @param input Process Input associated with waypoint. Needed for kinematics, etc.
- * @return True if in collision
- */
-bool WaypointInCollision(const Waypoint& waypoint, const ProcessInput& input)
+bool StateInCollision(const Eigen::Ref<const Eigen::VectorXd>& start_pos,
+                      const ProcessInput& input,
+                      const FixStateCollisionProfile& profile)
+{
+  using namespace tesseract_collision;
+  using namespace tesseract_environment;
+
+  auto env = input.tesseract->getEnvironment();
+  auto kin = input.tesseract->getManipulatorManager()->getFwdKinematicSolver(input.manip_info.manipulator);
+
+  std::vector<ContactResultMap> collisions;
+  tesseract_environment::StateSolver::Ptr state_solver = env->getStateSolver();
+  DiscreteContactManager::Ptr manager = env->getDiscreteContactManager();
+  AdjacencyMap::Ptr adjacency_map = std::make_shared<tesseract_environment::AdjacencyMap>(
+      env->getSceneGraph(), kin->getActiveLinkNames(), env->getCurrentState()->link_transforms);
+
+  manager->setActiveCollisionObjects(adjacency_map->getActiveLinkNames());
+  manager->setContactDistanceThreshold(profile.safety_margin);
+  collisions.clear();
+
+  tesseract_common::TrajArray traj(1, start_pos.size());
+  traj.row(0) = start_pos.transpose();
+
+  // This never returns any collisions
+  if (!checkTrajectory(collisions, *manager, *state_solver, kin->getJointNames(), traj))
+  {
+    CONSOLE_BRIDGE_logDebug("No collisions found");
+    return false;
+  }
+  else
+  {
+    CONSOLE_BRIDGE_logDebug("Waypoint is not contact free!");
+    for (std::size_t i = 0; i < collisions.size(); i++)
+      for (const auto& contact_vec : collisions[i])
+        for (const auto& contact : contact_vec.second)
+          CONSOLE_BRIDGE_logDebug(("timestep: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " +
+                                   contact.link_names[1] + " Dist: " + std::to_string(contact.distance))
+                                      .c_str());
+  }
+
+  return true;
+}
+
+bool WaypointInCollision(const Waypoint& waypoint, const ProcessInput& input, const FixStateCollisionProfile& profile)
 {
   // Get position associated with waypoint
   Eigen::VectorXd start_pos;
@@ -56,56 +93,9 @@ bool WaypointInCollision(const Waypoint& waypoint, const ProcessInput& input)
     CONSOLE_BRIDGE_logError("WaypointInCollision error: %s", e.what());
     return false;
   }
-
-  // Check if there are collisions
-  {
-    using namespace tesseract_collision;
-    using namespace tesseract_environment;
-
-    auto env = input.tesseract->getEnvironment();
-    auto kin = input.tesseract->getManipulatorManager()->getFwdKinematicSolver(input.manip_info.manipulator);
-
-    std::vector<ContactResultMap> collisions;
-    tesseract_environment::StateSolver::Ptr state_solver = env->getStateSolver();
-    DiscreteContactManager::Ptr manager = env->getDiscreteContactManager();
-    AdjacencyMap::Ptr adjacency_map = std::make_shared<tesseract_environment::AdjacencyMap>(
-        env->getSceneGraph(), kin->getActiveLinkNames(), env->getCurrentState()->link_transforms);
-
-    manager->setActiveCollisionObjects(adjacency_map->getActiveLinkNames());
-    manager->setContactDistanceThreshold(0);
-    collisions.clear();
-
-    tesseract_common::TrajArray traj(1, start_pos.size());
-    traj.row(0) = start_pos.transpose();
-
-    // This never returns any collisions
-    if (!checkTrajectory(collisions, *manager, *state_solver, kin->getJointNames(), traj))
-    {
-      CONSOLE_BRIDGE_logDebug("No collisions found");
-      return false;
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logDebug("Waypoint is not contact free!");
-      for (std::size_t i = 0; i < collisions.size(); i++)
-        for (const auto& contact_vec : collisions[i])
-          for (const auto& contact : contact_vec.second)
-            CONSOLE_BRIDGE_logDebug(("timestep: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " +
-                                     contact.link_names[1] + " Dist: " + std::to_string(contact.distance))
-                                        .c_str());
-    }
-  }
-
-  return true;
+  return StateInCollision(start_pos, input, profile);
 }
 
-/**
- * @brief Takes a waypoint and uses a small trajopt problem to push it out of collision if necessary
- * @param waypoint Must be a waypoint for which getJointPosition will return a position
- * @param input Process Input associated with waypoint. Needed for kinematics, etc.
- * @param profile Profile containing needed params
- * @return True if successful
- */
 bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint,
                                       const ProcessInput& input,
                                       const FixStateCollisionProfile& profile)
@@ -199,6 +189,70 @@ bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint,
   return setJointPosition(waypoint, results);
 }
 
+bool MoveWaypointFromCollisionRandomSampler(Waypoint& waypoint,
+                                            const ProcessInput& input,
+                                            const FixStateCollisionProfile& profile)
+{
+  // Get position associated with waypoint
+  Eigen::VectorXd start_pos;
+  try
+  {
+    start_pos = getJointPosition(waypoint);
+  }
+  catch (std::runtime_error& e)
+  {
+    CONSOLE_BRIDGE_logError("MoveWaypointFromCollision error: %s", e.what());
+    return false;
+  }
+
+  const auto kin = input.tesseract->getManipulatorManager()->getFwdKinematicSolver(input.manip_info.manipulator);
+  Eigen::MatrixXd limits = kin->getLimits().joint_limits;
+  Eigen::VectorXd range = limits.col(1).array() - limits.col(0).array();
+  Eigen::VectorXd pos_sampling_limits = range * profile.jiggle_factor;
+  Eigen::VectorXd neg_sampline_limits = range * -profile.jiggle_factor;
+
+  assert(start_pos.size() == range.size());
+  for (int i = 0; i < profile.sampling_attempts; i++)
+  {
+    Eigen::VectorXd start_sampled_pos =
+        start_pos + Eigen::VectorXd::Random(start_pos.size()).cwiseProduct(range) * profile.jiggle_factor;
+
+    // Make sure it doesn't violate joint limits
+    Eigen::VectorXd sampled_pos = start_sampled_pos;
+    sampled_pos = sampled_pos.cwiseMax(limits.col(0));
+    sampled_pos = sampled_pos.cwiseMin(limits.col(1));
+
+    if (!StateInCollision(sampled_pos, input, profile))
+    {
+      return setJointPosition(waypoint, sampled_pos);
+    }
+  }
+
+  return false;
+}
+
+bool ApplyCorrectionWorkflow(Waypoint& waypoint, const ProcessInput& input, const FixStateCollisionProfile& profile)
+{
+  for (const auto& method : profile.correction_workflow)
+  {
+    switch (method)
+    {
+      case FixStateCollisionProfile::CorrectionMethod::NONE:
+        return false;  // No correction and in collision, so return false
+      case FixStateCollisionProfile::CorrectionMethod::TRAJOPT:
+        if (MoveWaypointFromCollisionTrajopt(waypoint, input, profile))
+          return true;
+        break;
+      case FixStateCollisionProfile::CorrectionMethod::RANDOM_SAMPLER:
+        if (MoveWaypointFromCollisionRandomSampler(waypoint, input, profile))
+          return true;
+        break;
+    }
+  }
+  // If all methods have tried without returning, then correction failed
+  return false;
+}
+
 FixStateCollisionProcessGenerator::FixStateCollisionProcessGenerator(std::string name) : name_(std::move(name))
 {
   // Register default profile
@@ -268,10 +322,10 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input) co
       if (instr_const_ptr)
       {
         PlanInstruction* mutable_instruction = const_cast<PlanInstruction*>(instr_const_ptr);
-        if (WaypointInCollision(mutable_instruction->getWaypoint(), input))
+        if (WaypointInCollision(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
         {
           CONSOLE_BRIDGE_logInform("FixStateCollisionProcessGenerator is modifying the const input instructions");
-          if (!MoveWaypointFromCollisionTrajopt(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
+          if (!ApplyCorrectionWorkflow(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
             return 0;
         }
       }
@@ -283,10 +337,10 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input) co
       if (instr_const_ptr)
       {
         PlanInstruction* mutable_instruction = const_cast<PlanInstruction*>(instr_const_ptr);
-        if (WaypointInCollision(mutable_instruction->getWaypoint(), input))
+        if (WaypointInCollision(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
         {
           CONSOLE_BRIDGE_logInform("FixStateCollisionProcessGenerator is modifying the const input instructions");
-          if (!MoveWaypointFromCollisionTrajopt(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
+          if (!ApplyCorrectionWorkflow(mutable_instruction->getWaypoint(), input, *cur_composite_profile))
             return 0;
         }
       }
@@ -304,7 +358,8 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input) co
       bool in_collision = false;
       for (const auto& instruction : flattened)
       {
-        in_collision |= WaypointInCollision(instruction.get().cast_const<PlanInstruction>()->getWaypoint(), input);
+        in_collision |= WaypointInCollision(
+            instruction.get().cast_const<PlanInstruction>()->getWaypoint(), input, *cur_composite_profile);
       }
       if (!in_collision)
         break;
@@ -315,7 +370,8 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input) co
         const Instruction* instr_const_ptr = &instruction.get();
         Instruction* mutable_instruction = const_cast<Instruction*>(instr_const_ptr);
         PlanInstruction* plan = mutable_instruction->cast<PlanInstruction>();
-        if (!MoveWaypointFromCollisionTrajopt(plan->getWaypoint(), input, *cur_composite_profile))
+
+        if (!ApplyCorrectionWorkflow(plan->getWaypoint(), input, *cur_composite_profile))
           return 0;
       }
     }

tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt

@@ -43,3 +43,14 @@ target_code_coverage(${PROJECT_NAME}_unit ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE
 add_gtest_discover_tests(${PROJECT_NAME}_unit)
 add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME})
 add_dependencies(run_tests ${PROJECT_NAME}_unit)
+
+add_executable(${PROJECT_NAME}_fix_state_collision_process_generator_unit fix_state_collision_process_generator_unit.cpp)
+target_link_libraries(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE GTest::GTest GTest::Main tesseract::tesseract_support ${PROJECT_NAME})
+target_include_directories(${PROJECT_NAME}_fix_state_collision_process_generator_unit PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/examples>")
+target_compile_options(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS})
+target_clang_tidy(${PROJECT_NAME}_fix_state_collision_process_generator_unit ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
+target_cxx_version(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION})
+target_code_coverage(${PROJECT_NAME}_fix_state_collision_process_generator_unit ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING})
+add_gtest_discover_tests(${PROJECT_NAME}_fix_state_collision_process_generator_unit)
+add_dependencies(run_tests ${PROJECT_NAME}_fix_state_collision_process_generator_unit)