Switch to using state solver in descartes edge evaluator and ompl motion validator

Author: Levi-Armstrong

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/descartes_collision_edge_evaluator.h

@@ -55,20 +55,28 @@ class DescartesCollisionEdgeEvaluator : public descartes_light::EdgeEvaluator<Fl
                 std::vector<typename descartes_light::LadderGraph<FloatType>::EdgeList>& edges) override;
 
 protected:
-  tesseract_environment::StateSolver::Ptr state_solver_; /**< @brief The tesseract state solver */
-  tesseract_scene_graph::AllowedCollisionMatrix acm_;    /**< @brief The allowed collision matrix */
-  std::vector<std::string> active_link_names_;           /**< @brief A vector of active link names */
-  std::vector<std::string> joint_names_;                 /**< @brief A vector of joint names */
-  tesseract_collision::DiscreteContactManager::Ptr discrete_contact_manager_; /**< @brief The discrete contact manager
-                                                                               */
-  tesseract_collision::ContinuousContactManager::Ptr continuous_contact_manager_; /**< @brief The discrete contact
-                                                                                     manager */
-  double collision_safety_margin_;      /**< @brief The minimum allowed collision distance */
-  double longest_valid_segment_length_; /**< @brief Used to check collisions between two state if norm(state0-state1) >
-                                           longest_valid_segment_length. */
-  bool allow_collision_; /**< @brief If true and no valid edges are found it will return the one with the lowest cost */
-  bool debug_;           /**< @brief Enable debug information to be printed to the terminal */
-  std::size_t dof_;      /**< @brief The number of joints */
+  /** @brief The tesseract state solver */
+  tesseract_environment::StateSolver::ConstPtr state_solver_;
+  /** @brief The allowed collision matrix */
+  tesseract_scene_graph::AllowedCollisionMatrix acm_;
+  /** @brief A vector of active link names */
+  std::vector<std::string> active_link_names_;
+  /** @brief A vector of joint names */
+  std::vector<std::string> joint_names_;
+  /** @brief The discrete contact manager */
+  tesseract_collision::DiscreteContactManager::Ptr discrete_contact_manager_;
+  /** @brief The discrete contact manager */
+  tesseract_collision::ContinuousContactManager::Ptr continuous_contact_manager_;
+  /** @brief The minimum allowed collision distance */
+  double collision_safety_margin_;
+  /** @brief Used to check collisions between two state if norm(state0-state1) > longest_valid_segment_length. */
+  double longest_valid_segment_length_;
+  /** @brief If true and no valid edges are found it will return the one with the lowest cost */
+  bool allow_collision_;
+  /** @brief Enable debug information to be printed to the terminal */
+  bool debug_;
+  /** @brief The number of joints */
+  std::size_t dof_;
 
   /**
    * @brief Check continuous and discrete collision between two states
@@ -106,6 +114,9 @@ class DescartesCollisionEdgeEvaluator : public descartes_light::EdgeEvaluator<Fl
   /** @brief The discrete contact manager cache */
   mutable std::map<unsigned long int, tesseract_collision::DiscreteContactManager::Ptr> discrete_contact_managers_;
 
