From 4f37bcce9938dd458ca0c2ed31a013592cd5f494 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 29 Sep 2023 14:05:49 +0200 Subject: [PATCH 01/10] Enable using a subgroup of the move group in servo --- .../moveit_servo/config/servo_parameters.yaml | 8 +++- moveit_ros/moveit_servo/src/servo.cpp | 47 ++++++++++--------- moveit_ros/moveit_servo/src/utils/command.cpp | 23 ++++++++- 3 files changed, 54 insertions(+), 24 deletions(-) diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index bee05d1c3f..336af9fd3c 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -46,8 +46,14 @@ servo: must be passed to the node during launch time." } -############################# INCOMING COMMAND SETTINGS ######################## + active_subgroup: { + type: string, + default_value: "", + description: "This parameter can be used to switch online to actuating a sub-joint group of the move group with servo. \ + If it is empty, the full move group is actuated." + } +############################# INCOMING COMMAND SETTINGS ######################## pose_command_in_topic: { type: string, read_only: true, diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index bd622dbb6e..7087bbb366 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -63,8 +63,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrget_params(); - const bool params_valid = validateParams(servo_params_); - if (!params_valid) + if (!validateParams(servo_params_)) { RCLCPP_ERROR_STREAM(LOGGER, "Got invalid parameters, exiting."); std::exit(EXIT_FAILURE); @@ -160,14 +159,19 @@ void Servo::setCollisionChecking(const bool check_collision) bool Servo::validateParams(const servo::Params& servo_params) const { - bool params_valid = true; + if (params.move_group_name != servo_params_.move_group_name) + { + RCLCPP_ERROR_STREAM(LOGGER, "It is not possible to change the move group online from " + << servo_params_.move_group_name << " to " << params.move_group_name); + return false; + } auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); if (joint_model_group == nullptr) { RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`'); - params_valid = false; + return false; } if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) @@ -175,7 +179,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " "should be greater than 'lower_singularity_threshold.' " "Check the parameters YAML file used to launch this node."); - params_valid = false; + return false; } if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && @@ -185,7 +189,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const "publish_joint_velocities / " "publish_joint_accelerations must be true. " "Check the parameters YAML file used to launch this node."); - params_valid = false; + return false; } if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && @@ -194,7 +198,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " "you must select positions OR velocities." "Check the parameters YAML file used to launch this node."); - params_valid = false; + return false; } if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) @@ -202,43 +206,44 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " "than or equal to 'scene_collision_proximity_threshold'." "Check the parameters YAML file used to launch this node."); - params_valid = false; + return false; + } + + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name && + !joint_model_group->isSubgroup(servo_params.active_subgroup)) + { + RCLCPP_ERROR(LOGGER, + "The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.", + servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); + return false; } - return params_valid; + return true; } bool Servo::updateParams() { - bool params_updated = false; - if (servo_param_listener_->is_old(servo_params_)) { - auto params = servo_param_listener_->get_params(); + const auto params = servo_param_listener_->get_params(); - const bool params_valid = validateParams(params); - if (params_valid) + if (validateParams(params)) { if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) { RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " << std::to_string(params.override_velocity_scaling_factor)); } - if (params.move_group_name != servo_params_.move_group_name) - { - RCLCPP_INFO_STREAM(LOGGER, "Move group changed from " << servo_params_.move_group_name << " to " - << params.move_group_name); - } servo_params_ = params; - params_updated = true; + return true; } else { RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated."); } } - return params_updated; + return false; } servo::Params& Servo::getParams() diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 3b2e4cea7d..d08391f9b9 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -210,8 +210,9 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params) { - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); + auto const& group_name = + servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); std::vector current_joint_positions; robot_state->copyJointGroupPositions(joint_model_group, current_joint_positions); @@ -272,6 +273,24 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt delta_theta = pseudo_inverse * cartesian_position_delta; } + if (!servo_params.active_subgroup.empty()) + { + // Create full delta vector and add delta theta's at actuated joints + auto const& move_group_joint_names = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); + auto const& subgroup_joint_names = + robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); + + Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(move_group_joint_names.size()); + for (size_t index = 0; index < subgroup_joint_names.size(); index++) + { + auto const move_group_iterator = + find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), subgroup_joint_names.at(index)); + move_group_delta_theta[std::distance(move_group_joint_names.cbegin(), move_group_iterator)] = delta_theta[index]; + } + return std::make_pair(status, move_group_delta_theta); + } + return std::make_pair(status, delta_theta); } From 3ab4514325091a2fb1f743db7d6c0506c5c576d4 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 29 Sep 2023 14:36:18 +0200 Subject: [PATCH 02/10] Remove unnecessary validations since the param is const --- moveit_ros/moveit_servo/src/servo.cpp | 7 ------- moveit_ros/moveit_servo/src/utils/command.cpp | 8 ++++---- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 7087bbb366..2b9f975196 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -159,13 +159,6 @@ void Servo::setCollisionChecking(const bool check_collision) bool Servo::validateParams(const servo::Params& servo_params) const { - if (params.move_group_name != servo_params_.move_group_name) - { - RCLCPP_ERROR_STREAM(LOGGER, "It is not possible to change the move group online from " - << servo_params_.move_group_name << " to " << params.move_group_name); - return false; - } - auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); if (joint_model_group == nullptr) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index d08391f9b9..c51991a11a 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -210,7 +210,7 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params) { - auto const& group_name = + const auto& group_name = servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); @@ -276,15 +276,15 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!servo_params.active_subgroup.empty()) { // Create full delta vector and add delta theta's at actuated joints - auto const& move_group_joint_names = + const auto& move_group_joint_names = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); - auto const& subgroup_joint_names = + const auto& subgroup_joint_names = robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(move_group_joint_names.size()); for (size_t index = 0; index < subgroup_joint_names.size(); index++) { - auto const move_group_iterator = + const auto move_group_iterator = find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), subgroup_joint_names.at(index)); move_group_delta_theta[std::distance(move_group_joint_names.cbegin(), move_group_iterator)] = delta_theta[index]; } From 3690cb705d665521308abe14d60ee072a7a27109 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 29 Sep 2023 16:18:38 +0200 Subject: [PATCH 03/10] Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- moveit_ros/moveit_servo/config/servo_parameters.yaml | 2 +- moveit_ros/moveit_servo/src/utils/command.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index 336af9fd3c..f5dc614552 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -49,7 +49,7 @@ servo: active_subgroup: { type: string, default_value: "", - description: "This parameter can be used to switch online to actuating a sub-joint group of the move group with servo. \ + description: "This parameter can be used to switch online to actuating a subgroup of the move group. \ If it is empty, the full move group is actuated." } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index c51991a11a..cfb488544e 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -275,7 +275,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!servo_params.active_subgroup.empty()) { - // Create full delta vector and add delta theta's at actuated joints + // Create full delta vector and add delta theta values only at actuated joints const auto& move_group_joint_names = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); const auto& subgroup_joint_names = From 0b98ef80eb6d3322a8a7f8d0d39886a4cfed492b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 29 Sep 2023 16:34:17 +0200 Subject: [PATCH 04/10] Don't copy joints if subgroup == move group --- moveit_ros/moveit_servo/src/utils/command.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index cfb488544e..df7879a337 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -243,6 +243,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt // setup for IK call std::vector solution; + solution.reserve(current_joint_positions.size()); moveit_msgs::msg::MoveItErrorCodes err; kinematics::KinematicsQueryOptions opts; opts.return_approximate_solution = true; @@ -252,7 +253,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt // find the difference in joint positions that will get us to the desired pose for (size_t i = 0; i < current_joint_positions.size(); ++i) { - delta_theta[i] = solution[i] - current_joint_positions[i]; + delta_theta[i] = solution.at(i) - current_joint_positions.at(i); } } else @@ -273,7 +274,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt delta_theta = pseudo_inverse * cartesian_position_delta; } - if (!servo_params.active_subgroup.empty()) + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) { // Create full delta vector and add delta theta values only at actuated joints const auto& move_group_joint_names = From 2e395efc1bc67d8608821fab7e8488ef1843695a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 4 Oct 2023 15:57:08 +0200 Subject: [PATCH 05/10] Re-add params_valid in validateParams --- moveit_ros/moveit_servo/src/servo.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 2b9f975196..60c329fd33 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -159,12 +159,13 @@ void Servo::setCollisionChecking(const bool check_collision) bool Servo::validateParams(const servo::Params& servo_params) const { + bool params_valid = true; auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); if (joint_model_group == nullptr) { RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`'); - return false; + params_valid = false; } if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) @@ -172,7 +173,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " "should be greater than 'lower_singularity_threshold.' " "Check the parameters YAML file used to launch this node."); - return false; + params_valid = false; } if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && @@ -182,7 +183,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const "publish_joint_velocities / " "publish_joint_accelerations must be true. " "Check the parameters YAML file used to launch this node."); - return false; + params_valid = false; } if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && @@ -191,7 +192,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " "you must select positions OR velocities." "Check the parameters YAML file used to launch this node."); - return false; + params_valid = false; } if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) @@ -199,7 +200,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " "than or equal to 'scene_collision_proximity_threshold'." "Check the parameters YAML file used to launch this node."); - return false; + params_valid = false; } if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name && @@ -208,10 +209,10 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR(LOGGER, "The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.", servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); - return false; + params_valid = false; } - return true; + return params_valid; } bool Servo::updateParams() From 7e3a6a77f95e6ca1d0b1f0cdfc8598d54f0ca865 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 4 Oct 2023 16:53:44 +0200 Subject: [PATCH 06/10] Generalize active subgroup delta calculation --- moveit_ros/moveit_servo/src/utils/command.cpp | 63 ++++++++++++++----- 1 file changed, 46 insertions(+), 17 deletions(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index df7879a337..48d4f151c7 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -42,7 +42,36 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor"); -} + +/// @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the +/// whole move group is created and all entries zeroed. The elements of the subgroup deltas vector are copied into the +/// correct element of the bigger move group delta vector. +/// @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo +/// @param robot_state Current robot state +/// @param servo_params Servo params +/// @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero. +const Eigen::VectorXd createMoveGroupDelta(const Eigen::VectorXd& sub_group_deltas, + const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params) +{ + // Create full delta vector and add delta theta values only at actuated joints + const auto& move_group_joint_names = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); + const auto& subgroup_joint_names = + robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); + + Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(move_group_joint_names.size()); + + for (size_t index = 0; index < subgroup_joint_names.size(); index++) + { + const auto move_group_iterator = + find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), subgroup_joint_names.at(index)); + move_group_delta_theta[std::distance(move_group_joint_names.cbegin(), move_group_iterator)] = + sub_group_deltas[index]; + } + return move_group_delta_theta; +}; +} // namespace namespace moveit_servo { @@ -52,7 +81,10 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo { // Find the target joint position based on the commanded joint velocity StatusCode status = StatusCode::NO_WARNING; - const auto joint_names = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); + const auto& group_name = + servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); + const auto joint_names = joint_model_group->getActiveJointModelNames(); Eigen::VectorXd joint_position_delta(joint_names.size()); Eigen::VectorXd velocities(joint_names.size()); @@ -68,6 +100,8 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo } else { + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint name: " << command.names[i]); + names_valid = false; break; } @@ -86,13 +120,21 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo status = StatusCode::INVALID; if (!names_valid) { - RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command"); + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command. Either you're sending commands for a joint " + "that is not part of the move group or certain joints cannot be moved because a " + "subgroup is active and they are not part of it."); } if (!velocity_valid) { RCLCPP_WARN_STREAM(LOGGER, "Invalid velocity values in joint jog command"); } } + + if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) + { + return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params)); + } + return std::make_pair(status, joint_position_delta); } @@ -276,20 +318,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) { - // Create full delta vector and add delta theta values only at actuated joints - const auto& move_group_joint_names = - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); - const auto& subgroup_joint_names = - robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); - - Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(move_group_joint_names.size()); - for (size_t index = 0; index < subgroup_joint_names.size(); index++) - { - const auto move_group_iterator = - find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), subgroup_joint_names.at(index)); - move_group_delta_theta[std::distance(move_group_joint_names.cbegin(), move_group_iterator)] = delta_theta[index]; - } - return std::make_pair(status, move_group_delta_theta); + return std::make_pair(status, createMoveGroupDelta(delta_theta, robot_state, servo_params)); } return std::make_pair(status, delta_theta); From 244a032ca94c89446a908acf35bc3d647a2e2191 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 5 Oct 2023 12:31:45 +0200 Subject: [PATCH 07/10] Add more efficient move group joint position lookup --- .../include/moveit_servo/servo.hpp | 3 + .../include/moveit_servo/utils/command.hpp | 16 ++++-- .../include/moveit_servo/utils/datatypes.hpp | 3 + moveit_ros/moveit_servo/src/servo.cpp | 42 +++++++++++++- moveit_ros/moveit_servo/src/utils/command.cpp | 57 +++++++++++-------- 5 files changed, 89 insertions(+), 32 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 3c5d9a484a..b193e439fa 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -214,6 +214,9 @@ class Servo std::unique_ptr collision_monitor_; pluginlib::UniquePtr smoother_ = nullptr; + + // Map between joint subgroup names and corresponding joint name - move group indices map + std::unordered_map joint_name_to_index_maps_; }; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 2b49b51ffe..23582f5186 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -55,39 +55,47 @@ namespace moveit_servo * @param command The joint jog command. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params); + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); /** * \brief Compute the change in joint position for the given twist command. * @param command The twist command. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params); + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); /** * \brief Compute the change in joint position for the given pose command. * @param command The pose command. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params); + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); /** * \brief Computes the required change in joint angles for given Cartesian change, using the robot's IK solver. * @param cartesian_position_delta The change in Cartesian position. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, - const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params); + const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map); } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp index 31195c37f2..b35e147e95 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp @@ -129,4 +129,7 @@ struct KinematicState } }; +// Mapping joint names and their position in the move group vector +typedef std::unordered_map JointNameToMoveGroupIndexMap; + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 60c329fd33..d9626e3340 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -229,6 +229,33 @@ bool Servo::updateParams() << std::to_string(params.override_velocity_scaling_factor)); } + // If necessary, create new subgroup map + if (!params.active_subgroup.empty() && params.active_subgroup != params.move_group_name && + joint_name_to_index_maps_.count(params.active_subgroup) == 0) + { + const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel() + ->getJointModelGroup(params.move_group_name) + ->getActiveJointModelNames(); + const auto& subgroup_joint_names = planning_scene_monitor_->getRobotModel() + ->getJointModelGroup(params.active_subgroup) + ->getActiveJointModelNames(); + + JointNameToMoveGroupIndexMap new_map; + // For each joint name of the subgroup calculate the index in the move group joint vector + for (const auto& joint_name : subgroup_joint_names) + { + // Find sub group joint name in move group joint names + const auto move_group_iterator = + std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name); + // Calculate position and add a new mapping of joint name to move group joint vector position + new_map.insert(std::make_pair( + std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator))); + } + // Add new joint name to index map to existing maps + joint_name_to_index_maps_.insert(std::make_pair( + std::string(params.active_subgroup), std::move(new_map))); + } + servo_params_ = params; return true; } @@ -309,6 +336,12 @@ KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const K Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state) { + // Determine joint_name_group_index_map, if no subgroup is active, the map is empty + const auto& joint_name_group_index_map = + (!servo_params_.active_subgroup.empty() && servo_params_.active_subgroup != servo_params_.move_group_name) ? + joint_name_to_index_maps_.at(servo_params_.active_subgroup) : + JointNameToMoveGroupIndexMap(); + const int num_joints = robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); Eigen::VectorXd joint_position_deltas(num_joints); @@ -321,7 +354,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo { if (expected_type == CommandType::JOINT_JOG) { - delta_result = jointDeltaFromJointJog(std::get(command), robot_state, servo_params_); + delta_result = jointDeltaFromJointJog(std::get(command), robot_state, servo_params_, + joint_name_group_index_map); servo_status_ = delta_result.first; } else if (expected_type == CommandType::TWIST) @@ -329,7 +363,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo try { const TwistCommand command_in_planning_frame = toPlanningFrame(std::get(command)); - delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_); + delta_result = + jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map); servo_status_ = delta_result.first; } catch (tf2::TransformException& ex) @@ -343,7 +378,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo try { const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(command)); - delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_); + delta_result = + jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map); servo_status_ = delta_result.first; } catch (tf2::TransformException& ex) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 48d4f151c7..61cc0bd6a5 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -43,31 +43,30 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor"); -/// @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the -/// whole move group is created and all entries zeroed. The elements of the subgroup deltas vector are copied into the -/// correct element of the bigger move group delta vector. -/// @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo -/// @param robot_state Current robot state -/// @param servo_params Servo params -/// @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero. +/** + * @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the + * whole move group is created and all entries zeroed. The elements of the subgroup deltas vector are copied into the + * correct element of the bigger move group delta vector. + * @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo + * @param robot_state Current robot state + * @param servo_params Servo params + * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero. + */ const Eigen::VectorXd createMoveGroupDelta(const Eigen::VectorXd& sub_group_deltas, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params) + const servo::Params& servo_params, + const moveit_servo::JointNameToMoveGroupIndexMap& joint_name_group_index_map) { - // Create full delta vector and add delta theta values only at actuated joints - const auto& move_group_joint_names = - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); const auto& subgroup_joint_names = robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); - Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(move_group_joint_names.size()); - + // Create + Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero( + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size()); for (size_t index = 0; index < subgroup_joint_names.size(); index++) { - const auto move_group_iterator = - find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), subgroup_joint_names.at(index)); - move_group_delta_theta[std::distance(move_group_joint_names.cbegin(), move_group_iterator)] = - sub_group_deltas[index]; + move_group_delta_theta[joint_name_group_index_map.at(subgroup_joint_names.at(index))] = sub_group_deltas[index]; } return move_group_delta_theta; }; @@ -77,7 +76,8 @@ namespace moveit_servo { JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params) + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { // Find the target joint position based on the commanded joint velocity StatusCode status = StatusCode::NO_WARNING; @@ -132,14 +132,16 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) { - return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params)); + return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params, + joint_name_group_index_map)); } return std::make_pair(status, joint_position_delta); } JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params) + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { StatusCode status = StatusCode::NO_WARNING; const int num_joints = @@ -164,7 +166,8 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: } // Compute the required change in joint angles. - const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params); + const auto delta_result = + jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); status = delta_result.first; if (status != StatusCode::INVALID) { @@ -202,7 +205,8 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: } JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state, - const servo::Params& servo_params) + const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { StatusCode status = StatusCode::NO_WARNING; const int num_joints = @@ -226,7 +230,8 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle(); // Compute the required change in joint angles. - const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params); + const auto delta_result = + jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); status = delta_result.first; if (status != StatusCode::INVALID) { @@ -250,7 +255,8 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co } JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, - const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params) + const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params, + const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { const auto& group_name = servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; @@ -318,7 +324,8 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) { - return std::make_pair(status, createMoveGroupDelta(delta_theta, robot_state, servo_params)); + return std::make_pair(status, + createMoveGroupDelta(delta_theta, robot_state, servo_params, joint_name_group_index_map)); } return std::make_pair(status, delta_theta); From 03aaabff7d1c29b3ab2e66f6c2aaaf8a3a48b437 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 5 Oct 2023 14:58:21 +0200 Subject: [PATCH 08/10] Create subgroup map in the constructor --- moveit_ros/moveit_servo/src/servo.cpp | 59 ++++++++++++++------------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index d9626e3340..e0a5cd32f5 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -116,8 +116,38 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrstart(); servo_status_ = StatusCode::NO_WARNING; - RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); } + + const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel() + ->getJointModelGroup(servo_params_.move_group_name) + ->getActiveJointModelNames(); + // Create subgroup map + for (const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames()) + { + // Skip move group + if (sub_group_name == servo_params_.move_group_name) + { + continue; + } + const auto& subgroup_joint_names = + planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames(); + + JointNameToMoveGroupIndexMap new_map; + // For each joint name of the subgroup calculate the index in the move group joint vector + for (const auto& joint_name : subgroup_joint_names) + { + // Find sub group joint name in move group joint names + const auto move_group_iterator = + std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name); + // Calculate position and add a new mapping of joint name to move group joint vector position + new_map.insert(std::make_pair( + std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator))); + } + // Add new joint name to index map to existing maps + joint_name_to_index_maps_.insert( + std::make_pair(std::string(sub_group_name), std::move(new_map))); + } + RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); } Servo::~Servo() @@ -229,33 +259,6 @@ bool Servo::updateParams() << std::to_string(params.override_velocity_scaling_factor)); } - // If necessary, create new subgroup map - if (!params.active_subgroup.empty() && params.active_subgroup != params.move_group_name && - joint_name_to_index_maps_.count(params.active_subgroup) == 0) - { - const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel() - ->getJointModelGroup(params.move_group_name) - ->getActiveJointModelNames(); - const auto& subgroup_joint_names = planning_scene_monitor_->getRobotModel() - ->getJointModelGroup(params.active_subgroup) - ->getActiveJointModelNames(); - - JointNameToMoveGroupIndexMap new_map; - // For each joint name of the subgroup calculate the index in the move group joint vector - for (const auto& joint_name : subgroup_joint_names) - { - // Find sub group joint name in move group joint names - const auto move_group_iterator = - std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name); - // Calculate position and add a new mapping of joint name to move group joint vector position - new_map.insert(std::make_pair( - std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator))); - } - // Add new joint name to index map to existing maps - joint_name_to_index_maps_.insert(std::make_pair( - std::string(params.active_subgroup), std::move(new_map))); - } - servo_params_ = params; return true; } From e7baf707f14a7886abc6a002331376c3b2fa3287 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 6 Oct 2023 11:35:21 +0200 Subject: [PATCH 09/10] Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../moveit_servo/include/moveit_servo/utils/command.hpp | 6 +++--- moveit_ros/moveit_servo/src/servo.cpp | 8 +++++--- moveit_ros/moveit_servo/src/utils/command.cpp | 2 +- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 23582f5186..2446670dbf 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -55,7 +55,7 @@ namespace moveit_servo * @param command The joint jog command. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. - * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state, @@ -67,7 +67,7 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo * @param command The twist command. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. - * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, @@ -91,7 +91,7 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co * @param cartesian_position_delta The change in Cartesian position. * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor. * @param servo_params The servo parameters. - * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. * @return The status and joint position change required (delta). */ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index e0a5cd32f5..7decc87fa6 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -136,7 +136,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptris_old(servo_params_)) { const auto params = servo_param_listener_->get_params(); @@ -260,14 +262,14 @@ bool Servo::updateParams() } servo_params_ = params; - return true; + params_updated = true; } else { RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated."); } } - return false; + return params_updated; } servo::Params& Servo::getParams() diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 61cc0bd6a5..e6e932eac5 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -50,7 +50,7 @@ const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor * @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo * @param robot_state Current robot state * @param servo_params Servo params - * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position + * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position. * @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero. */ const Eigen::VectorXd createMoveGroupDelta(const Eigen::VectorXd& sub_group_deltas, From c05f0c92bc27e02a964b19a2c38bcbde09711e8e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 6 Oct 2023 11:35:43 +0200 Subject: [PATCH 10/10] Update moveit_ros/moveit_servo/src/servo.cpp --- moveit_ros/moveit_servo/src/servo.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 7decc87fa6..b131319371 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -248,7 +248,6 @@ bool Servo::validateParams(const servo::Params& servo_params) const bool Servo::updateParams() { bool params_updated = false; - if (servo_param_listener_->is_old(servo_params_)) { const auto params = servo_param_listener_->get_params();