Update descartes profiles

tesseract_motion_planners/descartes/CMakeLists.txt

@@ -8,11 +8,11 @@ add_library(
   src/descartes_collision.cpp
   src/descartes_collision_edge_evaluator.cpp
   src/descartes_robot_sampler.cpp
-  src/serialize.cpp
-  src/deserialize.cpp
+  src/descartes_solver_config.cpp
   src/descartes_utils.cpp
   src/profile/descartes_profile.cpp
-  src/profile/descartes_default_plan_profile.cpp)
+  src/profile/descartes_default_plan_profile.cpp
+  src/profile/descartes_default_solver_profile.cpp)
 target_link_libraries(
   ${PROJECT_NAME}_descartes
   PUBLIC ${PROJECT_NAME}_core

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h

@@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_environment/fwd.h>
 
 #include <tesseract_scene_graph/scene_state.h>
+#include <tesseract_motion_planners/descartes/descartes_solver_config.h>
 
 namespace tesseract_planning
 {
@@ -49,18 +50,11 @@ struct DescartesProblem
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   // LCOV_EXCL_STOP
 
-  // These are required for Tesseract to configure Descartes
-  std::shared_ptr<const tesseract_environment::Environment> env;
-  tesseract_scene_graph::SceneState env_state;
+  DescartesSolverConfig solver_config;
 
-  // Kinematic Objects
-  std::shared_ptr<const tesseract_kinematics::KinematicGroup> manip;
-
-  // These are required for descartes
   std::vector<typename descartes_light::EdgeEvaluator<FloatType>::ConstPtr> edge_evaluators{};
   std::vector<typename descartes_light::WaypointSampler<FloatType>::ConstPtr> samplers{};
   std::vector<typename descartes_light::StateEvaluator<FloatType>::ConstPtr> state_evaluators{};
-  int num_threads = static_cast<int>(std::thread::hardware_concurrency());
 };
 using DescartesProblemF = DescartesProblem<float>;
 using DescartesProblemD = DescartesProblem<double>;

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_solver_config.h

@@ -0,0 +1,49 @@
+/**
+ * @file descartes_solver_config.h
+ * @brief Tesseract Descartes Solver Config
+ *
+ * @author Levi Armstrong
+ * @date June 25, 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2020, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_SOLVER_CONFIG_H
+#define TESSERACT_MOTION_PLANNERS_DESCARTES_SOLVER_CONFIG_H
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
+
+namespace tesseract_planning
+{
+struct DescartesSolverConfig
+{
+  /** @brief Number of threads to use during planning */
+  int num_threads{ 1 };
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int);  // NOLINT
+};
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesSolverConfig)
+
+#endif  // TESSERACT_MOTION_PLANNERS_DESCARTES_SOLVER_CONFIG_H

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

@@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/descartes/descartes_motion_planner.h>
 #include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>
+#include <tesseract_motion_planners/descartes/profile/descartes_default_solver_profile.h>
 #include <tesseract_motion_planners/core/types.h>
 #include <tesseract_motion_planners/simple/interpolation.h>
 #include <tesseract_motion_planners/planner_utils.h>
@@ -85,7 +86,7 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
   descartes_light::SearchResult<FloatType> descartes_result;
   try
   {
-    descartes_light::LadderGraphSolver<FloatType> solver(problem->num_threads);
+    descartes_light::LadderGraphSolver<FloatType> solver(problem->solver_config.num_threads);
 
     // Build Graph
     if (!solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators))
@@ -125,8 +126,16 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
     return response;
   }
 
-  const std::vector<std::string> joint_names = problem->manip->getJointNames();
-  const Eigen::MatrixX2d joint_limits = problem->manip->getLimits().joint_limits;
+  assert(!request.instructions.getManipulatorInfo().empty());
+  const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo();
+
+  if (composite_mi.manipulator.empty())
+    throw std::runtime_error("Descartes, manipulator is empty!");
+
+  // Get Manipulator Information
+  tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(composite_mi.manipulator);
+  const std::vector<std::string> joint_names = manip->getJointNames();
+  const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits;
 
   // Enforce limits
   std::vector<Eigen::VectorXd> solution{};
@@ -233,98 +242,57 @@ DescartesMotionPlanner<FloatType>::createProblem(const PlannerRequest& request)
   prob->samplers.clear();
   prob->state_evaluators.clear();
 
