Add setLimits to forward and inverse kinematics

Levi-Armstrong · Levi-Armstrong · commit 631b001e012b · 2020-11-12T10:03:31.000-06:00
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/forward_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/forward_kinematics.h
@@ -150,7 +150,7 @@ class ForwardKinematics
    * @brief Setter for kinematic limits (joint, velocity, acceleration, etc.)
    * @param Kinematic Limits
    */
-  //    virtual void setLimits(tesseract_common::KinematicLimits& limits) = 0;
+  virtual void setLimits(tesseract_common::KinematicLimits limits) = 0;
 
   /**
    * @brief Number of joints in robot
diff --git a/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/inverse_kinematics.h b/tesseract/tesseract_kinematics/include/tesseract_kinematics/core/inverse_kinematics.h
@@ -124,7 +124,7 @@ class InverseKinematics
    * @brief Setter for kinematic limits (joint, velocity, acceleration, etc.)
    * @param Kinematic Limits
    */
-  //  virtual void setLimits(tesseract_common::KinematicLimits& limits) = 0;
+  virtual void setLimits(tesseract_common::KinematicLimits limits) = 0;
 
   /**
    * @brief Number of joints in robot
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
@@ -82,6 +82,8 @@ class RobotWithExternalPositionerInvKin : public InverseKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
   tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
   unsigned int numJoints() const override;
   const std::string& getBaseLinkName() 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
@@ -80,6 +80,8 @@ class RobotOnPositionerInvKin : public InverseKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
   tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
   unsigned int numJoints() const override;
   const std::string& getBaseLinkName() 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
@@ -113,6 +113,7 @@ class IKFastInvKin : public InverseKinematics
   const std::vector<std::string>& getLinkNames() const override;
   const std::vector<std::string>& getActiveLinkNames() const;
   const tesseract_common::KinematicLimits& getLimits() const override;
+  void setLimits(tesseract_common::KinematicLimits limits) override;
   const std::string& getBaseLinkName() const override;
   const std::string& getTipLinkName() const override;
   const std::string& getName() 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
@@ -191,6 +191,17 @@ const std::vector<std::string>& IKFastInvKin::getJointNames() const { return joi
 const std::vector<std::string>& IKFastInvKin::getLinkNames() const { return link_names_; }
 const std::vector<std::string>& IKFastInvKin::getActiveLinkNames() const { return active_link_names_; }
 const tesseract_common::KinematicLimits& IKFastInvKin::getLimits() const { return limits_; }
+
+void IKFastInvKin:: ::setLimits(tesseract_common::KinematicLimits limits)
+{
+  unsigned int nj = numJoints();
+  if (limits.joint_limits.size() != nj || limits.velocity_limits.size() != nj ||
+      limits.acceleration_limits.size() != nj)
+    throw std::runtime_error("Kinematics limits assigned are invalid!");
+
+  limits_ = std::move(limits);
+}
+
 const std::string& IKFastInvKin::getBaseLinkName() const { return base_link_name_; }
 const std::string& IKFastInvKin::getTipLinkName() const { return tip_link_name_; }
 const std::string& IKFastInvKin::getName() const { return name_; }
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
@@ -94,12 +94,19 @@ class KDLFwdKinChain : public ForwardKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
-  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const { return scene_graph_; }
-  unsigned int numJoints() const override { return kdl_data_.robot_chain.getNrOfJoints(); }
-  const std::string& getBaseLinkName() const override { return kdl_data_.base_name; }
-  const std::string& getTipLinkName() const override { return kdl_data_.tip_name; }
-  const std::string& getName() const override { return name_; }
-  const std::string& getSolverName() const override { return solver_name_; }
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
+  unsigned int numJoints() const override;
+
+  const std::string& getBaseLinkName() const override;
+
+  const std::string& getTipLinkName() const override;
+
+  const std::string& getName() const override;
+
+  const std::string& getSolverName() const override;
+
+  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
   /**
    * @brief Initializes Forward Kinematics as chain
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
@@ -92,16 +92,19 @@ class KDLFwdKinTree : public ForwardKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
-  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const { return scene_graph_; }
-  unsigned int numJoints() const override { return static_cast<unsigned>(joint_list_.size()); }
-  const std::string& getBaseLinkName() const override { return scene_graph_->getRoot(); }
-  const std::string& getTipLinkName() const override
-  {
-    return link_list_.back();
-  }  // TODO: Should make this be
-     // provided
-  const std::string& getName() const override { return name_; }
-  const std::string& getSolverName() const override { return solver_name_; }
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
+  unsigned int numJoints() const override;
+
+  const std::string& getBaseLinkName() const override;
+
+  const std::string& getTipLinkName() const override;  // TODO: Should make this be provided
+
+  const std::string& getName() const override;
+
+  const std::string& getSolverName() const override;
+
+  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
   /**
    * @brief Initializes Forward Kinematics as tree
@@ -121,15 +124,7 @@ class KDLFwdKinTree : public ForwardKinematics
    * @brief Checks if kinematics has been initialized
    * @return True if init() has completed successfully
    */
