Use generate_parameter_library to load KDL kinematics parameters

moveit_kinematics/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(pluginlib REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 
 # Finds Boost Components
 include(ConfigExtras.cmake)
@@ -32,6 +33,7 @@ set(THIS_PACKAGE_LIBRARIES
   cached_ik_kinematics_parameters
   moveit_cached_ik_kinematics_base
   moveit_cached_ik_kinematics_plugin
+  kdl_kinematics_parameters
   moveit_kdl_kinematics_plugin
   lma_kinematics_parameters
   moveit_lma_kinematics_plugin
@@ -44,6 +46,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_core
   moveit_ros_planning
   Boost
+  generate_parameter_library
 )
 
 pluginlib_export_plugin_description_file(moveit_core kdl_kinematics_plugin_description.xml)

moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt

@@ -1,5 +1,10 @@
 set(MOVEIT_LIB_NAME moveit_kdl_kinematics_plugin)
 
+generate_parameter_library(
+  kdl_kinematics_parameters # cmake target name for the parameter library
+  src/kdl_kinematics_parameters.yaml # path to input yaml file
+)
+
 add_library(${MOVEIT_LIB_NAME} SHARED
   src/kdl_kinematics_plugin.cpp
   src/chainiksolver_vel_mimic_svd.cpp)
@@ -16,6 +21,10 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   EIGEN3
 )
 
+target_link_libraries(${MOVEIT_LIB_NAME}
+  kdl_kinematics_parameters
+)
+
 # prevent pluginlib from using boost
 target_compile_definitions(${MOVEIT_LIB_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 # Causes the visibility macros to use dllexport rather than dllimport,

moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h

@@ -39,6 +39,7 @@
 // ROS
 #include <rclcpp/rclcpp.hpp>
 #include <random_numbers/random_numbers.h>
+#include <kdl_kinematics_parameters.hpp>
 
 // ROS msgs
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -174,13 +175,7 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
   std::vector<double> joint_weights_;
   Eigen::VectorXd joint_min_, joint_max_;  ///< joint limits
 
-  int max_solver_iterations_;
-  double epsilon_;
-  /** weight of orientation error vs position error
-   *
-   * < 1.0: orientation has less importance than position
-   * > 1.0: orientation has more importance than position
-   * = 0.0: perform position-only IK */
-  double orientation_vs_position_weight_;
+  std::shared_ptr<kdl_kinematics::ParamListener> param_listener_;
+  kdl_kinematics::Params params_;
 };
 }  // namespace kdl_kinematics_plugin

moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml

@@ -0,0 +1,52 @@
+kdl_kinematics:
+  joints: {
+    type: string_array,
+    default_value: [],
+    description: "Joints names to assign weights",
+  }
+
+  __map_joints:
+    weight: {
+      type: double,
+      default_value: 1.0,
+      description: "Joint weight",
+      validation: {
+        gt<>: [ 0.0 ]
+      }
+    }
+
+  max_solver_iterations: {
+    type: int,
+    default_value: 500,
+    description: "Maximum solver iterations",
+    validation: {
+      gt_eq<>: [ 0.0 ]
+    }
+  }
+
+  epsilon: {
+    type: double,
+    default_value: 0.00001,
+    description: "Epsilon. Default is 1e-5",
+    validation: {
+      gt<>: [ 0.0 ]
+    }
+  }
+
+  orientation_vs_position: {
+    type: double,
+    default_value: 1.0,
+    description: "Weight of orientation error vs position error
+                  * < 1.0: orientation has less importance than position
+                  * > 1.0: orientation has more importance than position
+                  * = 0.0: perform position-only IK",
+    validation: {
+      gt_eq<>: [ 0.0 ]
+    }
+  }
+
+  position_only_ik: {
+    type: bool,
+    default_value: false,
+    description: "position_only_ik overrules orientation_vs_position. If true, sets orientation_vs_position weight to 0.0",
+  }

moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp

