Enable using a subgroup of the move group in servo

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,

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -214,6 +214,9 @@ class Servo
   std::unique_ptr<CollisionMonitor> collision_monitor_;
 
   pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_ = nullptr;
+
+  // Map between joint subgroup names and corresponding joint name - move group indices map
+  std::unordered_map<std::string, JointNameToMoveGroupIndexMap> joint_name_to_index_maps_;
 };
 
 }  // namespace moveit_servo

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

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<std::string, std::size_t> JointNameToMoveGroupIndexMap;
+
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo.cpp

@@ -63,8 +63,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
 {
   servo_params_ = servo_param_listener_->get_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_ptr<const servo::P
     collision_monitor_->start();
 
     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, std::size_t>(
+          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, JointNameToMoveGroupIndexMap>(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,33 @@ 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 +341,12 @@ KinematicState Servo::haltJoints(const std::vector<int>& 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,15 +359,17 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
   {
     if (expected_type == CommandType::JOINT_JOG)
     {
-      delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_);
+      delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_,
+                                            joint_name_group_index_map);
       servo_status_ = delta_result.first;
     }
     else if (expected_type == CommandType::TWIST)
     {
       try
       {
         const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(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 +383,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
       try
       {
         const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(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)

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<double> 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<double> 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);
 }