Use generate_parameter_library to load LMA kinematics parameters (mov…

…eit#1673)

moveit_kinematics/CMakeLists.txt

@@ -32,6 +32,7 @@ set(THIS_PACKAGE_LIBRARIES
   moveit_cached_ik_kinematics_base
   moveit_cached_ik_kinematics_plugin
   moveit_kdl_kinematics_plugin
+  lma_kinematics_parameters
   moveit_lma_kinematics_plugin
   moveit_srv_kinematics_plugin
 )

moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt

@@ -1,5 +1,10 @@
 set(MOVEIT_LIB_NAME moveit_lma_kinematics_plugin)
 
+generate_parameter_library(
+  lma_kinematics_parameters # cmake target name for the parameter library
+  src/lma_kinematics_parameters.yaml # path to input yaml file
+)
+
 add_library(${MOVEIT_LIB_NAME} SHARED src/lma_kinematics_plugin.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
@@ -10,4 +15,8 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   tf2_kdl
 )
 
+target_link_libraries(${MOVEIT_LIB_NAME}
+  lma_kinematics_parameters
+)
+
 install(DIRECTORY include/ DESTINATION include)

moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h

@@ -39,6 +39,7 @@
 // ROS2
 #include <rclcpp/rclcpp.hpp>
 #include <random_numbers/random_numbers.h>
+#include <lma_kinematics_parameters.hpp>
 
 // ROS msgs
 #include <geometry_msgs/msg/pose.hpp>
@@ -155,13 +156,7 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase
   std::vector<std::string> joint_names_;
   rclcpp::Node::SharedPtr node_;
 
-  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<lma_kinematics::ParamListener> param_listener_;
+  lma_kinematics::Params params_;
 };
 }  // namespace lma_kinematics_plugin

moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_parameters.yaml

@@ -0,0 +1,37 @@
+lma_kinematics:
+
+  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/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp

@@ -84,6 +84,12 @@ bool LMAKinematicsPlugin::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<lma_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_)
@@ -123,18 +129,6 @@ bool LMAKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
   }
   dimension_ = joints_.size();
 
-  // 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_, 0.01);
-
-  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");
-
   // Setup the joint state groups that we need
   state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
 
@@ -241,21 +235,25 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po
     return false;
   }
 
+  const 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(0) = 1;
   cartesian_weights(1) = 1;
   cartesian_weights(2) = 1;
-  cartesian_weights(3) = orientation_vs_position_weight_;
-  cartesian_weights(4) = orientation_vs_position_weight_;
-  cartesian_weights(5) = orientation_vs_position_weight_;
+  cartesian_weights(3) = orientation_vs_position_weight;
+  cartesian_weights(4) = orientation_vs_position_weight;
+  cartesian_weights(5) = 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::ChainIkSolverPos_LMA ik_solver_pos(kdl_chain_, cartesian_weights, epsilon_, max_solver_iterations_);
+  KDL::ChainIkSolverPos_LMA ik_solver_pos(kdl_chain_, cartesian_weights, params_.epsilon, params_.max_solver_iterations);
   solution.resize(dimension_);
 
   KDL::Frame pose_desired;