prbt_manipulator_ikfast_moveit_plugin.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer
 *IPA
 * All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the all of the author's companies nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/
 
/*
 * IKFast Kinematics Solver Plugin for MoveIt wrapping an ikfast solver from OpenRAVE.
 *
 * AUTO-GENERATED by create_ikfast_moveit_plugin.py in moveit_kinematics package.
 *
 * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt kinematics plugin.
 */
 
 
#include <Eigen/Geometry>
#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter_value.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_kdl/tf2_kdl.hpp>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_state/robot_state.h>
 
 
using namespace moveit::core;
 
// Need a floating point tolerance when checking joint limits, in case the joint starts at limit
const double LIMIT_TOLERANCE = .0000001;
enum SEARCH_MODE
{
  OPTIMIZE_FREE_JOINT = 1,
  OPTIMIZE_MAX_JOINT = 2
};
 
namespace prbt_manipulator
{
#define IKFAST_NO_MAIN  // Don't include main() from IKFast
 
enum IkParameterizationType
{
  IKP_None = 0,
  IKP_Transform6D = 0x67000001,    
  IKP_Rotation3D = 0x34000002,     
  IKP_Translation3D = 0x33000003,  
  IKP_Direction3D = 0x23000004,    
  IKP_Ray4D = 0x46000005,          
  IKP_Lookat3D = 0x23000006,       
  IKP_TranslationDirection5D = 0x56000007,  
  IKP_TranslationXY2D = 0x22000008,             
  IKP_TranslationXYOrientation3D = 0x33000009,  
  IKP_TranslationLocalGlobal6D = 0x3600000a,  
 
  IKP_TranslationXAxisAngle4D = 0x4400000b,  
  IKP_TranslationYAxisAngle4D = 0x4400000c,  
  IKP_TranslationZAxisAngle4D = 0x4400000d,  
 
  IKP_TranslationXAxisAngleZNorm4D = 0x4400000e,  
  IKP_TranslationYAxisAngleXNorm4D = 0x4400000f,  
  IKP_TranslationZAxisAngleYNorm4D = 0x44000010,  
 
  IKP_NumberOfParameterizations = 16,  
 
  IKP_VelocityDataBit =
      0x00008000,  
  IKP_Transform6DVelocity = IKP_Transform6D | IKP_VelocityDataBit,
  IKP_Rotation3DVelocity = IKP_Rotation3D | IKP_VelocityDataBit,
  IKP_Translation3DVelocity = IKP_Translation3D | IKP_VelocityDataBit,
  IKP_Direction3DVelocity = IKP_Direction3D | IKP_VelocityDataBit,
  IKP_Ray4DVelocity = IKP_Ray4D | IKP_VelocityDataBit,
  IKP_Lookat3DVelocity = IKP_Lookat3D | IKP_VelocityDataBit,
  IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D | IKP_VelocityDataBit,
  IKP_TranslationXY2DVelocity = IKP_TranslationXY2D | IKP_VelocityDataBit,
  IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D | IKP_VelocityDataBit,
  IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D | IKP_VelocityDataBit,
  IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D | IKP_VelocityDataBit,
  IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D | IKP_VelocityDataBit,
  IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D | IKP_VelocityDataBit,
  IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D | IKP_VelocityDataBit,
  IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D | IKP_VelocityDataBit,
  IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D | IKP_VelocityDataBit,
 
  IKP_UniqueIdMask = 0x0000ffff,   
  IKP_CustomDataBit = 0x00010000,  
};
 
// struct for storing and sorting solutions
struct LimitObeyingSol
{
  std::vector<double> value;
  double dist_from_seed;
 
  bool operator<(const LimitObeyingSol& a) const
  {
    return dist_from_seed < a.dist_from_seed;
  }
};
 
static const rclcpp::Logger LOGGER = rclcpp::get_logger("ikfast");
 
// Code generated by IKFast56/61
#include "prbt_manipulator_ikfast_solver.cpp"
 
 
class IKFastKinematicsPlugin : public kinematics::KinematicsBase
{
  std::vector<std::string> joint_names_;
  std::vector<double> joint_min_vector_;
  std::vector<double> joint_max_vector_;
  std::vector<bool> joint_has_limits_vector_;
  std::vector<std::string> link_names_;
  const size_t num_joints_;
  std::vector<int> free_params_;
 
  // The ikfast and base frame are the start and end of the kinematic chain for which the
  // IKFast analytic solution was generated.
  const std::string IKFAST_TIP_FRAME_ = "flange";
  const std::string IKFAST_BASE_FRAME_ = "base_link";
 
  // prefix added to tip- and baseframe to allow different namespaces or multi-robot setups
  std::string link_prefix_;
 