@@ -81,54 +81,28 @@ bool KDLKinematicsPlugin::checkConsistency(const Eigen::VectorXd& seed_state,
 
 void KDLKinematicsPlugin::getJointWeights()
 {
-  const std::vector<std::string>& active_names = joint_model_group_->getActiveJointModelNames();
-  std::vector<std::string> names;
-  std::vector<double> weights;
-  if (lookupParam(node_, "joint_weights.weights", weights, weights))
-  {
-    if (!lookupParam(node_, "joint_weights.names", names, names) || (names.size() != weights.size()))
-    {
-      RCLCPP_ERROR(LOGGER, "Expecting list parameter joint_weights.names of same size as list joint_weights.weights");
-      // fall back to default weights
-      weights.clear();
-    }
-  }
-  else if (lookupParam(node_, "joint_weights", weights,
-                       weights))  // try reading weight lists (for all active joints) directly
+  const std::vector<std::string> joint_names = joint_model_group_->getActiveJointModelNames();
+  // Default all joint weights to 1.0
+  joint_weights_ = std::vector<double>(joint_names.size(), 1.0);
+
+  // Check if joint weight is assigned in kinematics YAML
+  // Loop through map (key: joint name and value: Struct with a weight member variable)
+  for (const auto& joint_weight : params_.joints_map)
   {
-    std::size_t num_active = active_names.size();
-    if (weights.size() == num_active)
-    {
-      joint_weights_ = weights;
-      return;
-    }
-    else if (!weights.empty())
+    // Check if joint is an active joint in the group
+    const auto joint_name = joint_weight.first;
+    auto it = std::find(joint_names.begin(), joint_names.end(), joint_name);
+    if (it == joint_names.cend())
     {
-      RCLCPP_ERROR(LOGGER, "Expecting parameter joint_weights to list weights for all active joints (%zu) in order",
-                   num_active);
-      // fall back to default weights
-      weights.clear();
+      RCLCPP_WARN(LOGGER, "Joint '%s' is not an active joint in group '%s'", joint_name.c_str(),
+                  joint_model_group_->getName().c_str());
+      continue;
     }
-  }
-
-  // by default assign weights of 1.0 to all joints
-  joint_weights_ = std::vector<double>(active_names.size(), 1.0);
-  if (weights.empty())  // indicates default case
-    return;
 
-  // modify weights of listed joints
-  assert(names.size() == weights.size());
-  for (size_t i = 0; i != names.size(); ++i)
-  {
-    auto it = std::find(active_names.begin(), active_names.end(), names[i]);
-    if (it == active_names.cend())
-      RCLCPP_WARN(LOGGER, "Joint '%s' is not an active joint in group '%s'", names[i].c_str(),
-                  joint_model_group_->getName().c_str());
-    else if (weights[i] < 0.0)
-      RCLCPP_WARN(LOGGER, "Negative weight %f for joint '%s' will be ignored", weights[i], names[i].c_str());
-    else
-      joint_weights_[it - active_names.begin()] = weights[i];
+    // Find index of the joint name and assign weight to the coressponding index
+    joint_weights_.at(it - joint_names.begin()) = joint_weight.second.weight;
   }
+
   RCLCPP_INFO_STREAM(
       LOGGER, "Joint weights for group '"
                   << getGroupName() << "': \n"
@@ -140,6 +114,12 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
                                      const std::vector<std::string>& tip_frames, double search_discretization)
 {
   node_ = node;
+
+  // Get Solver Parameters
+  std::string kinematics_param_prefix = "robot_description_kinematics." + group_name;
+  param_listener_ = std::make_shared<kdl_kinematics::ParamListener>(node, kinematics_param_prefix);
+  params_ = param_listener_->get_params();
+
   storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
   joint_model_group_ = robot_model_->getJointModelGroup(group_name);
   if (!joint_model_group_)
@@ -198,18 +178,6 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
     joint_max_(i) = solver_info_.limits[i].max_position;
   }
 
-  // Get Solver Parameters
-  lookupParam(node_, "max_solver_iterations", max_solver_iterations_, 500);
-  lookupParam(node_, "epsilon", epsilon_, 1e-5);
-  lookupParam(node_, "orientation_vs_position", orientation_vs_position_weight_, 1.0);
-
-  bool position_ik;
-  lookupParam(node_, "position_only_ik", position_ik, false);
-  if (position_ik)  // position_only_ik overrules orientation_vs_position
-    orientation_vs_position_weight_ = 0.0;
-  if (orientation_vs_position_weight_ == 0.0)
-    RCLCPP_INFO(LOGGER, "Using position only ik");
-
   getJointWeights();
 
   // Check for mimic joints
@@ -358,17 +326,22 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po
         consistency_limits_mimic.push_back(consistency_limits[i]);
     }
   }
