.. Description of examples code 10/11/24 Abhishekh Reddy Description of the examples code ================================ The examples demonstrate four different ways of moving the robotic arm using the Move Group Interface. Here's the same snippet of code from the source file with the comments removed. .. code-block:: C++ void UR3eMoveInterface::examplesMoveIt() { waitForMoveGroupInterface(); RCLCPP_INFO(this->get_logger(), "Running examples..."); // Example 1 RCLCPP_INFO(this->get_logger(), "Example 1"); std::vector target_joint_positions {M_PI_2, -M_PI_2, M_PI_2, -M_PI, -M_PI_2, 0}; move_group_interface_->setJointValueTarget(target_joint_positions); moveit::planning_interface::MoveGroupInterface::Plan motion_plan_joints; bool joint_space_plan_success {planToJointSpaceGoal(target_joint_positions, motion_plan_joints)}; if (joint_space_plan_success) move_group_interface_->execute(motion_plan_joints); // Example 2 RCLCPP_INFO(this->get_logger(), "Example 2"); std::vector waypoints; auto current_pose = move_group_interface_->getCurrentPose().pose; waypoints.push_back(current_pose); double triangle_side {0.25}; auto pose1 = current_pose; pose1.position.z += std::sqrt(3) * 0.25 * triangle_side; waypoints.push_back(pose1); auto pose2 = pose1; pose2.position.z -= std::sqrt(3) * 0.5 * triangle_side; pose2.position.y += 0.5 * triangle_side; waypoints.push_back(pose2); auto pose3 = pose2; pose3.position.y -= triangle_side; waypoints.push_back(pose3); auto pose4 = pose3; pose4.position.z += std::sqrt(3) * 0.5 * triangle_side; pose4.position.y += 0.5 * triangle_side; waypoints.push_back(pose4); auto pose5 = pose4; pose5.position.z -= std::sqrt(3) * 0.25 * triangle_side; waypoints.push_back(pose5); moveit_msgs::msg::RobotTrajectory cartesian_trajectory; bool cartesian_plan_success {planCartesianPath(waypoints, cartesian_trajectory)}; auto track_request {std::make_shared()}; track_request->tf_root_frame_name = move_group_interface_->getPlanningFrame(); track_request->tf_tip_frame_name = move_group_interface_->getEndEffectorLink(); track_request->plot_title = "Example: A Triangle in the vertical plane"; track_request->plot_axis_x = lab8::srv::TrackRequest::Request::Y_AXIS; track_request->plot_axis_y = lab8::srv::TrackRequest::Request::Z_AXIS; track_request->status = lab8::srv::TrackRequest::Request::START; bool track_request_success {sendEEFTrackRequest(track_request)}; if (cartesian_plan_success && track_request_success) move_group_interface_->execute(cartesian_trajectory); track_request->status = lab8::srv::TrackRequest::Request::STOP; sendEEFTrackRequest(track_request); // Example 3 RCLCPP_INFO(this->get_logger(), "Example 3"); geometry_msgs::msg::Pose target_pose; target_pose = move_group_interface_->getCurrentPose().pose; target_pose.position.z += 0.05; target_pose.position.y += 0.1; tf2::Quaternion current_orientation_tf; tf2::convert(target_pose.orientation, current_orientation_tf); tf2::Quaternion desired_rotation_tf; desired_rotation_tf.setRPY(0, -M_PI_2, 0); auto target_orientation_tf = desired_rotation_tf * current_orientation_tf; tf2::convert(target_orientation_tf, target_pose.orientation); move_group_interface_->setPoseTarget(target_pose); move_group_interface_->move(); // Example 4 move_group_interface_->setNamedTarget("up"); move_group_interface_->move(); } Each example follows these three steps at a high-level to move the robotic arm. - Set the target (Either in joint-space or Cartesian space) - Create a motion plan to the target state - Execute the motion plan Beginning part of the code ^^^^^^^^^^^^^^^^^^^^^^^^^^ This part of the code waits for the Move Group Interface node to initialize in the constructor before calling any of its methods. It is an empty while loop conditioned to exit when the Move Group Interface node initializes. .. code-block:: C++ waitForMoveGroupInterface(); Example 1 ^^^^^^^^^ The first example demonstrates moving the arm by setting a joint-space goal. Set the target -------------- A vector of doubles is used to store the desired joint positions in radians as a list. The size of this vector corresponds to the number of joints in the robotic arm. .. code-block:: C++ std::vector target_joint_positions {M_PI_2, -M_PI_2, M_PI_2, -M_PI, -M_PI_2, 0}; move_group_interface_->setJointValueTarget(target_joint_positions); The ``setJointValueTarget`` method from the Move Group Interface takes this vector as the target joint configuration, setting the goal for the robot's motion planning. .. tip:: The included math library ``cmath`` provides constants for certain angles in radians like :math:`\frac{\pi}{2}` (``M_PI_2``) and :math:`\pi` (``M_PI``). Create a motion plan to the target state ---------------------------------------- The motion plan is generated by the wrapper function ``planToJointSpaceGoal``, which internally calls the ``plan()`` method of the Move Group Interface. The generated plan is stored in an external ``MoveGroupInterface::Plan`` variable. .. code-block:: C++ moveit::planning_interface::MoveGroupInterface::Plan motion_plan_joints; bool joint_space_plan_success {planToJointSpaceGoal(target_joint_positions, motion_plan_joints)}; Wrapper functions, such as ``planToJointSpaceGoal``, abstract the configuration, condition checks, and error handling involved in generating a plan using the Move Group Interface. Execute the motion plan ----------------------- Finally, the ``execute()`` method is called to execute the motion plan which is previously generated and stored in a variable. .. code-block:: C++ if (joint_space_plan_success) move_group_interface_->execute(motion_plan_joints); Example 2 ^^^^^^^^^ This example demonstrates drawing an equilateral triangle with the arm using a list of waypoints for the end-effector to follow in Cartesian space. Set the target -------------- A vector of ``Pose`` messages is used as a list to store the target waypoints. Then the current pose of the end-effector is added as an initial waypoint. This position will also be the center point of the shape, which depends on the arm's position following its previous motion (Example 1). .. code-block:: C++ std::vector waypoints; auto current_pose = move_group_interface_->getCurrentPose().pose; waypoints.push_back(current_pose); Then the remaning waypoints relative to the first waypoint were added. For a triangle those waypoints relative to the first waypoint could be: A point directly above the center by half the height of the triangle :math:`\frac{1}{2} \times \frac{\sqrt{3}}{2} \times a` , where :math:`a` is the side length in meters which is hard coded to ``0.25``. .. code-block:: C++ double triangle_side {0.25}; auto pose1 = current_pose; pose1.position.z += std::sqrt(3) * 0.25 * triangle_side; waypoints.push_back(pose1); A point below the previous point by the triangle height :math:`\frac{\sqrt{3}}{2} \times a` and towards the right by half the side length :math:`\frac{a}{2}`. .. code-block:: C++ auto pose2 = pose1; pose2.position.z -= std::sqrt(3) * 0.5 * triangle_side; pose2.position.y += 0.5 * triangle_side; waypoints.push_back(pose2); A point towards the left from the previous point by side length :math:`a`. .. code-block:: C++ auto pose3 = pose2; pose3.position.y -= triangle_side; waypoints.push_back(pose3); A point above by the triangle height :math:`\frac{\sqrt{3}}{2} \times a` and towards the right by half the side length :math:`\frac{a}{2}`. (Same as the second point, closing the triangle loop) .. code-block:: C++ auto pose4 = pose3; pose4.position.z += std::sqrt(3) * 0.5 * triangle_side; pose4.position.y += 0.5 * triangle_side; waypoints.push_back(pose4); Finally, a point just below the previous point by half the triangle height :math:`\frac{1}{2} \times \frac{\sqrt{3}}{2} \times a`, returning back to the center/initial point. .. code-block:: C++ auto pose5 = pose4; pose5.position.z -= std::sqrt(3) * 0.25 * triangle_side; waypoints.push_back(pose5); .. note:: As per the ROS convention `REP103 `_ the directions of the coordinate axes are: - Positive Z-Axis for up direction - Positive X-Axis for forward direction - Positive Y-Axis for left direction (or right when seen from the front in this lab exercise) Create a motion plan to the target state ---------------------------------------- Unlike the previous example, Cartesian plan is stored in a different object type. ``planCartesianPath`` provides abstraction from calling the ``computeCartesianPath`` method in the Move Group Interface and time parameterization of the generated motion plan as a post-processing step. .. code-block:: C++ moveit_msgs::msg::RobotTrajectory cartesian_trajectory; bool cartesian_plan_success {planCartesianPath(waypoints, cartesian_trajectory)}; Execute the motion plan ----------------------- .. code-block:: C++ if (cartesian_plan_success && track_request_success) move_group_interface_->execute(cartesian_trajectory); Executes Cartesian motion plan to make the end-effector follow the triangular path. Example 3 ^^^^^^^^^ This example demonstrates moving the robotic arm by setting a Cartesian space goal for the end-effector. Set the target -------------- Only a single ``Pose`` message is used to set the target. This target pose is initialized with the current end-effector pose to begin with. .. code-block:: C++ geometry_msgs::msg::Pose target_pose; target_pose = move_group_interface_->getCurrentPose().pose; Linear displacements along the Z and Y axes are applied relative to the current position. .. code-block:: C++ target_pose.position.z += 0.05; target_pose.position.y += 0.1; Angular displacements are applied similarly, relative to the current orientation. However, since ``Pose`` messages store orientation using quaternions, it is not intuitive to set rotation values directly. Instead, angular displacements can be defined using roll-pitch-yaw values and then converted to quaternions. While quaternions from ``geometry_msgs`` do not support this conversion, ``TF2`` quaternions do, and they also provide functionality for converting between these two types. So first, the current orientation of the end-effector is converted from ``geometry_msgs`` quaternion to ``TF2`` quaternion. .. code-block:: C++ tf2::Quaternion current_orientation_tf; tf2::convert(target_pose.orientation, current_orientation_tf); Then, a new ``TF2`` quarternion message is defined with the desired rotations in roll-pitch-yaw. .. code-block:: C++ tf2::Quaternion desired_rotation_tf; desired_rotation_tf.setRPY(0, -M_PI_2, 0); This new rotation is then applied relative to current orientation by pre-multiplying it with the current orientation quarternion. .. code-block:: C++ auto target_orientation_tf = desired_rotation_tf * current_orientation_tf; tf2::convert(target_orientation_tf, target_pose.orientation); The resultant ``TF2`` quarternion is then converted back to ``geometry_msgs`` quaternion to set the target pose. .. code-block:: C++ move_group_interface_->setPoseTarget(target_pose); Create a motion plan to the target state ---------------------------------------- This will be automatically handled in the next step. Execute the motion plan ----------------------- .. code-block:: C++ move_group_interface_->move(); .. tip:: The ``move()`` method combines both the planning ``plan()`` and execution ``execute()`` steps into a single function call. However, using the ``plan()`` and ``execute()`` methods separately gives more flexibility in many cases. This does not work for planning and executing Cartesian paths like in Example 2. Example 4 ^^^^^^^^^ This example demonstrates moving the robotic arm to a predefined position using names. Set the target -------------- .. code-block:: C++ move_group_interface_->setNamedTarget("up"); The available options for named targets you can set is based on the MoveIt! configuration for the robot. More specifically, based on the definitions in the *Semantic Robot Description Format (.srdf)* file. The listed definitions in `Lab's SRDF file `_ are shown in the snippet below. .. code-block:: xml Create a motion plan to the target state ---------------------------------------- This will be automatically handled in the next step. Execute the motion plan ----------------------- .. code-block:: C++ move_group_interface_->move(); .. _end effector tracking: Using the end-effector tracking service (In Example 2) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This section explains how to use this service to plot the resulting shape drawn by the robot. This data can be included in your lab report. The service automatically starts by default when you launch the Lab 8 (in both simulation and on the real arm) unless the ``track_eef`` launch argument is set to ``false``. .. note:: The ``draw shape`` node already has the client for this service included. You just have to: - Send a ``START`` request just before starting to draw a shape (Before executing the Cartesian plan in code) - Then a ``STOP`` request just after drawing the shape (Just after the ``execute()`` method for Cartesian plan returns). The structure of the service message is as shown below: .. code-block:: C // Predefined Constants int8 X_AXIS = 0 int8 Y_AXIS = 1 int8 Z_AXIS = 2 bool STOP = 0 bool START = 1 // Request bool status string tf_root_frame_name string tf_tip_frame_name string plot_title int8 plot_axis_x int8 plot_axis_y // Response bool success The fields for the request can be populated using inbuilt constants or custom values as long as the data type matches with the field. Example service request to start tracking ----------------------------------------- .. code-block:: C++ auto track_request {std::make_shared()}; track_request->tf_root_frame_name = move_group_interface_->getPlanningFrame(); track_request->tf_tip_frame_name = move_group_interface_->getEndEffectorLink(); track_request->plot_title = "Example: A Triangle in the vertical plane"; track_request->plot_axis_x = lab8::srv::TrackRequest::Request::Y_AXIS; track_request->plot_axis_y = lab8::srv::TrackRequest::Request::Z_AXIS; track_request->status = lab8::srv::TrackRequest::Request::START; bool track_request_success {sendEEFTrackRequest(track_request)}; Sends a ``START`` request to track the end-effector poses: - With the end-effector frame name given by MoveIt!'s ``getEndEffectorLink`` method - With the reference frame name given by MoveIt!'s ``getPlanningFrame`` method (which is the robot base frame). - With a plot titled: ``Example: A Triangle in the vertical plane`` - With the X points on the plot to be the Y positions of the end-effector - With the Y points on the plot to be the Z positions of the end-effector The wrapper function ``sendEEFTrackRequest`` abstracts the specific implementation for sending a request using the client, waiting for the response and error handling. The response after sending a service request will be a ``bool`` which notifies whether the request has been successfully fulfilled. This value can be used for further error-handling in your code. Example service request to stop tracking ---------------------------------------- .. code-block:: C++ track_request->status = lab8::srv::TrackRequest::Request::STOP; bool track_request_success {sendEEFTrackRequest(track_request)}; You can reuse the same request message used to start tracking the poses. Just modify the ``status`` field to ``STOP`` value and resend it to stop tracking. Values in all the other fields can be ignored in this case. When this request is successfully fulfilled by the service, a plot of the end-effector positions should pop-up. In addition, a PNG of the plot and a CSV file with the raw data will be saved to the ``output/lab8/`` directory within the workspace folder. Reference --------- `Move Group C++ Interface Documentation `_ .. LINK REFERENCES --------------------------------------------------------------------------------- .. _REP103 Link: https://www.ros.org/reps/rep-0103.html .. _Lab SRDF Link: https://github.com/ENEE467/lab-workspace/blob/main/src/lab_moveit_config/srdf/lab.srdf .. _MoveIt! Link: https://moveit.picknik.ai/main/doc/examples/move_group_interface/move_group_interface_tutorial.html