Review updates

doc/examples/realtime_servo/CMakeLists.txt

@@ -4,7 +4,7 @@ ament_target_dependencies(pose_tracking_tutorial  ${THIS_PACKAGE_INCLUDE_DEPENDS
 
 install(
   TARGETS
-  pose_tracking_tutorial
+    pose_tracking_tutorial
   DESTINATION
     lib/${PROJECT_NAME}
 )

doc/examples/realtime_servo/config/panda_simulated_config.yaml

@@ -46,7 +46,7 @@ ee_frame: panda_hand  # The name of the end effector link, used to return the EE
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
 hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
-leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity.
 
 ## Topic names
 cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands

doc/examples/realtime_servo/realtime_servo_tutorial.rst

@@ -3,11 +3,18 @@ Realtime Servo
 
 MoveIt Servo facilitates realtime control of your robot arm.
 
-Servo accepts any of the following types of commands:
+
+.. raw:: html
+
+    <div style="position: relative; padding-bottom: 5%; height: 0; overflow: hidden; max-width: 100%; height: auto;">
+        <iframe width="700px" height="400px" src="https://www.youtube.com/embed/j45Lagelpwo" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
+    </div>
+
+MoveIt Servo accepts any of the following types of commands:
 
     1. Individual joint velocities.
-    2. The desired velocity of end-effector.
-    3. The desired pose of end-effector.
+    2. The desired velocity of end effector.
+    3. The desired pose of end effector.
 
 This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance.
 
@@ -20,57 +27,101 @@ If you haven't already done so, make sure you've completed the steps in :doc:`Ge
 Design overview
 ---------------
 
-Moveit Servo consists of two main parts, the core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which
-wraps the C++ interface and provides a ROS interface.
-
-The configuration of Servo is done through ROS parameters specified in `servo_parameters.yaml <https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/config/servo_parameters.yaml>`_
+Moveit Servo consists of two main parts: The core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which
+wraps the C++ interface and provides a ROS interface.The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml <moveit_ros/moveit_servo/config/servo_parameters.yaml>`
 
 In addition to the servoing capability, MoveIt Servo has some convenient features such as:
 
     - Checking for singularities
     - Checking for collisions
+    - Motion smoothing
+    - Joint position and velocity limits enforced
+
+Singularity checking and collision checking are safety features that scale down the velocities when approaching singularities or collisions (self collision or collision with other objects).
+The collision checking and smoothing are optional features that can be disabled using the ``check_collisions`` parameter and the ``use_smoothing`` parameters respectively.
+
+The inverse kinematics is handled through either the inverse Jacobain or the robot's IK solver if one was provided.
+
+
+Inverse Kinematics in Servo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin.
+To configure an IK plugin for use in MoveIt Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one
+in the :moveit_resources_codedir:`Panda config package <panda_moveit_config/config/kinematics.yaml>`.
+Several IK plugins are available :moveit_codedir:`within MoveIt <moveit_kinematics>`, as well as `externally <https://github.com/PickNikRobotics/bio_ik/tree/ros2>`_.
+:code:`bio_ik/BioIKKinematicsPlugin` is the most common choice.
+
+Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file:
+
+.. code-block:: python
+
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .to_moveit_configs()
+    )
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_node",
+        parameters=[
+            servo_params,
+            low_pass_filter_coeff,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed
+        ],
+    )
+
+
+The above excerpt is taken from :moveit_codedir:`servo_example.launch.py <moveit_ros/moveit_servo/launch/demo_ros_api.launch.py>` in MoveIt.
+In the above example, the :code:`kinematics.yaml` file is taken from the :moveit_resources_codedir:`moveit_resources </>` repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`.
+The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics.<group_name>.<param_name>`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`.
+
+Since :code:`moveit_servo` does not allow undeclared parameters found in the :code:`kinematics.yaml` file to be set on the Servo node, custom solver parameters need to be declared from inside your plugin code.
+
+For example, :code:`bio_ik` defines a :code:`getROSParam()` function in `bio_ik/src/kinematics_plugin.cpp <https://github.com/PickNikRobotics/bio_ik/blob/ros2/src/kinematics_plugin.cpp#L160>`_ that declares parameters if they're not found on the Servo Node.
+
+Setup on a New Robot
+--------------------
+
+The bare minimum requirements for running MoveIt Servo with your robot include:
+    1. A valid URDF and SRDF of the robot.
+    2. A controller that can accept joint positions or velocities.
+    3. Joint encoders that provide rapid and accurate joint position feedback.
+
+Because the kinematics are handled by the core parts of MoveIt, it is recommended that you have a valid config package for your robot and you can run the demo launch file included with it.
 