  // The transform tip and base bool are set to true if this solver is used with a kinematic
  // chain that extends beyond the ikfast tip and base frame. The solution will be valid so
  // long as there are no active, passive, or mimic joints between either the ikfast_tip_frame
  // and the tip_frame of the group or the ikfast_base_frame and the base_frame for the group.
  bool tip_transform_required_;
  bool base_transform_required_;
 
  // We store the transform from the ikfast_base_frame to the group base_frame as well as the
  // ikfast_tip_frame to the group tip_frame to transform input poses into the solver frame.
  Eigen::Isometry3d chain_base_to_group_base_;
  Eigen::Isometry3d group_tip_to_chain_tip_;
 
  bool initialized_;  // Internal variable that indicates whether solvers are configured and ready
 
  const std::vector<std::string>& getJointNames() const override
  {
    return joint_names_;
  }
  const std::vector<std::string>& getLinkNames() const override
  {
    return link_names_;
  }
 
public:
  IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), initialized_(false)
  {
    srand(time(nullptr));
    supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
    supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
    supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
  }
 
  // Returns the IK solution that is within joint limits closest to ik_seed_state
  bool getPositionIK(
      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, std::vector<double>& solution,
      moveit_msgs::msg::MoveItErrorCodes& error_code,
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
 
  bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
                     std::vector<std::vector<double>>& solutions, kinematics::KinematicsResult& result,
                     const kinematics::KinematicsQueryOptions& options) const override;
 
  bool searchPositionIK(
      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
      std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
 
  bool searchPositionIK(
      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
      const std::vector<double>& consistency_limits, std::vector<double>& solution,
      moveit_msgs::msg::MoveItErrorCodes& error_code,
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
 
  bool searchPositionIK(
      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
      std::vector<double>& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
 
  bool searchPositionIK(
      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
      const std::vector<double>& consistency_limits, std::vector<double>& solution,
      const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
 
  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
                     std::vector<geometry_msgs::msg::Pose>& poses) const override;
 
  void setSearchDiscretization(const std::map<unsigned int, double>& discretization);
 
  bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices) override;
 
private:
  bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, const std::string& group_name,
                  const std::string& base_frame, const std::vector<std::string>& tip_frames,
                  double search_discretization) override;
 
  size_t solve(KDL::Frame& pose_frame, const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions) const;
 
  void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;
 
  void getSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state, int i,
                   std::vector<double>& solution) const;
 
  double enforceLimits(double val, double min, double max) const;
 
  void fillFreeParams(int count, int* array);
  bool getCount(int& count, const int& max_count, const int& min_count) const;
 
  bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
 
  bool computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform,
                                bool& differs_from_identity);
  void transformToChainFrame(const geometry_msgs::msg::Pose& ik_pose, KDL::Frame& ik_pose_chain) const;
};  // end class
 
bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, const std::string& to,
                                                      Eigen::Isometry3d& transform, bool& differs_from_identity)
{
  RobotStatePtr robot_state = std::make_shared<RobotState>(robot_model_);
  robot_state->setToDefaultValues();
 
  auto* from_link = robot_model_->getLinkModel(from);
  auto* to_link = robot_model_->getLinkModel(to);
  if (!from_link || !to_link)
    return false;
 
  if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) !=
      robot_model_->getRigidlyConnectedParentLinkModel(to_link))
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Link frames " << from << " and " << to << " are not rigidly connected.");
    return false;
  }
 
  transform = robot_state->getGlobalLinkTransform(from_link).inverse() * robot_state->getGlobalLinkTransform(to_link);
  differs_from_identity = !transform.matrix().isIdentity();
  return true;
}
 
bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, const std::string& group_name,
                                        const std::string& base_frame, const std::vector<std::string>& tip_frames,
                                        double search_discretization)
{
  if (tip_frames.size() != 1)
  {
    RCLCPP_ERROR(LOGGER, "Expecting exactly one tip frame.");
    return false;
  }
 
  storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
  if (!lookupParam(node, "link_prefix", link_prefix_, std::string("")))
  {
    RCLCPP_INFO(LOGGER, "Using empty link_prefix.");
  }
  else
  {
    RCLCPP_INFO_STREAM(LOGGER, "Using link_prefix: '" << link_prefix_ << "'");
  }
 
  // verbose error output. subsequent checks in computeRelativeTransform return false then
  if (!robot_model.hasLinkModel(tip_frames_[0]))
    RCLCPP_ERROR_STREAM(LOGGER, "tip frame '" << tip_frames_[0] << "' does not exist.");
  if (!robot_model.hasLinkModel(base_frame_))
    RCLCPP_ERROR_STREAM(LOGGER, "base_frame '" << base_frame_ << "' does not exist.");
 
  if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_))
    RCLCPP_ERROR_STREAM(LOGGER, "prefixed tip frame '" << link_prefix_ + IKFAST_TIP_FRAME_
                                                       << "' does not exist. "
                                                          "Please check your link_prefix parameter.");
  if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_))
    RCLCPP_ERROR_STREAM(LOGGER, "prefixed base frame '" << link_prefix_ + IKFAST_BASE_FRAME_
                                                        << "' does not exist. "
                                                           "Please check your link_prefix parameter.");
  // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_.
  // It is often the case that fixed joints are added to these links to model things like
  // a robot mounted on a table or a robot with an end effector attached to the last link.
  // To support these use cases, we store the transform from the IKFAST_BASE_FRAME_ to the
  // base_frame_ and IKFAST_TIP_FRAME_ the tip_frame_ and transform to the input pose accordingly
  if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_,
                                tip_transform_required_) ||
      !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_,
                                base_transform_required_))
  {
    return false;
  }
 
  // IKFast56/61
  fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
 
  if (free_params_.size() > 1)
  {
    RCLCPP_ERROR(LOGGER, "Only one free joint parameter supported!");
    return false;
  }
  else if (free_params_.size() == 1)
  {
    redundant_joint_indices_.clear();
    redundant_joint_indices_.push_back(free_params_[0]);
    KinematicsBase::setSearchDiscretization(search_discretization);
  }
 
  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name);
  if (!jmg)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Unknown planning group: " << group_name);
    return false;
  }
 
  RCLCPP_DEBUG_STREAM(LOGGER, "Registering joints and links");
  const moveit::core::LinkModel* link = robot_model_->getLinkModel(tip_frames_[0]);
  const moveit::core::LinkModel* base_link = robot_model_->getLinkModel(base_frame_);
  while (link && link != base_link)
  {
    RCLCPP_DEBUG_STREAM(LOGGER, "Link " << link->getName());
    link_names_.push_back(link->getName());
    const moveit::core::JointModel* joint = link->getParentJointModel();
    if (joint->getType() != joint->UNKNOWN && joint->getType() != joint->FIXED && joint->getVariableCount() == 1)
    {
      RCLCPP_DEBUG_STREAM(LOGGER, "Adding joint " << joint->getName());
 
      joint_names_.push_back(joint->getName());
      const moveit::core::VariableBounds& bounds = joint->getVariableBounds()[0];
      joint_has_limits_vector_.push_back(bounds.position_bounded_);
      joint_min_vector_.push_back(bounds.min_position_);
      joint_max_vector_.push_back(bounds.max_position_);
    }
    link = link->getParentLinkModel();
  }
 
  if (joint_names_.size() != num_joints_)
  {
    RCLCPP_FATAL(LOGGER, "Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match", joint_names_.size(),
              num_joints_);
    return false;
  }
 
  std::reverse(link_names_.begin(), link_names_.end());
  std::reverse(joint_names_.begin(), joint_names_.end());
  std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
  std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
  std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
 
  for (size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
    RCLCPP_DEBUG_STREAM(LOGGER, joint_names_[joint_id] << " " << joint_min_vector_[joint_id] << " "
                                                    << joint_max_vector_[joint_id] << " "
                                                    << joint_has_limits_vector_[joint_id]);
 
  initialized_ = true;
  return true;
}
 
void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<unsigned int, double>& discretization)
{
  if (discretization.empty())
  {
    RCLCPP_ERROR(LOGGER, "The 'discretization' map is empty");
    return;
  }
 
  if (redundant_joint_indices_.empty())
  {
    RCLCPP_ERROR_STREAM(LOGGER, "This group's solver doesn't support redundant joints");
    return;
  }
 
  if (discretization.begin()->first != redundant_joint_indices_[0])
  {
    std::string redundant_joint = joint_names_[free_params_[0]];
    RCLCPP_ERROR_STREAM(LOGGER, "Attempted to discretize a non-redundant joint "
                                    << discretization.begin()->first << ", only joint '" << redundant_joint
                                    << "' with index " << redundant_joint_indices_[0] << " is redundant.");
    return;
  }
 
  if (discretization.begin()->second <= 0.0)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Discretization can not takes values that are <= 0");
    return;
  }
 
  redundant_joint_discretization_.clear();
  redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
}
 
bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& /* unused */)
{
  RCLCPP_ERROR_STREAM(LOGGER, "Changing the redundant joints isn't permitted by this group's solver ");
  return false;
}
 
