diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index bee05d1c3f..f5dc614552 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 subgroup of the move group. \ + 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/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..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,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 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, - 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 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, - 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 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, - 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 bd622dbb6e..b131319371 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); @@ -117,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 subgroup 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() @@ -161,7 +190,6 @@ 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) @@ -205,30 +233,32 @@ bool Servo::validateParams(const servo::Params& servo_params) const params_valid = 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()); + params_valid = false; + } + return params_valid; } 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; @@ -310,6 +340,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); @@ -322,7 +358,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) @@ -330,7 +367,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) @@ -344,7 +382,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 3b2e4cea7d..e6e932eac5 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -42,17 +42,49 @@ 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 + * @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, + const moveit::core::RobotStatePtr& robot_state, + const servo::Params& servo_params, + const moveit_servo::JointNameToMoveGroupIndexMap& joint_name_group_index_map) +{ + const auto& subgroup_joint_names = + robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames(); + + // 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++) + { + move_group_delta_theta[joint_name_group_index_map.at(subgroup_joint_names.at(index))] = sub_group_deltas[index]; + } + return move_group_delta_theta; +}; +} // namespace 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; - 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,18 +120,28 @@ 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, + 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 = @@ -122,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) { @@ -160,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 = @@ -184,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) { @@ -208,10 +255,12 @@ 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 moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_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); std::vector current_joint_positions; robot_state->copyJointGroupPositions(joint_model_group, current_joint_positions); @@ -242,6 +291,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; @@ -251,7 +301,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 @@ -272,6 +322,12 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt delta_theta = pseudo_inverse * cartesian_position_delta; } + 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, joint_name_group_index_map)); + } + return std::make_pair(status, delta_theta); }