-  bool checkInitialized() const
-  {
-    if (!initialized_)
-    {
-      CONSOLE_BRIDGE_logError("Kinematics has not been initialized!");
-    }
-
-    return initialized_;
-  }
+  bool checkInitialized() const;
 
 private:
   bool initialized_{ false };                               /**< Identifies if the object has been initialized */
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
@@ -82,12 +82,19 @@ class KDLInvKinChainLMA : public InverseKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
-  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const { return scene_graph_; }
-  unsigned int numJoints() const override { return kdl_data_.robot_chain.getNrOfJoints(); }
-  const std::string& getBaseLinkName() const override { return kdl_data_.base_name; }
-  const std::string& getTipLinkName() const override { return kdl_data_.tip_name; }
-  const std::string& getName() const override { return name_; }
-  const std::string& getSolverName() const override { return solver_name_; }
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
+  unsigned int numJoints() const override;
+
+  const std::string& getBaseLinkName() const override;
+
+  const std::string& getTipLinkName() const override;
+
+  const std::string& getName() const override;
+
+  const std::string& getSolverName() const override;
+
+  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
   /**
    * @brief Initializes Inverse Kinematics as chain
@@ -119,15 +126,7 @@ class KDLInvKinChainLMA : public InverseKinematics
    * @brief Checks if kinematics has been initialized
    * @return True if init() has completed successfully
    */
-  bool checkInitialized() const
-  {
-    if (!initialized_)
-    {
-      CONSOLE_BRIDGE_logError("Kinematics has not been initialized!");
-    }
-
-    return initialized_;
-  }
+  bool checkInitialized() const;
 
 private:
   bool initialized_{ false };                               /**< Identifies if the object has been initialized */
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
@@ -84,12 +84,19 @@ class KDLInvKinChainNR : public InverseKinematics
 
   const tesseract_common::KinematicLimits& getLimits() const override;
 
-  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const { return scene_graph_; }
-  unsigned int numJoints() const override { return kdl_data_.robot_chain.getNrOfJoints(); }
-  const std::string& getBaseLinkName() const override { return kdl_data_.base_name; }
-  const std::string& getTipLinkName() const override { return kdl_data_.tip_name; }
-  const std::string& getName() const override { return name_; }
-  const std::string& getSolverName() const override { return solver_name_; }
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+
+  unsigned int numJoints() const override;
+
+  const std::string& getBaseLinkName() const override;
+
+  const std::string& getTipLinkName() const override;
+
+  const std::string& getName() const override;
+
+  const std::string& getSolverName() const override;
+
+  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
   /**
    * @brief Initializes KDL Forward Kinematics
@@ -121,15 +128,7 @@ class KDLInvKinChainNR : public InverseKinematics
    * @brief Checks if kinematics has been initialized
    * @return True if init() has completed successfully
    */
-  bool checkInitialized() const
-  {
-    if (!initialized_)
-    {
-      CONSOLE_BRIDGE_logError("Kinematics has not been initialized!");
-    }
-
-    return initialized_;
-  }
+  bool checkInitialized() const;
 
 private:
   bool initialized_{ false };                                  /**< Identifies if the object has been initialized */
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
@@ -67,14 +67,15 @@ class OPWInvKin : public InverseKinematics
   bool checkJoints(const Eigen::Ref<const Eigen::VectorXd>& vec) const override;
   unsigned int numJoints() const override;
 
