Add ManipulatorManager update method

mpowelson · Levi-Armstrong · commit 1a2511941d88 · 2020-11-12T10:03:31.000-06:00
diff --git a/tesseract/tesseract/include/tesseract/manipulator_manager.h b/tesseract/tesseract/include/tesseract/manipulator_manager.h
@@ -55,6 +55,12 @@ class ManipulatorManager
 
   bool init(tesseract_environment::Environment::ConstPtr environment, tesseract_scene_graph::SRDFModel::Ptr srdf_model);
 
+  /**
+   * @brief Updates all of the stored solvers
+   * @return True if successful
+   */
+  bool update();
+
   /**
    * @brief This will clone the manager and assign the new environment object
    * @param environment The environment the clone is associated with.
@@ -303,13 +309,13 @@ class ManipulatorManager
   tesseract_kinematics::ForwardKinematicsFactory::ConstPtr fwd_kin_tree_default_factory_;
   tesseract_kinematics::InverseKinematicsFactory::ConstPtr inv_kin_chain_default_factory_;
   std::unordered_map<std::string, tesseract_kinematics::ForwardKinematicsFactory::ConstPtr> fwd_kin_factories_;
-  std::map<std::pair<std::string, std::string>, tesseract_kinematics::ForwardKinematics::ConstPtr>
+  std::map<std::pair<std::string, std::string>, tesseract_kinematics::ForwardKinematics::Ptr>
       fwd_kin_manipulators_;
-  std::unordered_map<std::string, tesseract_kinematics::ForwardKinematics::ConstPtr> fwd_kin_manipulators_default_;
+  std::unordered_map<std::string, tesseract_kinematics::ForwardKinematics::Ptr> fwd_kin_manipulators_default_;
   std::unordered_map<std::string, tesseract_kinematics::InverseKinematicsFactory::ConstPtr> inv_kin_factories_;
-  std::map<std::pair<std::string, std::string>, tesseract_kinematics::InverseKinematics::ConstPtr>
+  std::map<std::pair<std::string, std::string>, tesseract_kinematics::InverseKinematics::Ptr>
       inv_kin_manipulators_;
-  std::unordered_map<std::string, tesseract_kinematics::InverseKinematics::ConstPtr> inv_kin_manipulators_default_;
+  std::unordered_map<std::string, tesseract_kinematics::InverseKinematics::Ptr> inv_kin_manipulators_default_;
 
   bool registerDefaultChainSolver(const std::string& group_name, const tesseract_scene_graph::ChainGroup& chain_group);
   bool registerDefaultJointSolver(const std::string& group_name, const tesseract_scene_graph::JointGroup& joint_group);
diff --git a/tesseract/tesseract/src/manipulator_manager.cpp b/tesseract/tesseract/src/manipulator_manager.cpp
@@ -76,6 +76,18 @@ bool ManipulatorManager::init(tesseract_environment::Environment::ConstPtr envir
   return success;
 }
 
+bool ManipulatorManager::update()
+{
+  bool success = true;
+  for (auto& fk : fwd_kin_manipulators_)
+    success &= fk.second->update();
+
+  for (auto& ik : inv_kin_manipulators_)
+    success &= ik.second->update();
+
+  return success;
+}
+
 ManipulatorManager::Ptr ManipulatorManager::clone(tesseract_environment::Environment::ConstPtr environment) const
 {
   auto cloned_manager = std::make_shared<ManipulatorManager>(*this);
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/forward_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/forward_kinematics.h
@@ -58,6 +58,12 @@ class ForwardKinematics
   ForwardKinematics(ForwardKinematics&&) = delete;
   ForwardKinematics& operator=(ForwardKinematics&&) = delete;
 
+  /**
+   * @brief Updates kinematics if kinematic parameters have changed
+   * @return True if successful
+   */
+  virtual bool update() = 0;
+
   /**
    * @brief Calculates tool pose of robot chain
    * @param pose Transform of end-of-tip relative to root
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/inverse_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/inverse_kinematics.h
@@ -57,6 +57,12 @@ class InverseKinematics
   InverseKinematics(InverseKinematics&&) = delete;
   InverseKinematics& operator=(InverseKinematics&&) = delete;
 
+  /**
+   * @brief Updates kinematics if kinematic parameters have changed
+   * @return True if successful
+   */
+  virtual bool update() = 0;
+
   /**
    * @brief Calculates joint solutions given a pose.
    * @param solutions A vector of solutions, so check the size of the vector to determine the number of solutions
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/rep_inverse_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/rep_inverse_kinematics.h
@@ -61,6 +61,8 @@ class RobotWithExternalPositionerInvKin : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/rop_inverse_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/rop_inverse_kinematics.h
@@ -59,6 +59,8 @@ class RobotOnPositionerInvKin : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/ikfast/ikfast_inv_kin.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/ikfast/ikfast_inv_kin.h
@@ -95,6 +95,8 @@ class IKFastInvKin : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp b/tesseract/tesseract_kinematics/include/tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp
@@ -50,6 +50,11 @@ InverseKinematics::Ptr IKFastInvKin::clone() const
   return std::move(cloned_invkin);
 }
 
+bool IKFastInvKin::update()
+{
+  return init(name_, base_link_name_, tip_link_name_, joint_names_, link_names_, active_link_names_, limits_);
+}
+
 bool IKFastInvKin::calcInvKin(Eigen::VectorXd& solutions,
                               const Eigen::Isometry3d& pose,
                               const Eigen::Ref<const Eigen::VectorXd>& seed) const
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h
@@ -66,6 +66,8 @@ class KDLFwdKinChain : public ForwardKinematics
 
   ForwardKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcFwdKin(Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
 
   bool calcFwdKin(tesseract_common::VectorIsometry3d& poses,
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_tree.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_fwd_kin_tree.h
@@ -64,6 +64,8 @@ class KDLFwdKinTree : public ForwardKinematics
 
   ForwardKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcFwdKin(Eigen::Isometry3d& pose, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override;
 
   bool calcFwdKin(tesseract_common::VectorIsometry3d& poses,
@@ -137,6 +139,7 @@ class KDLFwdKinTree : public ForwardKinematics
   std::string solver_name_{ "KDLFwdKinTree" };              /**< Name of this solver */
   std::vector<std::string> joint_list_;                     /**< List of joint names */
   KDL::JntArray start_state_;  /**< Intial state of the tree. Should include all joints in the model. */