-The collision checking is an optional feature that can be disabled using the ``check_collisions`` parameter.
 
 Using the C++ API
 ------------------
-The C++ interface allows the user to create and utilize an instance of Servo within their C++ programs.
-This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture,
-or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface.
+This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture, or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface.
 
-When using Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``.
+When using MoveIt Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``.
 The output from Servo when using the C++ interface is ``KinematicState``, a struct containing joint names, positions, velocities and accelerations.
-As given by the definitions in `datatypes <https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp>`_ header file.
+As given by the definitions in :moveit_codedir:`datatypes <moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp>` header file.
 
-The first step is to create a Servo instance.
+The first step is to create a ``Servo`` instance.
 
 .. code-block:: c++
 
     // Import the Servo headers.
     #include <moveit_servo/servo.hpp>
     #include <moveit_servo/utils/common.hpp>
 
-.. code-block:: c++
-
     // The node to be used by Servo.
     rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("servo_tutorial");
 
-.. code-block:: c++
-
     // Get the Servo parameters.
     const std::string param_namespace = "moveit_servo";
     const std::shared_ptr<const servo::ParamListener> servo_param_listener =
         std::make_shared<const servo::ParamListener>(node, param_namespace);
     const servo::Params servo_params = servo_param_listener->get_params();
 
-.. code-block:: c++
-
     // Create the planning scene monitor.
     const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
         createPlanningSceneMonitor(node, servo_params);
 
-.. code-block:: c++
-
     // Create a Servo instance.
     Servo servo = Servo(node, servo_param_listener, planning_scene_monitor);
 
@@ -129,16 +180,16 @@ Using the PoseCommand
 
 The ``next_joint_state`` result can then be used for further steps in the control pipeline.
 
-The status of Servo resulting from the last command can be obtained by:
+The status of MoveIt Servo resulting from the last command can be obtained by:
 
 .. code-block:: c++
 
     StatusCode status = servo.getStatus();
 
-The satus can be used as input for various decisions.
+The user can use status for higher-level decision making.
 
-See `moveit_servo/demos <https://github.com/ros-planning/moveit2/tree/main/moveit_ros/moveit_servo/demos/cpp_interface>`_ for complete examples of using the C++ interface.
-The demos can be launched using the launch files found in `moveit_servo/launch <https://github.com/ros-planning/moveit2/tree/main/moveit_ros/moveit_servo/launch>`_.
+See :moveit_codedir:`moveit_servo/demos <moveit_ros/moveit_servo/demos/cpp_interface>` for complete examples of using the C++ interface.
+The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch <moveit_ros/moveit_servo/launch>`.
 
     - ``ros2 launch moveit_servo demo_joint_jog.launch.py``
     - ``ros2 launch moveit_servo demo_twist.launch.py``
@@ -148,31 +199,32 @@ The demos can be launched using the launch files found in `moveit_servo/launch <
 Using the ROS API
 -----------------
 
-To use Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen `here <https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py>`_.
+To use MoveIt Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen :moveit_codedir:`here <moveit_ros/moveit_servo/launch/demo_ros_api.launch.py>`.
 
-When using Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters.
+When using MoveIt Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters.
 
-    1. ``control_msgs::msg::JointJog`` on the topic specified by ``joint_command_in_topic`` parameter.
-    2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by ``cartesian_command_in_topic`` parameter.
-    3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by ``pose_command_in_topic`` parameter.
+    1. ``control_msgs::msg::JointJog`` on the topic specified by the ``joint_command_in_topic`` parameter.
+    2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. For now, the twist message must be in the planning frame of the robot. (This will be updated soon.)
+    3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter.
 
-The output from Servo can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray``
+Twist and Pose commands require that the ``header.frame_id`` is always specified.
+The output from ``ServoNode`` (the ROS interface) can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray``
 selected using the *command_out_type* parameter, and published on the topic specified by *command_out_topic* parameter.
 