size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector<double>& vfree,
                                     IkSolutionList<IkReal>& solutions) const
{
  // IKFast56/61
  solutions.Clear();
 
  double trans[3];
  trans[0] = pose_frame.p[0];  //-.18;
  trans[1] = pose_frame.p[1];
  trans[2] = pose_frame.p[2];
 
  KDL::Rotation mult;
  KDL::Vector direction;
 
  switch (GetIkType())
  {
    case IKP_Transform6D:
    case IKP_Translation3D:
      // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
 
      mult = pose_frame.M;
 
      double vals[9];
      vals[0] = mult(0, 0);
      vals[1] = mult(0, 1);
      vals[2] = mult(0, 2);
      vals[3] = mult(1, 0);
      vals[4] = mult(1, 1);
      vals[5] = mult(1, 2);
      vals[6] = mult(2, 0);
      vals[7] = mult(2, 1);
      vals[8] = mult(2, 2);
 
      // IKFast56/61
      ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
      return solutions.GetNumSolutions();
 
    case IKP_Direction3D:
    case IKP_Ray4D:
    case IKP_TranslationDirection5D:
      // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
      // direction.
 
      direction = pose_frame.M * KDL::Vector(0, 0, 1);
      ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
      return solutions.GetNumSolutions();
 
    case IKP_TranslationXAxisAngle4D:
    case IKP_TranslationYAxisAngle4D:
    case IKP_TranslationZAxisAngle4D:
      // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin
      // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the
      // manipulator base link’s coordinate system)
      RCLCPP_ERROR(LOGGER, "IK for this IkParameterizationType not implemented yet.");
      return 0;
 
    case IKP_TranslationLocalGlobal6D:
      // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
      // effector coordinate system.
      RCLCPP_ERROR(LOGGER, "IK for this IkParameterizationType not implemented yet.");
      return 0;
 
    case IKP_Rotation3D:
    case IKP_Lookat3D:
    case IKP_TranslationXY2D:
    case IKP_TranslationXYOrientation3D:
      RCLCPP_ERROR(LOGGER, "IK for this IkParameterizationType not implemented yet.");
      return 0;
 
    case IKP_TranslationXAxisAngleZNorm4D:
      double roll, pitch, yaw;
      // For **TranslationXAxisAngleZNorm4D** - end effector origin reaches desired 3D translation, manipulator
      // direction needs to be orthogonal to z axis and be rotated at a certain angle starting from the x axis (defined
      // in the manipulator base link’s coordinate system)
      pose_frame.M.GetRPY(roll, pitch, yaw);
      ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
      return solutions.GetNumSolutions();
 
    case IKP_TranslationYAxisAngleXNorm4D:
      // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator
      // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined
      // in the manipulator base link’s coordinate system)
      pose_frame.M.GetRPY(roll, pitch, yaw);
      ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
      return solutions.GetNumSolutions();
 
    case IKP_TranslationZAxisAngleYNorm4D:
      // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator
      // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined
      // in the manipulator base link’s coordinate system)
      pose_frame.M.GetRPY(roll, pitch, yaw);
      ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
      return solutions.GetNumSolutions();
 
    default:
      RCLCPP_ERROR(LOGGER, "Unknown IkParameterizationType! "
                        "Was the solver generated with an incompatible version of Openrave?");
      return 0;
  }
}
 
void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
                                         std::vector<double>& solution) const
{
  solution.clear();
  solution.resize(num_joints_);
 
  // IKFast56/61
  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
  std::vector<IkReal> vsolfree(sol.GetFree().size());
  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : nullptr);
 
  for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
  {
    if (joint_has_limits_vector_[joint_id])
    {
      solution[joint_id] = enforceLimits(solution[joint_id], joint_min_vector_[joint_id], joint_max_vector_[joint_id]);
    }
  }
}
 
void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions,
                                         const std::vector<double>& ik_seed_state, int i,
                                         std::vector<double>& solution) const
{
  solution.clear();
  solution.resize(num_joints_);
 
  // IKFast56/61
  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
  std::vector<IkReal> vsolfree(sol.GetFree().size());
  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : nullptr);
 
  // rotate joints by +/-360° where it is possible and useful
  for (std::size_t i = 0; i < num_joints_; ++i)
  {
    if (joint_has_limits_vector_[i])
    {
      solution[i] = enforceLimits(solution[i], joint_min_vector_[i], joint_max_vector_[i]);
      double signed_distance = solution[i] - ik_seed_state[i];
      while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE))
      {
        signed_distance -= 2 * M_PI;
        solution[i] -= 2 * M_PI;
      }
      while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE))
      {
        signed_distance += 2 * M_PI;
        solution[i] += 2 * M_PI;
      }
    }
  }
}
 
