Improve descartes plan profile to include sample min, max and ik solv…

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_utils.h

@@ -38,39 +38,52 @@ namespace tesseract_planning
 using PoseSamplerFn = std::function<tesseract_common::VectorIsometry3d(const Eigen::Isometry3d& tool_pose)>;
 
 /**
- * @brief Given a tool pose create samples from [-PI, PI) around the provided axis.
+ * @brief Given a tool pose create samples from [minimum, maximum] around the provided axis.
  * @param tool_pose Tool pose to be sampled
- * @param resolution The resolution to sample at
  * @param axis The axis to sample around
+ * @param resolution The resolution to sample at
+ * @param minimum The minimum angle to start sampling at
+ * @param maximum The maximum angle to stop sampling at
  * @return A vector of tool poses
  */
 tesseract_common::VectorIsometry3d sampleToolAxis(const Eigen::Isometry3d& tool_pose,
+                                                  const Eigen::Vector3d& axis,
                                                   double resolution,
-                                                  const Eigen::Vector3d& axis);
+                                                  double minimum,
+                                                  double maximum);
 
 /**
- * @brief Given a tool pose create samples from [-PI, PI) around the x axis.
+ * @brief Given a tool pose create samples from [minimum, maximum] around the x axis.
  * @param tool_pose Tool pose to be sampled
  * @param resolution The resolution to sample at
+ * @param minimum The minimum angle to start sampling at
+ * @param maximum The maximum angle to stop sampling at
  * @return A vector of tool poses
  */
-tesseract_common::VectorIsometry3d sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution);
+tesseract_common::VectorIsometry3d
+sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum);
 
 /**
- * @brief Given a tool pose create samples from [-PI, PI) around the y axis.
+ * @brief Given a tool pose create samples from [minimum, maximum] around the y axis.
  * @param tool_pose Tool pose to be sampled
  * @param resolution The resolution to sample at
+ * @param minimum The minimum angle to start sampling at
+ * @param maximum The maximum angle to stop sampling at
  * @return A vector of tool poses
  */
-tesseract_common::VectorIsometry3d sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution);
+tesseract_common::VectorIsometry3d
+sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum);
 
 /**
- * @brief Given a tool pose create samples from [-PI, PI) around the z axis.
+ * @brief Given a tool pose create samples from [minimum, maximum] around the z axis.
  * @param tool_pose Tool pose to be sampled
  * @param resolution The resolution to sample at
+ * @param minimum The minimum angle to start sampling at
+ * @param maximum The maximum angle to stop sampling at
  * @return A vector of tool poses
  */
