[Servo] Change planning frame to base frame of active joint subgroup

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -37,10 +37,6 @@ is_primary_planning_scene_monitor: true
 
 ## MoveIt properties
 move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
-planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
-
-## Other frames
-ee_frame: panda_link8  # The name of the IK tip link
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -31,21 +31,6 @@ servo:
                   must be passed to the node during launch time."
   }
 
-  planning_frame: {
-    type: string,
-    description: "The name of moveit planning frame.\
-                  This parameter does not have a default value and \
-                  must be passed to the node during launch time."
-  }
-
-  ee_frame: {
-    type: string,
-    description: "The name of end effector frame of your robot, \
-                  this should also be the IK tip frame of your IK solver. \
-                  This parameter does not have a default value and \
-                  must be passed to the node during launch time."
-  }
-
   active_subgroup: {
     type: string,
     default_value: "",

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -38,10 +38,6 @@ is_primary_planning_scene_monitor: true
 
 ## MoveIt properties
 move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
-planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
-
-## Other frames
-ee_frame: panda_link8  # The name of the IK tip link
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -51,6 +51,12 @@
 using moveit::getLogger;
 using namespace moveit_servo;
 
+namespace
+{
+constexpr auto K_BASE_FRAME = "panda_link0";
+constexpr auto K_TIP_FRAME = "panda_link8";
+}  // namespace
+
 int main(int argc, char* argv[])
 {
   rclcpp::init(argc, argv);
@@ -75,6 +81,11 @@ int main(int argc, char* argv[])
       createPlanningSceneMonitor(demo_node, servo_params);
   Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor);
 
+  // Helper function to get the current pose of a specified frame.
+  const auto get_current_pose = [planning_scene_monitor](const std::string& target_frame) {
+    return planning_scene_monitor->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame);
+  };
+
   // Wait for some time, so that the planning scene is loaded in rviz.
   // This is just for convenience, should not be used for sync in real application.
   std::this_thread::sleep_for(std::chrono::seconds(3));
@@ -88,9 +99,9 @@ int main(int argc, char* argv[])
 
   // The dynamically updated target pose.
   PoseCommand target_pose;
-  target_pose.frame_id = servo_params.planning_frame;
+  target_pose.frame_id = K_BASE_FRAME;
   // Initializing the target pose as end effector pose, this can be any pose.
-  target_pose.pose = servo.getEndEffectorPose();
+  target_pose.pose = get_current_pose(K_TIP_FRAME);
 
   // The pose tracking lambda that will be run in a separate thread.
   auto pose_tracker = [&]() {
@@ -130,7 +141,7 @@ int main(int argc, char* argv[])
   {
     {
       std::lock_guard<std::mutex> pguard(pose_guard);
-      target_pose.pose = servo.getEndEffectorPose();
+      target_pose.pose = get_current_pose(K_TIP_FRAME);
       const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox(
           terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance);
       const bool satisfies_angular_tolerance =

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -83,7 +83,7 @@ int main(int argc, char* argv[])
 
   // Move end effector in the +z direction at 10 cm/s
   // while turning around z axis in the +ve direction at 0.5 rad/s
-  TwistCommand target_twist{ servo_params.planning_frame, { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } };
+  TwistCommand target_twist{ "panda_link0", { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } };
 
   // Frequency at which commands will be sent to the robot controller.
   rclcpp::WallRate rate(1.0 / servo_params.publish_period);

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -103,11 +103,6 @@ class Servo
    */
   std::string getStatusMessage() const;
 
-  /**
-   * \brief Returns the end effector pose in planning frame
-   */
-  Eigen::Isometry3d getEndEffectorPose() const;
-
   /**
    * \brief Start/Stop collision checking thread.
    * @param check_collision Stops collision checking thread if false, starts it if true.
@@ -138,9 +133,12 @@ class Servo
    * If the command frame is part of the robot model, directly look up the transform using the robot model.
    * Else, fall back to using TF to look up the transform.
    * @param command_frame The command frame name.
-   * @return The transformation between planning frame and command frame.
+   * @param planning_frame The planning frame name.
+   * @return The transformation between planning frame and command frame, or std::nullopt if there was a failure looking
+   * up a transform.
    */
-  Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame) const;
+  std::optional<Eigen::Isometry3d> getPlanningToCommandFrameTransform(const std::string& command_frame,
+                                                                      const std::string& planning_frame) const;
 
   /**
    * \brief Convert a given twist command to planning frame,
@@ -149,16 +147,18 @@ class Servo
    * https://core.ac.uk/download/pdf/154240607.pdf
    * https://www.seas.upenn.edu/~meam520/notes02/Forces8.pdf
    * @param command The twist command to be converted.
+   * @param planning_frame The name of the planning frame.
    * @return The transformed twist command.
    */
-  TwistCommand toPlanningFrame(const TwistCommand& command) const;
+  std::optional<TwistCommand> toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const;
 
   /**
    * \brief Convert a given pose command to planning frame
-   * @param command The pose command to be converted
-   * @return The transformed pose command
+   * @param command The pose command to be converted.
+   * @param planning_frame The name of the planning frame.
+   * @return The transformed pose command.
    */
-  PoseCommand toPlanningFrame(const PoseCommand& command) const;
+  std::optional<PoseCommand> toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const;
 
   /**
    * \brief Compute the change in joint position required to follow the received command.

moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp

@@ -67,23 +67,27 @@ 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 planning_frame The planning frame name.
  * @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 std::string& planning_frame,
                                      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 planning_frame The planning frame name.
+ * @param ee_frame The end effector frame name.
  * @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 std::string& planning_frame,
+                                    const std::string& ee_frame,
                                     const JointNameToMoveGroupIndexMap& joint_name_group_index_map);
 
 /**

moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp

@@ -54,6 +54,23 @@
 
 namespace moveit_servo
 {
+/**
+ * \brief Get the base frame of the current active joint group or subgroup's IK solver.
+ * @param robot_state A pointer to the current robot state.
+ * @param group_name The currently active joint group name.
+ * @return The IK solver base frame, if one exists, otherwise std::nullopt.
+ */
+std::optional<std::string> getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state,
+                                                const std::string& group_name);
+
+/**
+ * \brief Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.
+ * @param robot_state A pointer to the current robot state.
+ * @param group_name The currently active joint group name.
+ * @return The IK solver tip frame, if one exists, otherwise std::nullopt.
+ */
+std::optional<std::string> getIKSolverTipFrame(const moveit::core::RobotStatePtr& robot_state,
+                                               const std::string& group_name);
 
 /**
  * \brief Checks if a given VectorXd is a valid command.
@@ -112,8 +129,7 @@ std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& s
 
 /**
  * \brief Computes scaling factor for velocity when the robot is near a singularity.
- * @param joint_model_group The joint model group of the robot, used for fetching the Jacobian.
- * @param current_state The current state of the robot, used for singularity look ahead.
+ * @param robot_state A pointer to the current robot state.
  * @param target_delta_x The vector containing the required change in Cartesian position.
  * @param servo_params The servo parameters, contains the singularity thresholds.
  * @return The velocity scaling factor and the reason for scaling.