+  /** @brief The state solver manager cache */
+  mutable std::map<unsigned long int, tesseract_environment::StateSolver::Ptr> state_solver_managers_;
+
   /**
    * @brief Perform a continuous collision check between two states
    * @param segment Trajectory containing two states

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp

@@ -159,22 +159,27 @@ bool DescartesCollisionEdgeEvaluator<FloatType>::continuousCollisionCheck(
   // It was time using chronos time elapsed and it was faster to cache the contact manager
   unsigned long int hash = std::hash<std::thread::id>{}(std::this_thread::get_id());
   tesseract_collision::ContinuousContactManager::Ptr cm;
+  tesseract_environment::StateSolver::Ptr ss;
   mutex_.lock();
   auto it = continuous_contact_managers_.find(hash);
   if (it == continuous_contact_managers_.end())
   {
     cm = continuous_contact_manager_->clone();
     continuous_contact_managers_[hash] = cm;
+
+    ss = state_solver_->clone();
+    state_solver_managers_[hash] = ss;
   }
   else
   {
     cm = it->second;
+    ss = state_solver_managers_[hash];
   }
   mutex_.unlock();
 
   return tesseract_environment::checkTrajectory(results,
                                                 *cm,
-                                                *state_solver_,
+                                                *ss,
                                                 joint_names_,
                                                 segment,
                                                 longest_valid_segment_length_,
@@ -192,22 +197,27 @@ bool DescartesCollisionEdgeEvaluator<FloatType>::discreteCollisionCheck(
   // It was time using chronos time elapsed and it was faster to cache the contact manager
   unsigned long int hash = std::hash<std::thread::id>{}(std::this_thread::get_id());
   tesseract_collision::DiscreteContactManager::Ptr cm;
+  tesseract_environment::StateSolver::Ptr ss;
   mutex_.lock();
   auto it = discrete_contact_managers_.find(hash);
   if (it == discrete_contact_managers_.end())
   {
     cm = discrete_contact_manager_->clone();
     discrete_contact_managers_[hash] = cm;
+
+    ss = state_solver_->clone();
+    state_solver_managers_[hash] = ss;
   }
   else
   {
     cm = it->second;
+    ss = state_solver_managers_[hash];
   }
   mutex_.unlock();
 
   return tesseract_environment::checkTrajectory(results,
                                                 *cm,
-                                                *state_solver_,
+                                                *ss,
                                                 joint_names_,
                                                 segment,
                                                 longest_valid_segment_length_,

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/ompl/continuous_motion_validator.h

@@ -42,7 +42,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
 {
 public:
   ContinuousMotionValidator(const ompl::base::SpaceInformationPtr& space_info,
-                            tesseract_environment::Environment::ConstPtr env,
+                            const tesseract_environment::Environment::ConstPtr& env,
                             tesseract_kinematics::ForwardKinematics::ConstPtr kin,
                             double collision_safety_margin);
 
@@ -71,6 +71,9 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
   /** @brief The Tesseract Environment */
   tesseract_environment::Environment::ConstPtr env_;
 
+  /**< @brief The tesseract state solver */
+  tesseract_environment::StateSolver::ConstPtr state_solver_;
+
   /** @brief The Tesseract Forward Kinematics */
   tesseract_kinematics::ForwardKinematics::ConstPtr kin_;
 
@@ -99,6 +102,9 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
 
   /** @brief The discrete contact manager cache */
   mutable std::map<unsigned long int, tesseract_collision::DiscreteContactManager::Ptr> discrete_contact_managers_;
+
+  /** @brief The state solver manager cache */
+  mutable std::map<unsigned long int, tesseract_environment::StateSolver::Ptr> state_solver_managers_;
 };
 }  // namespace tesseract_motion_planners
 

tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/continuous_motion_validator.cpp

@@ -35,24 +35,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace tesseract_motion_planners
 {
 ContinuousMotionValidator::ContinuousMotionValidator(const ompl::base::SpaceInformationPtr& space_info,
-                                                     tesseract_environment::Environment::ConstPtr env,
+                                                     const tesseract_environment::Environment::ConstPtr& env,
                                                      tesseract_kinematics::ForwardKinematics::ConstPtr kin,
                                                      double collision_safety_margin)
-  : MotionValidator(space_info), env_(std::move(env)), kin_(std::move(kin))
+  : MotionValidator(space_info)
+  , state_solver_(env->getStateSolver())
+  , kin_(std::move(kin))
+  , continuous_contact_manager_(env->getContinuousContactManager())
+  , discrete_contact_manager_(env->getDiscreteContactManager())
 {
   joints_ = kin_->getJointNames();
 
   // kinematics objects does not know of every link affected by its motion so must compute adjacency map
   // to determine all active links.
   tesseract_environment::AdjacencyMap adj_map(
-      env_->getSceneGraph(), kin_->getActiveLinkNames(), env_->getCurrentState()->transforms);
+      env->getSceneGraph(), kin_->getActiveLinkNames(), env->getCurrentState()->transforms);
   links_ = adj_map.getActiveLinkNames();
 
-  continuous_contact_manager_ = env_->getContinuousContactManager();
   continuous_contact_manager_->setActiveCollisionObjects(links_);
   continuous_contact_manager_->setContactDistanceThreshold(collision_safety_margin);
 
-  discrete_contact_manager_ = env_->getDiscreteContactManager();
   discrete_contact_manager_->setActiveCollisionObjects(links_);
   discrete_contact_manager_->setContactDistanceThreshold(collision_safety_margin);
 }