-tesseract_common::VectorIsometry3d sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution);
+tesseract_common::VectorIsometry3d
+sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum);
 
 /**
  * @brief This is the default sample with if a fixed pose sampler

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

@@ -89,12 +89,6 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
     assert(instruction.get().isMoveInstruction());
     const auto& move_instruction = instruction.get().template as<MoveInstructionPoly>();
 
-    // If plan instruction has manipulator information then use it over the one provided by the composite.
-    tesseract_common::ManipulatorInfo manip_info = composite_mi.getCombined(move_instruction.getManipulatorInfo());
-
-    if (manip_info.empty())
-      throw std::runtime_error("Descartes, manipulator info is empty!");
-
     // Get Plan Profile
     auto cur_plan_profile =
         getProfile<DescartesPlanProfile<FloatType>>(name_,
@@ -109,10 +103,10 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
         !move_instruction.getWaypoint().as<JointWaypointPoly>().isConstrained())
       continue;
 
-    waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, manip_info, request.env));
-    state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, manip_info, request.env));
+    waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, composite_mi, request.env));
+    state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, composite_mi, request.env));
     if (index != 0)
-      edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, manip_info, request.env));
+      edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, composite_mi, request.env));
 
     ++index;
   }

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp

@@ -69,18 +69,32 @@ PoseSamplerFn DescartesDefaultPlanProfile<FloatType>::createPoseSampler(
   if (target_pose_fixed)
     return sampleFixed;
 
-  return [axis = target_pose_sample_axis, resolution = target_pose_sample_resolution](
-             const Eigen::Isometry3d& tool_pose) { return sampleToolAxis(tool_pose, resolution, axis); };
+  return [axis = target_pose_sample_axis,
+          resolution = target_pose_sample_resolution,
+          minimum = target_pose_sample_min,
+          maximum = target_pose_sample_max](const Eigen::Isometry3d& tool_pose) {
+    return sampleToolAxis(tool_pose, axis, resolution, minimum, maximum);
+  };
 }
 
 template <typename FloatType>
 std::unique_ptr<descartes_light::WaypointSampler<FloatType>>
 DescartesDefaultPlanProfile<FloatType>::createWaypointSampler(
     const MoveInstructionPoly& move_instruction,
-    const tesseract_common::ManipulatorInfo& manip_info,
+    const tesseract_common::ManipulatorInfo& composite_manip_info,
     const std::shared_ptr<const tesseract_environment::Environment>& env) const
 {
+  // If plan instruction has manipulator information then use it over the one provided by the composite.
+  tesseract_common::ManipulatorInfo manip_info =
+      composite_manip_info.getCombined(move_instruction.getManipulatorInfo());
+  if (!manipulator_ik_solver.empty())
+    manip_info.manipulator_ik_solver = manipulator_ik_solver;
+
+  if (manip_info.empty())
+    throw std::runtime_error("Descartes, manipulator info is empty!");
+
   auto manip = DescartesPlanProfile<FloatType>::createKinematicGroup(manip_info, *env);
+
   if (!move_instruction.getWaypoint().isCartesianWaypoint())
   {
     assert(checkJointPositionFormat(manip->getJointNames(), move_instruction.getWaypoint()));
@@ -121,10 +135,20 @@ DescartesDefaultPlanProfile<FloatType>::createWaypointSampler(
 template <typename FloatType>
 std::unique_ptr<descartes_light::EdgeEvaluator<FloatType>> DescartesDefaultPlanProfile<FloatType>::createEdgeEvaluator(
     const MoveInstructionPoly& move_instruction,
-    const tesseract_common::ManipulatorInfo& manip_info,
+    const tesseract_common::ManipulatorInfo& composite_manip_info,
     const std::shared_ptr<const tesseract_environment::Environment>& env) const
 {
+  // If plan instruction has manipulator information then use it over the one provided by the composite.
+  tesseract_common::ManipulatorInfo manip_info =
+      composite_manip_info.getCombined(move_instruction.getManipulatorInfo());
+  if (!manipulator_ik_solver.empty())
+    manip_info.manipulator_ik_solver = manipulator_ik_solver;
+
+  if (manip_info.empty())
+    throw std::runtime_error("Descartes, manipulator info is empty!");
+
   auto manip = DescartesPlanProfile<FloatType>::createKinematicGroup(manip_info, *env);
+
   if (move_instruction.getWaypoint().isCartesianWaypoint())
   {
     if (!enable_edge_collision)
@@ -155,7 +179,7 @@ template <typename FloatType>
 std::unique_ptr<descartes_light::StateEvaluator<FloatType>>
 DescartesDefaultPlanProfile<FloatType>::createStateEvaluator(
     const MoveInstructionPoly& /*move_instruction*/,