double IKFastKinematicsPlugin::enforceLimits(double joint_value, double min, double max) const
{
  // If the joint_value is greater than max subtract 2 * PI until it is less than the max
  while (joint_value > max)
  {
    joint_value -= 2 * M_PI;
  }
 
  // If the joint_value is less than the min, add 2 * PI until it is more than the min
  while (joint_value < min)
  {
    joint_value += 2 * M_PI;
  }
  return joint_value;
}
 
void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
{
  free_params_.clear();
  for (int i = 0; i < count; ++i)
    free_params_.push_back(array[i]);
}
 
bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
{
  if (count > 0)
  {
    if (-count >= min_count)
    {
      count = -count;
      return true;
    }
    else if (count + 1 <= max_count)
    {
      count = count + 1;
      return true;
    }
    else
    {
      return false;
    }
  }
  else
  {
    if (1 - count <= max_count)
    {
      count = 1 - count;
      return true;
    }
    else if (count - 1 >= min_count)
    {
      count = count - 1;
      return true;
    }
    else
      return false;
  }
}
 
bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
                                           const std::vector<double>& joint_angles,
                                           std::vector<geometry_msgs::msg::Pose>& poses) const
{
  if (GetIkType() != IKP_Transform6D)
  {
    // ComputeFk() is the inverse function of ComputeIk(), so the format of
    // eerot differs depending on IK type. The Transform6D IK type is the only
    // one for which a 3x3 rotation matrix is returned, which means we can only
    // compute FK for that IK type.
    RCLCPP_ERROR(LOGGER, "Can only compute FK for Transform6D IK type!");
    return false;
  }
 
  KDL::Frame p_out;
  if (link_names.size() == 0)
  {
    RCLCPP_WARN_STREAM(LOGGER, "Link names with nothing");
    return false;
  }
 
  if (link_names.size() != 1 || link_names[0] != getTipFrame())
  {
    RCLCPP_ERROR(LOGGER, "Can compute FK for %s only", getTipFrame().c_str());
    return false;
  }
 
  bool valid = true;
 
  IkReal eerot[9], eetrans[3];
 
  if (joint_angles.size() != num_joints_)
  {
    RCLCPP_ERROR(LOGGER, "Unexpected number of joint angles");
    return false;
  }
 
  std::vector<IkReal> angles(num_joints_, 0);
  for (unsigned char i = 0; i < num_joints_; i++)
    angles[i] = joint_angles[i];
 
  // IKFast56/61
  ComputeFk(angles.data(), eetrans, eerot);
 
  for (int i = 0; i < 3; ++i)
    p_out.p.data[i] = eetrans[i];
 
  for (int i = 0; i < 9; ++i)
    p_out.M.data[i] = eerot[i];
 
  poses.resize(1);
  poses[0] = tf2::toMsg(p_out);
 
  return valid;
}
 
bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
                                              const std::vector<double>& ik_seed_state, double timeout,
                                              std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
                                              const kinematics::KinematicsQueryOptions& options) const
{
  std::vector<double> consistency_limits;
  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
                          options);
}
 
bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
                                              const std::vector<double>& ik_seed_state, double timeout,
                                              const std::vector<double>& consistency_limits,
                                              std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
                                              const kinematics::KinematicsQueryOptions& options) const
{
  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
                          options);
}
 
bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
                                              const std::vector<double>& ik_seed_state, double timeout,
                                              std::vector<double>& solution, const IKCallbackFn& solution_callback,
                                              moveit_msgs::msg::MoveItErrorCodes& error_code,
                                              const kinematics::KinematicsQueryOptions& options) const
{
  std::vector<double> consistency_limits;
  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
                          options);
}
 
bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
                                              const std::vector<double>& ik_seed_state, double /* unused */,
                                              const std::vector<double>& consistency_limits,
                                              std::vector<double>& solution, const IKCallbackFn& solution_callback,
                                              moveit_msgs::msg::MoveItErrorCodes& error_code,
                                              const kinematics::KinematicsQueryOptions& options) const
{
  // "SEARCH_MODE" is fixed during code generation
  SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
 
  // Check if there are no redundant joints
  if (free_params_.size() == 0)
  {
    RCLCPP_DEBUG_STREAM(LOGGER, "No need to search since no free params/redundant joints");
 
    std::vector<geometry_msgs::msg::Pose> ik_poses(1, ik_pose);
    std::vector<std::vector<double>> solutions;
    kinematics::KinematicsResult kinematic_result;
    // Find all IK solutions within joint limits
    if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
    {
      RCLCPP_DEBUG_STREAM(LOGGER, "No solution whatsoever");
      error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
      return false;
    }
 
    // sort solutions by their distance to the seed
    std::vector<LimitObeyingSol> solutions_obey_limits;
    for (std::size_t i = 0; i < solutions.size(); ++i)
    {
      double dist_from_seed = 0.0;
      for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
      {
        dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
      }
 
      solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
    }
    std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
 
    // check for collisions if a callback is provided
    if (solution_callback)
    {
      for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
      {
        solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
        if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
        {
          solution = solutions_obey_limits[i].value;
          RCLCPP_DEBUG_STREAM(LOGGER, "Solution passes callback");
          return true;
        }
      }
 
      RCLCPP_DEBUG_STREAM(LOGGER, "Solution has error code " << error_code.val);
      return false;
    }
    else
    {
      solution = solutions_obey_limits[0].value;
      error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
      return true;  // no collision check callback provided
    }
  }
 
  // -------------------------------------------------------------------------------------------------
  // Error Checking
  if (!initialized_)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Kinematics not active");
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }
 
  if (ik_seed_state.size() != num_joints_)
  {
    RCLCPP_ERROR_STREAM(LOGGER,
                        "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }
 
  if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Consistency limits be empty or must have size " << num_joints_ << " instead of size "
                                                                                 << consistency_limits.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }
 
  // -------------------------------------------------------------------------------------------------
  // Initialize
 
  KDL::Frame frame;
  transformToChainFrame(ik_pose, frame);
 
  std::vector<double> vfree(free_params_.size());
 
  int counter = 0;
 
  double initial_guess = ik_seed_state[free_params_[0]];
  vfree[0] = initial_guess;
 
  // -------------------------------------------------------------------------------------------------
  // Handle consistency limits if needed
  int num_positive_increments;
  int num_negative_increments;
  double search_discretization = redundant_joint_discretization_.at(free_params_[0]);
 
  if (!consistency_limits.empty())
  {
    // MoveIt replaced consistency_limit (scalar) w/ consistency_limits (vector)
    // Assume [0]th free_params element for now.  Probably wrong.
    double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
    double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
 
    num_positive_increments = static_cast<int>((max_limit - initial_guess) / search_discretization);
    num_negative_increments = static_cast<int>((initial_guess - min_limit) / search_discretization);
  }
  else  // no consistency limits provided
  {
    num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization;
    num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization;
  }
 
  // -------------------------------------------------------------------------------------------------
  // Begin searching
 
  RCLCPP_DEBUG_STREAM(LOGGER, "Free param is " << free_params_[0] << " initial guess is " << initial_guess
                                            << ", # positive increments: " << num_positive_increments
                                            << ", # negative increments: " << num_negative_increments);
  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
    RCLCPP_WARN_STREAM_ONCE(LOGGER, "Large search space, consider increasing the search discretization");
 
  double best_costs = -1.0;
  std::vector<double> best_solution;
  int nattempts = 0, nvalid = 0;
 
  while (true)
  {
    IkSolutionList<IkReal> solutions;
    size_t numsol = solve(frame, vfree, solutions);
 
    RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast");
 
    if (numsol > 0)
    {
      for (size_t s = 0; s < numsol; ++s)
      {
        nattempts++;
        std::vector<double> sol;
        getSolution(solutions, ik_seed_state, s, sol);
 
        bool obeys_limits = true;
        for (size_t i = 0; i < sol.size(); i++)
        {
          if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
          {
            obeys_limits = false;
            break;
          }
          // RCLCPP_INFO_STREAM(LOGGER,"Num " << i << " value " << sol[i] << " has limits " <<
          // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
        }
        if (obeys_limits)
        {
          getSolution(solutions, ik_seed_state, s, solution);
 
          // This solution is within joint limits, now check if in collision (if callback provided)
          if (solution_callback)
          {
            solution_callback(ik_pose, solution, error_code);
          }
          else
          {
            error_code.val = error_code.SUCCESS;
          }
 
          if (error_code.val == error_code.SUCCESS)
          {
            nvalid++;
            if (search_mode & OPTIMIZE_MAX_JOINT)
            {
              // Costs for solution: Largest joint motion
              double costs = 0.0;
              for (unsigned int i = 0; i < solution.size(); i++)
              {
                double d = fabs(ik_seed_state[i] - solution[i]);
                if (d > costs)
                  costs = d;
              }
              if (costs < best_costs || best_costs == -1.0)
              {
                best_costs = costs;
                best_solution = solution;
              }
            }
            else
              // Return first feasible solution
              return true;
          }
        }
      }
    }
 
    if (!getCount(counter, num_positive_increments, -num_negative_increments))
    {
      // Everything searched
      error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
      break;
    }
 
    vfree[0] = initial_guess + search_discretization * counter;
    // RCLCPP_DEBUG_STREAM(LOGGER,"Attempt " << counter << " with 0th free joint having value " << vfree[0]);
  }
 
  RCLCPP_DEBUG_STREAM(LOGGER, "Valid solutions: " << nvalid << "/" << nattempts);
 
  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
  {
    solution = best_solution;
    error_code.val = error_code.SUCCESS;
    return true;
  }
 
  // No solution found
  error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
  return false;
}
 