-The command type for Servo can be selected using the ``ServoCommandType`` service, see definition `ServoCommandType <https://github.com/ros-planning/moveit_msgs/blob/ros2/srv/ServoCommandType.srv>`_.
+The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType <srv/ServoCommandType.srv>`.
 
-From cli : ``ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"``
+From cli : ``ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"``
 
 Programmatically:
 
 .. code-block:: c++
 
-        swith_input_client = node->create_client<moveit_msgs::srv::ServoCommandType>("servo_node/switch_command_type");
+        switch_input_client = node->create_client<moveit_msgs::srv::ServoCommandType>("/<node_name>/switch_command_type");
         auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
         request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST;
-        if (swith_input_client->wait_for_service(std::chrono::seconds(1)))
+        if (switch_input_client->wait_for_service(std::chrono::seconds(1)))
         {
-          auto result = swith_input_client->async_send_request(request);
+          auto result = switch_input_client->async_send_request(request);
           if (result.get()->success)
           {
             RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Twist");
@@ -183,14 +235,14 @@ Programmatically:
           }
         }
 
-Similarly, Servo can be paused using the pause service of type ``std_msgs::srv::SetBool``.
+Similarly, servoing can be paused using the pause service ``<node_name>/pause_servo`` of type ``std_msgs::srv::SetBool``.
 
-When using the ROS interface, the status of Servo is available on the topic ``/servo_node/status``, see definition `ServoStatus <https://github.com/ros-planning/moveit_msgs/blob/ros2/msg/ServoStatus.msg>`_.
+When using the ROS interface, the status of Servo is available on the topic ``/<node_name>/status``, see definition :moveit_msgs_codedir:`ServoStatus <msg/ServoStatus.msg>`.
 
 Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``.
 
 Once the demo is running, the robot can be teleoperated through the keyboard.
 
 Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``.
 
-An example of using the pose commands in the context of servoing to open a door can be seen in this `example <https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp>`_.
+An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example <examples/realtime_servo/src/pose_tracking_tutorial.cpp>`.

doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp

@@ -1,4 +1,44 @@
-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*      Title       : pose_tracking_tutorial.cpp
+ *      Project     : moveit2_tutorials
+ *      Created     : 08/07/2023
+ *      Author      : V Mohammed Ibrahim
+ *
+ *      Description : Example of using pose tracking via the ROS API in a door opening scenario.
+ */
 
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -9,6 +49,9 @@
 #include <moveit_servo/utils/common.hpp>
 #include <moveit_msgs/srv/servo_command_type.hpp>
 
+/**
+ * \brief Handles the simulation of the collision object representing the door.
+ */
 class Door
 {
 public:
@@ -82,6 +125,9 @@ class Door
   shape_msgs::msg::SolidPrimitive door_primitive_;
 };
 
+/**
+ * \brief Generates the path to follow when opening the door.
+ */
 std::vector<Eigen::Vector3d> getPath()
 {
   const double start_angle = M_PI / 2 + (M_PI / 8);
@@ -99,6 +145,9 @@ std::vector<Eigen::Vector3d> getPath()
   return traj;
 }
 
+/**
+ * \brief Creates an Rviz marker message to represent a waypoint in the path.
+ */
 visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& position, const std::string& frame)
 {
   visualization_msgs::msg::Marker marker;
@@ -125,6 +174,9 @@ visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& positio
   return marker;
 }
 
+/**
+ * \brief Generates a PoseStamped message with the given position and orientation.
+ */
 geometry_msgs::msg::PoseStamped getPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation)
 {
   geometry_msgs::msg::PoseStamped target_pose;
@@ -194,29 +246,34 @@ int main(int argc, char* argv[])
   const double publish_period = 0.15;
   rclcpp::WallRate rate(1 / publish_period);
 
-  size_t i = path.size() - 1;
-  while (i > 0)
+  // The path needs to be reversed since the last point in the path is where we want to start.
+  std::reverse(path.begin(), path.end());
+
+  for (auto& waypoint : path)
   {
-    auto target_pose = getPose(path[i], Eigen::Quaterniond(ee_pose.rotation()));
+    auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation()));
     target_pose.header.stamp = node->now();
     pose_publisher->publish(target_pose);
     rate.sleep();
-    i--;
   }
 
+  // Simulated door opening
+
   double door_angle = M_PI / 2;
   const double step = 0.01745329;  // 1 degree in radian
-  i = 0;
-  while (i < path.size())
+
+  // Reverse again to so that we follow that path in reverse order.
+  std::reverse(path.begin(), path.end());
+  for (auto& waypoint : path)
   {
     ee_pose.rotate(Eigen::AngleAxisd(-step, Eigen::Vector3d::UnitZ()));
-    auto target_pose = getPose(path[i], Eigen::Quaterniond(ee_pose.rotation()));
+    auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation()));
     target_pose.header.stamp = node->now();
     pose_publisher->publish(target_pose);
     rate.sleep();
+
     door.rotateDoor(door_angle);
     door_angle += step;
-    i++;
   }
 
   executor->cancel();