Add KinematicsInformation class and modify srdf model class

Author: Levi-Armstrong

tesseract/tesseract_environment/CMakeLists.txt

@@ -34,7 +34,7 @@ add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE})
 tesseract_variables()
 
 # Create interface for core
-add_library(${PROJECT_NAME}_core src/core/environment.cpp src/manipulator_manager/manipulator_manager.cpp)
+add_library(${PROJECT_NAME}_core src/core/environment.cpp src/core/manipulator_manager.cpp)
 target_link_libraries(${PROJECT_NAME}_core PUBLIC
   tesseract::tesseract_common
   tesseract::tesseract_collision_core

tesseract/tesseract_environment/include/tesseract_environment/core/commands.h

@@ -37,6 +37,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_scene_graph/joint.h>
 #include <tesseract_scene_graph/link.h>
 #include <tesseract_scene_graph/graph.h>
+#include <tesseract_scene_graph/kinematics_information.h>
 
 namespace tesseract_environment
 {
@@ -57,7 +58,8 @@ enum class CommandType
   ADD_SCENE_GRAPH = 12,
   CHANGE_JOINT_POSITION_LIMITS = 13,
   CHANGE_JOINT_VELOCITY_LIMITS = 14,
-  CHANGE_JOINT_ACCELERATION_LIMITS = 15
+  CHANGE_JOINT_ACCELERATION_LIMITS = 15,
+  ADD_KINEMATICS_INFORMATION = 16
 };
 
 class Command
@@ -290,6 +292,23 @@ class AddSceneGraphCommand : public Command
   std::string prefix_;
 };
 
+class AddKinematicsInformationCommand : public Command
+{
+public:
+  AddKinematicsInformationCommand(tesseract_scene_graph::KinematicsInformation kinematics_information)
+    : Command(CommandType::ADD_KINEMATICS_INFORMATION), kinematics_information_(std::move(kinematics_information))
+  {
+  }
+
+  const tesseract_scene_graph::KinematicsInformation& getKinematicsInformation() const
+  {
+    return kinematics_information_;
+  }
+
+private:
+  tesseract_scene_graph::KinematicsInformation kinematics_information_;
+};
+
 class ChangeJointPositionLimitsCommand : public Command
 {
 public:

tesseract/tesseract_environment/include/tesseract_environment/core/environment.h

@@ -35,13 +35,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_environment/core/types.h>
 #include <tesseract_environment/core/commands.h>
+#include <tesseract_environment/core/state_solver.h>
+#include <tesseract_environment/core/manipulator_manager.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
 #include <tesseract_collision/core/discrete_contact_manager_factory.h>
 #include <tesseract_collision/core/continuous_contact_manager.h>
 #include <tesseract_collision/core/continuous_contact_manager_factory.h>
 #include <tesseract_scene_graph/graph.h>
-#include <tesseract_environment/core/state_solver.h>
-#include <tesseract_environment/manipulator_manager/manipulator_manager.h>
+#include <tesseract_scene_graph/utils.h>
 
 namespace tesseract_environment
 {
@@ -70,7 +71,7 @@ class Environment
    */
   template <typename S>
   bool init(const tesseract_scene_graph::SceneGraph& scene_graph,
-            tesseract_scene_graph::SRDFModel::Ptr srdf_model = nullptr)
+            const tesseract_scene_graph::SRDFModel::ConstPtr& srdf_model = nullptr)
   {
     initialized_ = false;
     revision_ = 0;
@@ -88,6 +89,9 @@ class Environment
       return false;
     }
 
+    if (srdf_model != nullptr)
+      processSRDFAllowedCollisions(*scene_graph_, *srdf_model);
+
     // Add this to the command history. This should always the the first thing in the command history
     ++revision_;
     commands_.push_back(std::make_shared<AddSceneGraphCommand>(scene_graph, nullptr, ""));
@@ -111,15 +115,22 @@ class Environment
       return false;
     }
 
+    manipulator_manager_ = std::make_shared<ManipulatorManager>();
     if (!srdf_model)
     {
       CONSOLE_BRIDGE_logDebug("Environment is being initialized without an SRDF Model. Manipulators will not be "
                               "registered");
-      srdf_model = std::make_shared<tesseract_scene_graph::SRDFModel>();
-      srdf_model->getName() = scene_graph_->getName();
+      manipulator_manager_->init(scene_graph_, tesseract_scene_graph::KinematicsInformation());
+    }
+    else
+    {
+      manipulator_manager_->init(scene_graph_, srdf_model->getKinematicsInformation());
     }