// Used when there are no redundant joints - aka no free params
bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
                                           std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
                                           const kinematics::KinematicsQueryOptions& /* unused */) const
{
  RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK");
 
  if (!initialized_)
  {
    RCLCPP_ERROR(LOGGER, "kinematics not active");
    return false;
  }
 
  if (ik_seed_state.size() < num_joints_)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size()
                                                          << " entries, this ikfast solver requires " << num_joints_);
    return false;
  }
 
  // Check if seed is in bound
  for (std::size_t i = 0; i < ik_seed_state.size(); i++)
  {
    // Add tolerance to limit check
    if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
                                        (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
    {
      RCLCPP_DEBUG_STREAM(LOGGER, "IK seed not in limits! " << static_cast<int>(i) << " value " << ik_seed_state[i]
                                                         << " has limit: " << joint_has_limits_vector_[i] << "  being  "
                                                         << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
      return false;
    }
  }
 
  std::vector<double> vfree(free_params_.size());
  for (std::size_t i = 0; i < free_params_.size(); ++i)
  {
    int p = free_params_[i];
    RCLCPP_ERROR(LOGGER, "%u is %f", p, ik_seed_state[p]);  // DTC
    vfree[i] = ik_seed_state[p];
  }
 
  KDL::Frame frame;
  transformToChainFrame(ik_pose, frame);
 
  IkSolutionList<IkReal> solutions;
  size_t numsol = solve(frame, vfree, solutions);
  RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast");
 
  std::vector<LimitObeyingSol> solutions_obey_limits;
 
  if (numsol)
  {
    std::vector<double> solution_obey_limits;
    for (std::size_t s = 0; s < numsol; ++s)
    {
      std::vector<double> sol;
      getSolution(solutions, ik_seed_state, s, sol);
      RCLCPP_DEBUG(LOGGER, "Sol %d: %e   %e   %e   %e   %e   %e", static_cast<int>(s), sol[0], sol[1], sol[2], sol[3],
                sol[4], sol[5]);
 
      bool obeys_limits = true;
      for (std::size_t i = 0; i < sol.size(); i++)
      {
        // Add tolerance to limit check
        if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
                                            (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
        {
          // One element of solution is not within limits
          obeys_limits = false;
          RCLCPP_DEBUG_STREAM(LOGGER, "Not in limits! " << static_cast<int>(i) << " value " << sol[i]
                                                     << " has limit: " << joint_has_limits_vector_[i] << "  being  "
                                                     << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
          break;
        }
      }
      if (obeys_limits)
      {
        // All elements of this solution obey limits
        getSolution(solutions, ik_seed_state, s, solution_obey_limits);
        double dist_from_seed = 0.0;
        for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
        {
          dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
        }
 
        solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
      }
    }
  }
  else
  {
    RCLCPP_DEBUG_STREAM(LOGGER, "No IK solution");
  }
 
  // Sort the solutions under limits and find the one that is closest to ik_seed_state
  if (!solutions_obey_limits.empty())
  {
    std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
    solution = solutions_obey_limits[0].value;
    error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
    return true;
  }
 
  error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
  return false;
}
 
bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
                                           const std::vector<double>& ik_seed_state,
                                           std::vector<std::vector<double>>& solutions,
                                           kinematics::KinematicsResult& result,
                                           const kinematics::KinematicsQueryOptions& options) const
{
  RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK with multiple solutions");
 
  if (!initialized_)
  {
    RCLCPP_ERROR(LOGGER, "kinematics not active");
    result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE;
    return false;
  }
 
  if (ik_poses.empty())
  {
    RCLCPP_ERROR(LOGGER, "ik_poses is empty");
    result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
    return false;
  }
 
  if (ik_poses.size() > 1)
  {
    RCLCPP_ERROR(LOGGER, "ik_poses contains multiple entries, only one is allowed");
    result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
    return false;
  }
 
  if (ik_seed_state.size() < num_joints_)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size()
                                                          << " entries, this ikfast solver requires " << num_joints_);
    return false;
  }
 
  KDL::Frame frame;
  transformToChainFrame(ik_poses[0], frame);
 
  // solving ik
  std::vector<IkSolutionList<IkReal>> solution_set;
  IkSolutionList<IkReal> ik_solutions;
  std::vector<double> vfree;
  int numsol = 0;
  std::vector<double> sampled_joint_vals;
  if (!redundant_joint_indices_.empty())
  {
    // initializing from seed
    sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
 
    // checking joint limits when using no discretization
    if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION &&
        joint_has_limits_vector_[redundant_joint_indices_.front()])
    {
      double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
      double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
 
      double jv = sampled_joint_vals[0];
      if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
      {
        result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS;
        RCLCPP_ERROR_STREAM(LOGGER, "ik seed is out of bounds");
        return false;
      }
    }
 
    // computing all solutions sets for each sampled value of the redundant joint
    if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
    {
      result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
      return false;
    }
 
    for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
    {
      vfree.clear();
      vfree.push_back(sampled_joint_vals[i]);
      numsol += solve(frame, vfree, ik_solutions);
      solution_set.push_back(ik_solutions);
    }
  }
  else
  {
    // computing for single solution set
    numsol = solve(frame, vfree, ik_solutions);
    solution_set.push_back(ik_solutions);
  }
 
  RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast");
  bool solutions_found = false;
  if (numsol > 0)
  {
    /*
      Iterating through all solution sets and storing those that do not exceed joint limits.
    */
    for (unsigned int r = 0; r < solution_set.size(); r++)
    {
      ik_solutions = solution_set[r];
      numsol = ik_solutions.GetNumSolutions();
      for (int s = 0; s < numsol; ++s)
      {
        std::vector<double> sol;
        getSolution(ik_solutions, ik_seed_state, s, sol);
 
        bool obeys_limits = true;
        for (unsigned int i = 0; i < sol.size(); i++)
        {
          // Add tolerance to limit check
          if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
                                              (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
          {
            // One element of solution is not within limits
            obeys_limits = false;
            RCLCPP_DEBUG_STREAM(LOGGER, "Not in limits! " << i << " value " << sol[i]
                                                       << " has limit: " << joint_has_limits_vector_[i] << "  being  "
                                                       << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
            break;
          }
        }
        if (obeys_limits)
        {
          // All elements of solution obey limits
          solutions_found = true;
          solutions.push_back(sol);
        }
      }
    }
 
    if (solutions_found)
    {
      result.kinematic_error = kinematics::KinematicErrors::OK;
      return true;
    }
  }
  else
  {
    RCLCPP_DEBUG_STREAM(LOGGER, "No IK solution");
  }
 
  result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
  return false;
}
 
bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method,
                                                  std::vector<double>& sampled_joint_vals) const
{
  int index = redundant_joint_indices_.front();
  double joint_dscrt = redundant_joint_discretization_.at(index);
  double joint_min = joint_min_vector_[index];
  double joint_max = joint_max_vector_[index];
 
  switch (method)
  {
    case kinematics::DiscretizationMethods::ALL_DISCRETIZED:
    {
      size_t steps = std::ceil((joint_max - joint_min) / joint_dscrt);
      for (size_t i = 0; i < steps; i++)
      {
        sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
      }
      sampled_joint_vals.push_back(joint_max);
    }
    break;
    case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED:
    {
      int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
      steps = steps > 0 ? steps : 1;
      double diff = joint_max - joint_min;
      for (int i = 0; i < steps; i++)
      {
        sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
      }
    }
 
    break;
    case kinematics::DiscretizationMethods::NO_DISCRETIZATION:
 
      break;
    default:
      RCLCPP_ERROR_STREAM(LOGGER, "Discretization method " << method << " is not supported");
      return false;
  }
 
  return true;
}
 
void IKFastKinematicsPlugin::transformToChainFrame(const geometry_msgs::msg::Pose& ik_pose, KDL::Frame& ik_pose_chain) const
{
  if (tip_transform_required_ || base_transform_required_)
  {
    Eigen::Isometry3d ik_eigen_pose;
    tf2::fromMsg(ik_pose, ik_eigen_pose);
    if (tip_transform_required_)
      ik_eigen_pose = ik_eigen_pose * group_tip_to_chain_tip_;
 
    if (base_transform_required_)
      ik_eigen_pose = chain_base_to_group_base_ * ik_eigen_pose;
 
    tf2::transformEigenToKDL(ik_eigen_pose, ik_pose_chain);
  }
  else
  {
    tf2::fromMsg(ik_pose, ik_pose_chain);
  }
}
 
}  // namespace prbt_manipulator
 
// register IKFastKinematicsPlugin as a KinematicsBase implementation
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(prbt_manipulator::IKFastKinematicsPlugin, kinematics::KinematicsBase);