-  const std::vector<std::string>& getJointNames() const override { return joint_names_; }
-  const std::vector<std::string>& getLinkNames() const override { return link_names_; }
-  const std::vector<std::string>& getActiveLinkNames() const override { return active_link_names_; }
-  const tesseract_common::KinematicLimits& getLimits() const override { return limits_; }
-  const std::string& getBaseLinkName() const override { return base_link_name_; }
-  const std::string& getTipLinkName() const override { return tip_link_name_; }
-  const std::string& getName() const override { return name_; }
-  const std::string& getSolverName() const override { return solver_name_; }
+  const std::vector<std::string>& getJointNames() const override;
+  const std::vector<std::string>& getLinkNames() const override;
+  const std::vector<std::string>& getActiveLinkNames() const override;
+  const tesseract_common::KinematicLimits& getLimits() const override;
+  void setLimits(tesseract_common::KinematicLimits limits) override;
+  const std::string& getBaseLinkName() const override;
+  const std::string& getTipLinkName() const override;
+  const std::string& getName() const override;
+  const std::string& getSolverName() const override;
 
   /**
    * @brief init Initialize OPW Inverse Kinematics
@@ -101,15 +102,7 @@ class OPWInvKin : public InverseKinematics
    * @brief Checks if kinematics has been initialized
    * @return True if init() has completed successfully
    */
-  bool checkInitialized() const
-  {
-    if (!initialized_)
-    {
-      CONSOLE_BRIDGE_logError("Kinematics has not been initialized!");
-    }
-
-    return initialized_;
-  }
+  bool checkInitialized() const;
 
 protected:
   bool initialized_{ false };                  /**< @brief Identifies if the object has been initialized */
diff --git a/tesseract/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp b/tesseract/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp
@@ -181,22 +181,51 @@ const std::vector<std::string>& RobotWithExternalPositionerInvKin::getActiveLink
 
 const tesseract_common::KinematicLimits& RobotWithExternalPositionerInvKin::getLimits() const { return limits_; }
 
-tesseract_scene_graph::SceneGraph::ConstPtr RobotWithExternalPositionerInvKin::getSceneGraph() const
+void RobotWithExternalPositionerInvKin::setLimits(tesseract_common::KinematicLimits limits)
 {
-  return scene_graph_;
-};
+  unsigned int nj = numJoints();
+  if (limits.joint_limits.size() != nj || limits.velocity_limits.size() != nj ||
+      limits.acceleration_limits.size() != nj)
+    throw std::runtime_error("Kinematics limits assigned are invalid!");
+
+  unsigned int pj = positioner_fwd_kin_->numJoints();
+  tesseract_common::KinematicLimits positioner_limits;
+  positioner_limits.joint_limits = limits.joint_limits.topRows(pj);
+  positioner_limits.velocity_limits = limits.velocity_limits.head(pj);
+  positioner_limits.acceleration_limits = limits.acceleration_limits.head(pj);
+  positioner_fwd_kin_->setLimits(positioner_limits);
+
+  unsigned int mj = manip_inv_kin_->numJoints();
+  tesseract_common::KinematicLimits manipulator_limits;
+  manipulator_limits.joint_limits = limits.joint_limits.bottomRows(mj);
+  manipulator_limits.velocity_limits = limits.velocity_limits.tail(mj);
+  manipulator_limits.acceleration_limits = limits.acceleration_limits.tail(mj);
+  manip_inv_kin_->setLimits(manipulator_limits);
+
+  limits_ = std::move(limits);
+}
+
 unsigned int RobotWithExternalPositionerInvKin::numJoints() const { return dof_; }
+
 const std::string& RobotWithExternalPositionerInvKin::getBaseLinkName() const
 {
   return positioner_fwd_kin_->getTipLinkName();
 }
+
 const std::string& RobotWithExternalPositionerInvKin::getTipLinkName() const
 {
   return manip_inv_kin_->getTipLinkName();
 }
+
 const std::string& RobotWithExternalPositionerInvKin::getName() const { return name_; }
+
 const std::string& RobotWithExternalPositionerInvKin::getSolverName() const { return solver_name_; }
 