+
+  auto orientation_vs_position_weight = params_.position_only_ik ? 0.0 : params_.orientation_vs_position;
+  if (orientation_vs_position_weight == 0.0)
+    RCLCPP_INFO(LOGGER, "Using position only ik");
+
   Eigen::Matrix<double, 6, 1> cartesian_weights;
   cartesian_weights.topRows<3>().setConstant(1.0);
-  cartesian_weights.bottomRows<3>().setConstant(orientation_vs_position_weight_);
+  cartesian_weights.bottomRows<3>().setConstant(orientation_vs_position_weight);
 
   KDL::JntArray jnt_seed_state(dimension_);
   KDL::JntArray jnt_pos_in(dimension_);
   KDL::JntArray jnt_pos_out(dimension_);
   jnt_seed_state.data = Eigen::Map<const Eigen::VectorXd>(ik_seed_state.data(), ik_seed_state.size());
   jnt_pos_in = jnt_seed_state;
 
-  KDL::ChainIkSolverVelMimicSVD ik_solver_vel(kdl_chain_, mimic_joints_, orientation_vs_position_weight_ == 0.0);
+  KDL::ChainIkSolverVelMimicSVD ik_solver_vel(kdl_chain_, mimic_joints_, orientation_vs_position_weight == 0.0);
   solution.resize(dimension_);
 
   KDL::Frame pose_desired;
@@ -393,7 +366,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po
     }
 
     int ik_valid =
-        CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, max_solver_iterations_,
+        CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, params_.max_solver_iterations,
                   Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()), cartesian_weights);
     if (ik_valid == 0 || options.return_approximate_solution)  // found acceptable solution
     {
@@ -451,7 +424,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con
     const double position_error = delta_twist.vel.Norm();
     const double orientation_error = ik_solver.isPositionOnly() ? 0 : delta_twist.rot.Norm();
     const double delta_twist_norm = std::max(position_error, orientation_error);
-    if (delta_twist_norm <= epsilon_)
+    if (delta_twist_norm <= params_.epsilon)
     {
       success = true;
       break;
@@ -481,9 +454,9 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con
     const double delta_q_norm = delta_q.data.lpNorm<1>();
     RCLCPP_DEBUG(LOGGER, "[%3d] pos err: %f  rot err: %f  delta_q: %f", i, position_error, orientation_error,
                  delta_q_norm);
-    if (delta_q_norm < epsilon_)  // stuck in singularity
+    if (delta_q_norm < params_.epsilon)  // stuck in singularity
     {
-      if (step_size < epsilon_)  // cannot reach target
+      if (step_size < params_.epsilon)  // cannot reach target
         break;
       // wiggle joints
       last_delta_twist_norm = DBL_MAX;

moveit_kinematics/package.xml

@@ -27,6 +27,7 @@
   <depend>moveit_core</depend>
   <depend>class_loader</depend>
   <depend>pluginlib</depend>
+  <depend>generate_parameter_library</depend>
   <depend>eigen</depend>
   <depend>tf2</depend>
   <depend>tf2_kdl</depend>