-    manipulator_manager_ = std::make_shared<ManipulatorManager>();
-    manipulator_manager_->init(scene_graph_, srdf_model);
+
+    // Add this to the command history.
+    ++revision_;
+    commands_.push_back(
+        std::make_shared<AddKinematicsInformationCommand>(manipulator_manager_->getKinematicsInformation()));
 
     is_contact_allowed_fn_ = std::bind(&tesseract_scene_graph::SceneGraph::isCollisionAllowed,
                                        scene_graph_,
@@ -175,13 +186,13 @@ class Environment
    * @brief Get the current revision number
    * @return Revision number
    */
-  int getRevision() const { return revision_; }
+  int getRevision() const;
 
   /**
    * @brief Get Environment command history post initialization
    * @return List of commands
    */
-  const Commands& getCommandHistory() const { return commands_; }
+  const Commands& getCommandHistory() const;
 
   /**
    * @brief Applies the commands to the environment
@@ -211,25 +222,44 @@ class Environment
    * @brief Check if environment has been initialized
    * @return True if initialized otherwise false
    */
-  virtual bool checkInitialized() const { return initialized_; }
+  virtual bool checkInitialized() const;
 
   /**
    * @brief Get the Scene Graph
    * @return SceneGraphConstPtr
    */
-  virtual const tesseract_scene_graph::SceneGraph::ConstPtr& getSceneGraph() const { return scene_graph_const_; }
+  virtual const tesseract_scene_graph::SceneGraph::ConstPtr& getSceneGraph() const;
 
-  ManipulatorManager::Ptr getManipulatorManager() { return manipulator_manager_; }
-  ManipulatorManager::ConstPtr getManipulatorManager() const { return manipulator_manager_; }
+  /**
+   * @brief Get the manipulator manager
+   * @todo This should go away and should expose methods for adding and removing objects from the manager and storing
+   *       as commands
+   * @warning Changes made to the manager are not capture in the command history.
+   * @return The manipulator manager
+   */
+  virtual ManipulatorManager::Ptr getManipulatorManager();
+
+  /**
+   * @brief Get the manipulator manager const
+   * @return The manipulator manager const
+   */
+  virtual ManipulatorManager::ConstPtr getManipulatorManager() const;
+
+  /**
+   * @brief Add kinematics information to the environment
+   * @param kin_info The kinematics information
+   * @return true if successful, otherwise false
+   */
+  virtual bool addKinematicsInformation(const tesseract_scene_graph::KinematicsInformation& kin_info);
 
   /** @brief Give the environment a name */
-  virtual void setName(const std::string& name) { scene_graph_->setName(name); }
+  virtual void setName(const std::string& name);
 
   /** @brief Get the name of the environment
    *
    * This may be empty, if so check urdf name
    */
-  virtual const std::string& getName() const { return scene_graph_->getName(); }
+  virtual const std::string& getName() const;
 
   /**
    * @brief Set the current state of the environment
@@ -258,7 +288,7 @@ class Environment
                                  const Eigen::Ref<const Eigen::VectorXd>& joint_values) const;
 
   /** @brief Get the current state of the environment */
-  virtual EnvState::ConstPtr getCurrentState() const { return current_state_; }
+  virtual EnvState::ConstPtr getCurrentState() const;
 
   /**
    * @brief Adds a link to the environment
@@ -531,12 +561,7 @@ class Environment
   virtual bool setActiveDiscreteContactManager(const std::string& name);
 
   /** @brief Get a copy of the environments active discrete contact manager */
-  virtual tesseract_collision::DiscreteContactManager::Ptr getDiscreteContactManager() const
-  {
-    if (!discrete_manager_)
-      return nullptr;
-    return discrete_manager_->clone();
-  }
+  virtual tesseract_collision::DiscreteContactManager::Ptr getDiscreteContactManager() const;
 
   /** @brief Get a copy of the environments available discrete contact manager by name */
   virtual tesseract_collision::DiscreteContactManager::Ptr getDiscreteContactManager(const std::string& name) const;
@@ -549,12 +574,7 @@ class Environment
   virtual bool setActiveContinuousContactManager(const std::string& name);
 
   /** @brief Get a copy of the environments active continuous contact manager */
-  virtual tesseract_collision::ContinuousContactManager::Ptr getContinuousContactManager() const
-  {
-    if (!continuous_manager_)
-      return nullptr;
-    return continuous_manager_->clone();
-  }
+  virtual tesseract_collision::ContinuousContactManager::Ptr getContinuousContactManager() const;
 
   /** @brief Get a copy of the environments available continuous contact manager by name */
   virtual tesseract_collision::ContinuousContactManager::Ptr getContinuousContactManager(const std::string& name) const;
@@ -566,10 +586,7 @@ class Environment
    * in the environment.
    */
   bool registerDiscreteContactManager(const std::string& name,
-                                      tesseract_collision::DiscreteContactManagerFactory::CreateMethod create_function)
-  {
-    return discrete_factory_.registar(name, std::move(create_function));
-  }
+                                      tesseract_collision::DiscreteContactManagerFactory::CreateMethod create_function);
 
   /**
    * @brief Set the discrete contact manager
@@ -579,10 +596,7 @@ class Environment
    */
   bool
   registerContinuousContactManager(const std::string& name,
-                                   tesseract_collision::ContinuousContactManagerFactory::CreateMethod create_function)
-  {
-    return continuous_factory_.registar(name, std::move(create_function));
-  }
+                                   tesseract_collision::ContinuousContactManagerFactory::CreateMethod create_function);
 
   /** @brief Merge a graph into the current environment
    * @param scene_graph Const ref to the graph to be merged (said graph will be copied)

tesseract/tesseract_environment/include/tesseract_environment/manipulator_manager/manipulator_manager.h renamed to tesseract/tesseract_environment/include/tesseract_environment/core/manipulator_manager.h

@@ -33,7 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_scene_graph/srdf_model.h>
-#include <tesseract_scene_graph/srdf/types.h>
 #include <tesseract_kinematics/core/forward_kinematics_factory.h>
 #include <tesseract_kinematics/core/inverse_kinematics_factory.h>
 
@@ -52,7 +51,8 @@ class ManipulatorManager
   ManipulatorManager(ManipulatorManager&&) = default;
   ManipulatorManager& operator=(ManipulatorManager&&) = default;
 
-  bool init(tesseract_scene_graph::SceneGraph::ConstPtr scene_graph, tesseract_scene_graph::SRDFModel::Ptr srdf_model);
+  bool init(tesseract_scene_graph::SceneGraph::ConstPtr scene_graph,
+            tesseract_scene_graph::KinematicsInformation kinematics_information);
 
   /**
    * @brief Updates all of the stored solvers
@@ -66,8 +66,11 @@ class ManipulatorManager
    */
   ManipulatorManager::Ptr clone(tesseract_scene_graph::SceneGraph::ConstPtr scene_graph) const;
 