-    const tesseract_common::ManipulatorInfo& /*manip_info*/,
+    const tesseract_common::ManipulatorInfo& /*composite_manip_info*/,
     const std::shared_ptr<const tesseract_environment::Environment>& /*env*/) const
 {
   return std::make_unique<descartes_light::StateEvaluator<FloatType>>();

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h

@@ -49,9 +49,15 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile<FloatType>
 
   DescartesDefaultPlanProfile() = default;
 
+  /** @brief Target pose sampling params */
   bool target_pose_fixed{ true };
   Eigen::Vector3d target_pose_sample_axis{ 0, 0, 1 };
   double target_pose_sample_resolution{ M_PI_2 };
+  double target_pose_sample_min{ -M_PI };
+  double target_pose_sample_max{ M_PI - M_PI_2 };  // Subtract resolution to avoid duplicate states at -PI and PI
+
+  /** @brief Override the manipulator ik solver */
+  std::string manipulator_ik_solver;
 
   /**
    * @brief Flag to indicate that collisions should not cause failures during state/edge evaluation
@@ -80,17 +86,17 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile<FloatType>
 
   std::unique_ptr<descartes_light::WaypointSampler<FloatType>>
   createWaypointSampler(const MoveInstructionPoly& move_instruction,
-                        const tesseract_common::ManipulatorInfo& manip_info,
+                        const tesseract_common::ManipulatorInfo& composite_manip_info,
                         const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
 
   std::unique_ptr<descartes_light::EdgeEvaluator<FloatType>>
   createEdgeEvaluator(const MoveInstructionPoly& move_instruction,
-                      const tesseract_common::ManipulatorInfo& manip_info,
+                      const tesseract_common::ManipulatorInfo& composite_manip_info,
                       const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
 
   std::unique_ptr<descartes_light::StateEvaluator<FloatType>>
   createStateEvaluator(const MoveInstructionPoly& move_instruction,
-                       const tesseract_common::ManipulatorInfo& manip_info,
+                       const tesseract_common::ManipulatorInfo& composite_manip_info,
                        const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
 
 protected:

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h

@@ -90,17 +90,17 @@ class DescartesPlanProfile : public Profile
 
   virtual std::unique_ptr<descartes_light::WaypointSampler<FloatType>>
   createWaypointSampler(const MoveInstructionPoly& move_instruction,
-                        const tesseract_common::ManipulatorInfo& manip_info,
+                        const tesseract_common::ManipulatorInfo& composite_manip_info,
                         const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
 
   virtual std::unique_ptr<descartes_light::EdgeEvaluator<FloatType>>
   createEdgeEvaluator(const MoveInstructionPoly& move_instruction,
-                      const tesseract_common::ManipulatorInfo& manip_info,
+                      const tesseract_common::ManipulatorInfo& composite_manip_info,
                       const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
 
   virtual std::unique_ptr<descartes_light::StateEvaluator<FloatType>>
   createStateEvaluator(const MoveInstructionPoly& move_instruction,
-                       const tesseract_common::ManipulatorInfo& manip_info,
+                       const tesseract_common::ManipulatorInfo& composite_manip_info,
                        const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
 
 protected:

tesseract_motion_planners/descartes/src/descartes_utils.cpp

@@ -29,34 +29,39 @@
 namespace tesseract_planning
 {
 tesseract_common::VectorIsometry3d sampleToolAxis(const Eigen::Isometry3d& tool_pose,
+                                                  const Eigen::Vector3d& axis,
                                                   double resolution,
-                                                  const Eigen::Vector3d& axis)
+                                                  double minimum,
+                                                  double maximum)
 {
   tesseract_common::VectorIsometry3d samples;
-  auto cnt = static_cast<int>(std::ceil(2.0 * M_PI / resolution)) + 1;
-  Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(cnt, -M_PI, M_PI);
-  samples.reserve(static_cast<size_t>(angles.size()) - 1UL);
-  for (long i = 0; i < static_cast<long>(angles.size() - 1); ++i)
+  auto cnt = static_cast<int>(std::ceil((maximum - minimum) / resolution)) + 1;
+  Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(cnt, minimum, maximum);
+  samples.reserve(static_cast<size_t>(angles.size()));
+  for (long i = 0; i < static_cast<long>(angles.size()); ++i)
   {
     Eigen::Isometry3d p = tool_pose * Eigen::AngleAxisd(angles(i), axis);
     samples.push_back(p);
   }
   return samples;
 }
 
-tesseract_common::VectorIsometry3d sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution)
+tesseract_common::VectorIsometry3d
+sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum)
 {
-  return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitX());  // NOLINT
+  return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitX(), resolution, minimum, maximum);  // NOLINT
 }
 
-tesseract_common::VectorIsometry3d sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution)
+tesseract_common::VectorIsometry3d
+sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum)
 {
-  return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitY());  // NOLINT
+  return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitY(), resolution, minimum, maximum);  // NOLINT
 }
 
-tesseract_common::VectorIsometry3d sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution)
+tesseract_common::VectorIsometry3d
+sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum)
 {
-  return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitZ());  // NOLINT
+  return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitZ(), resolution, minimum, maximum);  // NOLINT
 }
 
 tesseract_common::VectorIsometry3d sampleFixed(const Eigen::Isometry3d& tool_pose)

tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp

@@ -45,6 +45,9 @@ void DescartesDefaultPlanProfile<FloatType>::serialize(Archive& ar, const unsign
   ar& BOOST_SERIALIZATION_NVP(target_pose_fixed);
   ar& BOOST_SERIALIZATION_NVP(target_pose_sample_axis);
   ar& BOOST_SERIALIZATION_NVP(target_pose_sample_resolution);
+  ar& BOOST_SERIALIZATION_NVP(target_pose_sample_min);
+  ar& BOOST_SERIALIZATION_NVP(target_pose_sample_max);
+  ar& BOOST_SERIALIZATION_NVP(manipulator_ik_solver);
   ar& BOOST_SERIALIZATION_NVP(allow_collision);
   ar& BOOST_SERIALIZATION_NVP(enable_collision);
   ar& BOOST_SERIALIZATION_NVP(vertex_collision_check_config);