From 057ae14672e98e92d05c45bc1abea110868b56d4 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Wed, 23 Feb 2022 21:01:45 -0600
Subject: [PATCH] Add overload method for calcInvKin to take single
 KinGroupIKInput

---
 .../tesseract_kinematics/core/kinematic_group.h       | 11 ++++++++++-
 tesseract_kinematics/core/src/kinematic_group.cpp     |  6 ++++++
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h
index 62f32c78fca..7f66182062a 100644
--- a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h
+++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h
@@ -119,7 +119,6 @@ class KinematicGroup : public JointGroup
   /**
    * @brief Calculates joint solutions given a pose.
    * @details If redundant solutions are needed see utility function getRedundantSolutions.
-   * @param solutions A vector of solutions, so check the size of the vector to determine the number of solutions
    * @param tip_link_poses The input information to solve inverse kinematics for. There must be an input for each link
    * provided in getTipLinkNames
    * @param seed Vector of seed joint angles (size must match number of joints in robot chain)
@@ -127,6 +126,16 @@ class KinematicGroup : public JointGroup
    */
   IKSolutions calcInvKin(const KinGroupIKInputs& tip_link_poses, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
 
+  /**
+   * @brief Calculates joint solutions given a pose.
+   * @details If redundant solutions are needed see utility function getRedundantSolutions.
+   * @param tip_link_pose The input information to solve inverse kinematics for. This is a convenience function for
+   * when only one tip link exists
+   * @param seed Vector of seed joint angles (size must match number of joints in robot chain)
+   * @return A vector of solutions, If empty it failed to find a solution (including uninitialized)
+   */
+  IKSolutions calcInvKin(const KinGroupIKInput& tip_link_pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
+
   /** @brief Returns all possible working frames in which goal poses can be defined
    * @details The inverse kinematics solver requires that all poses be defined relative to a single working frame.
    * However if this working frame is static, a pose can be defined in another static frame in the environment and
diff --git a/tesseract_kinematics/core/src/kinematic_group.cpp b/tesseract_kinematics/core/src/kinematic_group.cpp
index e6caa3b98cd..df4b3357665 100644
--- a/tesseract_kinematics/core/src/kinematic_group.cpp
+++ b/tesseract_kinematics/core/src/kinematic_group.cpp
@@ -174,6 +174,12 @@ IKSolutions KinematicGroup::calcInvKin(const KinGroupIKInputs& tip_link_poses,
   return solutions_filtered;
 }
 
+IKSolutions KinematicGroup::calcInvKin(const KinGroupIKInput& tip_link_pose,
+                                       const Eigen::Ref<const Eigen::VectorXd>& seed) const
+{
+  return calcInvKin(KinGroupIKInputs{ tip_link_pose }, seed);
+}
+
 std::vector<std::string> KinematicGroup::getAllValidWorkingFrames() const { return working_frames_; }
 
 std::vector<std::string> KinematicGroup::getAllPossibleTipLinkNames() const