diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index afaa99a64d..9a5bd60ae1 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/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) diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 603c7db8dc..10ba980871 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/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, diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 5757655b7e..117be8e40f 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -39,6 +39,7 @@ // ROS #include #include +#include // ROS msgs #include @@ -174,13 +175,7 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase std::vector 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 param_listener_; + kdl_kinematics::Params params_; }; } // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml new file mode 100644 index 0000000000..d0c253db62 --- /dev/null +++ b/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", + } diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 8eb9076263..412d95c871 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/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& active_names = joint_model_group_->getActiveJointModelNames(); - std::vector names; - std::vector 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 joint_names = joint_model_group_->getActiveJointModelNames(); + // Default all joint weights to 1.0 + joint_weights_ = std::vector(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(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& 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(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,9 +326,14 @@ 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 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_); @@ -368,7 +341,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po jnt_seed_state.data = Eigen::Map(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(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; diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 048eb5dd78..8234b33c5b 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -27,6 +27,7 @@ moveit_core class_loader pluginlib + generate_parameter_library eigen tf2 tf2_kdl