+tesseract_scene_graph::SceneGraph::ConstPtr RobotWithExternalPositionerInvKin::getSceneGraph() const
+{
+  return scene_graph_;
+}
+
 bool RobotWithExternalPositionerInvKin::checkInitialized() const
 {
   if (!initialized_)
@@ -338,9 +367,9 @@ bool RobotWithExternalPositionerInvKin::init(tesseract_scene_graph::SceneGraph::
   manip_base_to_positioner_base_ = robot_to_positioner;
   scene_graph_ = std::move(scene_graph);
   name_ = std::move(name);
-  manip_inv_kin_ = std::move(manipulator);
+  manip_inv_kin_ = manipulator->clone();
   manip_reach_ = manipulator_reach;
-  positioner_fwd_kin_ = std::move(positioner);
+  positioner_fwd_kin_ = positioner->clone();
   positioner_sample_resolution_ = positioner_sample_resolution;
   dof_ = positioner_fwd_kin_->numJoints() + manip_inv_kin_->numJoints();
 
diff --git a/tesseract/tesseract_kinematics/src/core/rop_inverse_kinematics.cpp b/tesseract/tesseract_kinematics/src/core/rop_inverse_kinematics.cpp
@@ -181,13 +181,42 @@ const std::vector<std::string>& RobotOnPositionerInvKin::getActiveLinkNames() co
 
 const tesseract_common::KinematicLimits& RobotOnPositionerInvKin::getLimits() const { return limits_; }
 
-tesseract_scene_graph::SceneGraph::ConstPtr RobotOnPositionerInvKin::getSceneGraph() const { return scene_graph_; };
+void RobotOnPositionerInvKin::setLimits(tesseract_common::KinematicLimits limits)
+{
+  unsigned int nj = numJoints();
+  if (limits.joint_limits.size() != nj || limits.velocity_limits.size() != nj ||
+      limits.acceleration_limits.size() != nj)
+    throw std::runtime_error("Kinematics limits assigned are invalid!");
+
+  unsigned int pj = positioner_fwd_kin_->numJoints();
+  tesseract_common::KinematicLimits positioner_limits;
+  positioner_limits.joint_limits = limits.joint_limits.topRows(pj);
+  positioner_limits.velocity_limits = limits.velocity_limits.head(pj);
+  positioner_limits.acceleration_limits = limits.acceleration_limits.head(pj);
+  positioner_fwd_kin_->setLimits(positioner_limits);
+
+  unsigned int mj = manip_inv_kin_->numJoints();
+  tesseract_common::KinematicLimits manipulator_limits;
+  manipulator_limits.joint_limits = limits.joint_limits.bottomRows(mj);
+  manipulator_limits.velocity_limits = limits.velocity_limits.tail(mj);
+  manipulator_limits.acceleration_limits = limits.acceleration_limits.tail(mj);
+  manip_inv_kin_->setLimits(manipulator_limits);
+
+  limits_ = std::move(limits);
+}
+
 unsigned int RobotOnPositionerInvKin::numJoints() const { return dof_; }
+
 const std::string& RobotOnPositionerInvKin::getBaseLinkName() const { return positioner_fwd_kin_->getBaseLinkName(); }
+
 const std::string& RobotOnPositionerInvKin::getTipLinkName() const { return manip_inv_kin_->getTipLinkName(); }
+
 const std::string& RobotOnPositionerInvKin::getName() const { return name_; }
+
 const std::string& RobotOnPositionerInvKin::getSolverName() const { return solver_name_; }
 
+tesseract_scene_graph::SceneGraph::ConstPtr RobotOnPositionerInvKin::getSceneGraph() const { return scene_graph_; };
+
 bool RobotOnPositionerInvKin::checkInitialized() const
 {
   if (!initialized_)
@@ -262,9 +291,9 @@ bool RobotOnPositionerInvKin::init(tesseract_scene_graph::SceneGraph::ConstPtr s
 
   scene_graph_ = std::move(scene_graph);
   name_ = std::move(name);
-  manip_inv_kin_ = std::move(manipulator);
+  manip_inv_kin_ = manipulator->clone();
   manip_reach_ = manipulator_reach;
-  positioner_fwd_kin_ = std::move(positioner);
+  positioner_fwd_kin_ = positioner->clone();
   positioner_sample_resolution_ = positioner_sample_resolution;
   dof_ = positioner_fwd_kin_->numJoints() + manip_inv_kin_->numJoints();
 
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_chain.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_chain.cpp
diff --git a/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_tree.cpp b/tesseract/tesseract_kinematics/src/kdl/kdl_fwd_kin_tree.cpp
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
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
diff --git a/tesseract/tesseract_kinematics/src/opw/opw_inv_kin.cpp b/tesseract/tesseract_kinematics/src/opw/opw_inv_kin.cpp