-  /** @brief Get the SRDF Model */
-  tesseract_scene_graph::SRDFModel::ConstPtr getSRDFModel() const;
+  /** @brief Kinematics Information */
+  bool addKinematicsInformation(const tesseract_scene_graph::KinematicsInformation& kinematics_information);
+
+  /** @brief Get the kinematics information */
+  const tesseract_scene_graph::KinematicsInformation& getKinematicsInformation() const;
 
   /** @brief Get Group Names */
   const tesseract_scene_graph::GroupNames& getGroupNames() const;
@@ -122,12 +125,6 @@ class ManipulatorManager
   const tesseract_scene_graph::GroupTCPs& getGroupTCPs() const;
   bool hasGroupTCP(const std::string& group_name, const std::string& tcp_name) const;
 
-  // This is exposed for the SRDF editor should not use in normal applications
-  void addAllowedCollision(const std::string& link_1, const std::string& link_2, const std::string& reason);
-  void removeAllowedCollision(const std::string& link_1, const std::string& link_2);
-  void clearAllowedCollisions();
-  const tesseract_scene_graph::AllowedCollisionMatrix& getAllowedCollisionMatrix() const;
-
   /**
    * @brief Register a forward kinematics factory
    * @param factory The factory to register
@@ -301,7 +298,7 @@ class ManipulatorManager
   tesseract_kinematics::InverseKinematics::Ptr getInvKinematicSolver(const std::string& manipulator) const;
 
 private:
-  tesseract_scene_graph::SRDFModel::Ptr srdf_model_;
+  tesseract_scene_graph::KinematicsInformation kinematics_information_;
   tesseract_scene_graph::SceneGraph::ConstPtr scene_graph_;
   tesseract_kinematics::ForwardKinematicsFactory::ConstPtr fwd_kin_chain_default_factory_;
   tesseract_kinematics::ForwardKinematicsFactory::ConstPtr fwd_kin_tree_default_factory_;