@@ -125,25 +127,30 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State
   // It was time using chronos time elapsed and it was faster to cache the contact manager
   unsigned long int hash = std::hash<std::thread::id>{}(std::this_thread::get_id());
   tesseract_collision::ContinuousContactManager::Ptr cm;
+  tesseract_environment::StateSolver::Ptr ss;
   mutex_.lock();
   auto it = continuous_contact_managers_.find(hash);
   if (it == continuous_contact_managers_.end())
   {
     cm = continuous_contact_manager_->clone();
     continuous_contact_managers_[hash] = cm;
+
+    ss = state_solver_->clone();
+    state_solver_managers_[hash] = ss;
   }
   else
   {
     cm = it->second;
+    ss = state_solver_managers_[hash];
   }
   mutex_.unlock();
 
   const auto dof = si_->getStateDimension();
   Eigen::Map<Eigen::VectorXd> start_joints(start->values, dof);
   Eigen::Map<Eigen::VectorXd> finish_joints(finish->values, dof);
 
-  tesseract_environment::EnvState::Ptr state0 = env_->getState(joints_, start_joints);
-  tesseract_environment::EnvState::Ptr state1 = env_->getState(joints_, finish_joints);
+  tesseract_environment::EnvState::Ptr state0 = ss->getState(joints_, start_joints);
+  tesseract_environment::EnvState::Ptr state1 = ss->getState(joints_, finish_joints);
 
   for (const auto& link_name : links_)
     cm->setCollisionObjectsTransform(link_name, state0->transforms[link_name], state1->transforms[link_name]);
@@ -161,23 +168,28 @@ bool ContinuousMotionValidator::discreteCollisionCheck(const ompl::base::State*
   // It was time using chronos time elapsed and it was faster to cache the contact manager
   unsigned long int hash = std::hash<std::thread::id>{}(std::this_thread::get_id());
   tesseract_collision::DiscreteContactManager::Ptr cm;
+  tesseract_environment::StateSolver::Ptr ss;
   mutex_.lock();
   auto it = discrete_contact_managers_.find(hash);
   if (it == discrete_contact_managers_.end())
   {
     cm = discrete_contact_manager_->clone();
     discrete_contact_managers_[hash] = cm;
+
+    ss = state_solver_->clone();
+    state_solver_managers_[hash] = ss;
   }
   else
   {
     cm = it->second;
+    ss = state_solver_managers_[hash];
   }
   mutex_.unlock();
 
   const auto dof = si_->getStateDimension();
   Eigen::Map<Eigen::VectorXd> finish_joints(finish->values, dof);
 
-  tesseract_environment::EnvState::Ptr state1 = env_->getState(joints_, finish_joints);
+  tesseract_environment::EnvState::Ptr state1 = ss->getState(joints_, finish_joints);
 
   for (const auto& link_name : links_)
     cm->setCollisionObjectsTransform(link_name, state1->transforms[link_name]);

tesseract/tesseract_planning/tesseract_motion_planners/test/ompl_trajopt_planner_test.cpp

@@ -66,6 +66,7 @@ using namespace tesseract_motion_planners;
 const static int SEED = 1;
 const static std::vector<double> start_state = { -0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 };
 const static std::vector<double> end_state = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 };
+const static bool PLANNER_VERBOSE = false;
 
 std::string locateResource(const std::string& url)
 {
@@ -208,7 +209,7 @@ TYPED_TEST(OMPLTrajOptTestFixture, OMPLTrajOptFreespacePlannerUnit)  // NOLINT
       ompl_config, std::make_shared<tesseract_motion_planners::TrajOptPlannerFreespaceConfig>(trajopt_config));
 
   tesseract_motion_planners::PlannerResponse planning_response;
-  tesseract_common::StatusCode status = this->planner.solve(planning_response);
+  tesseract_common::StatusCode status = this->planner.solve(planning_response, PLANNER_VERBOSE);
 
   // Expect the planning to succeed
   if (!status)