+  std::unordered_map<std::string, double> input_start_state_; /**< Input start state before it has been translated into KDL types */
   std::vector<int> joint_qnr_; /**< The kdl segment number corrisponding to joint in joint_lists_ */
   std::unordered_map<std::string, unsigned int> joint_to_qnr_; /**< The tree joint name to qnr */
   std::vector<std::string> link_list_;                         /**< List of link names */
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h
@@ -61,6 +61,8 @@ class KDLInvKinChainLMA : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h
@@ -63,6 +63,8 @@ class KDLInvKinChainNR : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/opw/opw_inv_kin.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/opw/opw_inv_kin.h
@@ -53,6 +53,8 @@ class OPWInvKin : public InverseKinematics
 
   InverseKinematics::Ptr clone() const override;
 
+  bool update() override;
+
   bool calcInvKin(Eigen::VectorXd& solutions,
                   const Eigen::Isometry3d& pose,
                   const Eigen::Ref<const Eigen::VectorXd>& seed) const override;
diff --git a/tesseract/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp b/tesseract/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp
@@ -42,6 +42,11 @@ InverseKinematics::Ptr RobotWithExternalPositionerInvKin::clone() const
   return cloned_invkin;
 }
 
+bool RobotWithExternalPositionerInvKin::update()
+{
+  return init(scene_graph_, manip_inv_kin_, manip_reach_, positioner_fwd_kin_, positioner_sample_resolution_, name_);
+}
+
 bool RobotWithExternalPositionerInvKin::calcInvKinHelper(std::vector<double>& solutions,
                                                          const Eigen::Isometry3d& pose,
                                                          const Eigen::Ref<const Eigen::VectorXd>& seed) const
