From 76be59ccb48df7553cd15259f83d1a5ab5fe301c Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 18 Mar 2024 16:27:03 -0400 Subject: [PATCH 1/2] Fix KDL IK plugin parameters specification --- .../src/kdl_kinematics_parameters.yaml | 17 +++++++++-------- .../src/kdl_kinematics_plugin.cpp | 7 +++---- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml index d0c253db62..3d252f88f4 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_parameters.yaml @@ -5,15 +5,16 @@ kdl_kinematics: description: "Joints names to assign weights", } - __map_joints: - weight: { - type: double, - default_value: 1.0, - description: "Joint weight", - validation: { - gt<>: [ 0.0 ] + weights: + __map_joints: + value: { + type: double, + default_value: 1.0, + description: "Joint weight", + validation: { + gt<>: [ 0.0 ] + } } - } max_solver_iterations: { type: int, 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 54d3f0aa4f..616abd449f 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -95,11 +95,10 @@ void KDLKinematicsPlugin::getJointWeights() 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) + // Loop through map (key: joint name and weight: struct with a value member variable) + for (const auto& [joint_name, weight] : params_.weights.joints_map) { // 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()) { @@ -109,7 +108,7 @@ void KDLKinematicsPlugin::getJointWeights() } // Find index of the joint name and assign weight to the coressponding index - joint_weights_.at(it - joint_names.begin()) = joint_weight.second.weight; + joint_weights_.at(it - joint_names.begin()) = weight.value; } RCLCPP_INFO_STREAM( From 4124cb970a99fcba6d5393c67ee13818d1c27333 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 18 Mar 2024 17:38:34 -0400 Subject: [PATCH 2/2] Add allow_nonzero_velocity_at_trajectory_end parameter to exported ros2_controllers config file --- .../moveit_setup_controllers/src/ros2_controllers_config.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp index 97f1907211..b21ec11f1c 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp @@ -206,6 +206,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_); emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces; emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces; + emitter << YAML::Key << "allow_nonzero_velocity_at_trajectory_end" << YAML::Value << true; } } emitter << YAML::EndMap;