-  // Assume all the plan instructions have the same manipulator as the composite
-  assert(!request.instructions.getManipulatorInfo().empty());
-  const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo();
-
-  if (composite_mi.manipulator.empty())
-    throw std::runtime_error("Descartes, manipulator is empty!");
-
-  // Get Manipulator Information
-  try
-  {
-    if (composite_mi.manipulator_ik_solver.empty())
-      prob->manip = request.env->getKinematicGroup(composite_mi.manipulator);
-    else
-      prob->manip = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver);
-  }
-  catch (...)
-  {
-    throw std::runtime_error("Descartes problem generator failed to create kinematic group!");
-  }
-
-  if (!prob->manip)
-  {
-    CONSOLE_BRIDGE_logError("No Kinematics Group found");
-    return prob;
-  }
-
-  prob->env = request.env;
-  prob->env_state = request.env->getState();
-
-  std::vector<std::string> joint_names = prob->manip->getJointNames();
+  // Get solver config
+  auto solver_profile =
+      getProfile<DescartesSolverProfile<FloatType>>(name_,
+                                                    request.instructions.getProfile(name_),
+                                                    *request.profiles,
+                                                    std::make_shared<DescartesDefaultSolverProfile<FloatType>>());
+  prob->solver_config = solver_profile->createSolverConfig(request.instructions, *request.env);
 
   // Flatten the input for planning
   auto move_instructions = request.instructions.flatten(&moveFilter);
 
   // Transform plan instructions into descartes samplers
   int index = 0;
-  for (const auto& move_instruction : move_instructions)
+  for (const auto& instruction : move_instructions)
   {
-    const auto& instruction = move_instruction.get();
-
-    assert(instruction.isMoveInstruction());
-    const auto& plan_instruction = instruction.template as<MoveInstructionPoly>();
+    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 mi = composite_mi.getCombined(plan_instruction.getManipulatorInfo());
+    tesseract_common::ManipulatorInfo manip_info =
+        request.instructions.getManipulatorInfo().getCombined(move_instruction.getManipulatorInfo());
 
-    if (mi.manipulator.empty())
+    if (manip_info.manipulator.empty())
       throw std::runtime_error("Descartes, manipulator is empty!");
 
-    if (mi.tcp_frame.empty())
+    if (manip_info.tcp_frame.empty())
       throw std::runtime_error("Descartes, tcp_frame is empty!");
 
-    if (mi.working_frame.empty())
+    if (manip_info.working_frame.empty())
       throw std::runtime_error("Descartes, working_frame is empty!");
 
     // Get Plan Profile
     auto cur_plan_profile =
         getProfile<DescartesPlanProfile<FloatType>>(name_,
-                                                    plan_instruction.getProfile(name_),
+                                                    move_instruction.getProfile(name_),
                                                     *request.profiles,
                                                     std::make_shared<DescartesDefaultPlanProfile<FloatType>>());
 
     if (!cur_plan_profile)
       throw std::runtime_error("DescartesMotionPlanner: Invalid profile");
 
-    if (plan_instruction.getWaypoint().isCartesianWaypoint())
-    {
-      const auto& cur_wp = plan_instruction.getWaypoint().template as<CartesianWaypointPoly>();
-      cur_plan_profile->apply(*prob, cur_wp.getTransform(), plan_instruction, composite_mi, index);
-      ++index;
-    }
-    else if (plan_instruction.getWaypoint().isJointWaypoint())
-    {
-      if (plan_instruction.getWaypoint().as<JointWaypointPoly>().isConstrained())
-      {
-        assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint()));
-        const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint());
-        cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index);
-        ++index;
-      }
-    }
-    else if (plan_instruction.getWaypoint().isStateWaypoint())
-    {
-      assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint()));
-      const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint());
-      cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index);
-      ++index;
-    }
-    else
-    {
-      throw std::runtime_error("DescartesMotionPlanner: unknown waypoint type");
-    }
+    if (move_instruction.getWaypoint().isJointWaypoint() &&
+        !move_instruction.getWaypoint().as<JointWaypointPoly>().isConstrained())
+      continue;
+
+    prob->samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, manip_info, request.env));
+    prob->state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, manip_info, request.env));
+    if (index != 0)
+      prob->edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, manip_info, request.env));
+
+    ++index;
   }
 
   // Call the base class generate which checks the problem to make sure everything is in order