diff --git a/tesseract/tesseract_kinematics/src/core/rop_inverse_kinematics.cpp b/tesseract/tesseract_kinematics/src/core/rop_inverse_kinematics.cpp
@@ -42,6 +42,11 @@ InverseKinematics::Ptr RobotOnPositionerInvKin::clone() const
   return cloned_invkin;
 }
 
+bool RobotOnPositionerInvKin::update()
+{
+  return init(scene_graph_, manip_inv_kin_, manip_reach_, positioner_fwd_kin_, positioner_sample_resolution_, name_);
+}
+
 bool RobotOnPositionerInvKin::calcInvKinHelper(std::vector<double>& solutions,
                                                const Eigen::Isometry3d& pose,
                                                const Eigen::Ref<const Eigen::VectorXd>& seed) const
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_chain.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_chain.cpp
@@ -44,6 +44,11 @@ ForwardKinematics::Ptr KDLFwdKinChain::clone() const
   return cloned_fwdkin;
 }
 
+bool KDLFwdKinChain::update()
+{
+  return init(scene_graph_, kdl_data_.base_name, kdl_data_.tip_name, name_);
+}
+
 bool KDLFwdKinChain::calcFwdKinHelper(Eigen::Isometry3d& pose,
                                       const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
                                       int segment_num) const
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_tree.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_tree.cpp
@@ -45,6 +45,12 @@ ForwardKinematics::Ptr KDLFwdKinTree::clone() const
   return cloned_fwdkin;
 }
 
+bool KDLFwdKinTree::update()
+{
+  return init(scene_graph_, joint_list_, name_, input_start_state_);
+}
+
+
 KDL::JntArray KDLFwdKinTree::getKDLJntArray(const std::vector<std::string>& joint_names,
                                             const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const
 {
@@ -59,6 +65,7 @@ KDL::JntArray KDLFwdKinTree::getKDLJntArray(const std::vector<std::string>& join
 
 void KDLFwdKinTree::setStartState(std::unordered_map<std::string, double> start_state)
 {
+  input_start_state_ = start_state;
   KDL::JntArray kdl_joints;
   kdl_joints.resize(static_cast<unsigned>(start_state.size()));
   for (const auto& jnt : start_state)
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_inv_kin_chain_lma.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_inv_kin_chain_lma.cpp
@@ -45,6 +45,11 @@ InverseKinematics::Ptr KDLInvKinChainLMA::clone() const
   return cloned_invkin;
 }
 
+bool KDLInvKinChainLMA::update()
+{
+  return init(scene_graph_, kdl_data_.base_name, kdl_data_.tip_name, name_);
+}
+
 bool KDLInvKinChainLMA::calcInvKinHelper(Eigen::VectorXd& solutions,
                                          const Eigen::Isometry3d& pose,
                                          const Eigen::Ref<const Eigen::VectorXd>& seed,
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_inv_kin_chain_nr.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_inv_kin_chain_nr.cpp
@@ -45,6 +45,11 @@ InverseKinematics::Ptr KDLInvKinChainNR::clone() const
   return cloned_invkin;
 }
 
+bool KDLInvKinChainNR::update()
+{
+    return init(scene_graph_, kdl_data_.base_name, kdl_data_.tip_name, name_);
+}
+
 bool KDLInvKinChainNR::calcInvKinHelper(Eigen::VectorXd& solutions,
                                         const Eigen::Isometry3d& pose,
                                         const Eigen::Ref<const Eigen::VectorXd>& seed,
diff --git a/tesseract/tesseract_kinematics/src/opw/opw_inv_kin.cpp b/tesseract/tesseract_kinematics/src/opw/opw_inv_kin.cpp
@@ -42,6 +42,11 @@ InverseKinematics::Ptr OPWInvKin::clone() const
   return cloned_invkin;
 }
 
+bool OPWInvKin::update()
+{
+  return init(name_, params_, base_link_name_, tip_link_name_, joint_names_, link_names_, active_link_names_, limits_);
+}
+
 bool OPWInvKin::calcInvKin(Eigen::VectorXd& solutions,
                            const Eigen::Isometry3d& pose,
                            const Eigen::Ref<const Eigen::VectorXd>& /*seed*/) const
