Skip to content

Fix Servo tutorials for post-GSoC 2023 API changes #1086

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
alltheseas wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Fix Servo tutorials for post-GSoC 2023 API changes #1086

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bafaa98
Update Realtime Servo tutorial for post-GSoC API changes
alltheseas Feb 18, 2026
6ae67d9
Rewrite teleoperation guide for current Servo architecture
alltheseas Feb 21, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 66 additions & 14 deletions doc/examples/realtime_servo/realtime_servo_tutorial.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ 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 :moveit_codedir:`servo_parameters.yaml <moveit_ros/moveit_servo/config/servo_parameters.yaml>`
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:

Expand All @@ -40,7 +40,10 @@ In addition to the servoing capability, MoveIt Servo has some convenient feature
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.
The inverse kinematics is handled through either the inverse Jacobian or the robot's IK solver if one was provided.

If your robot has subgroups (for example, a dual-arm setup), you can switch which subgroup
Servo actuates at runtime using the ``active_subgroup`` parameter.


Inverse Kinematics in Servo
Expand Down Expand Up @@ -74,7 +77,7 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R
)


The above excerpt is taken from :moveit_codedir:`servo_example.launch.py <moveit_ros/moveit_servo/launch/demo_ros_api.launch.py>` in MoveIt.
The above excerpt is taken from :moveit_codedir:`demo_ros_api.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`.

Expand Down Expand Up @@ -117,6 +120,7 @@ The first step is to create a ``Servo`` instance.
// Import the Servo headers.
#include <moveit_servo/servo.hpp>
#include <moveit_servo/utils/common.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>

// The node to be used by Servo.
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("servo_tutorial");
Expand All @@ -134,43 +138,56 @@ The first step is to create a ``Servo`` instance.
// Create a Servo instance.
Servo servo = Servo(node, servo_param_listener, planning_scene_monitor);

// Create a publisher for sending trajectory commands to the robot controller.
auto trajectory_publisher = node->create_publisher<trajectory_msgs::msg::JointTrajectory>(
servo_params.command_out_topic, rclcpp::SystemDefaultsQoS());

// Get the current robot state. This is needed by getNextJointState().
auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState();
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);

Once you have a ``Servo`` instance and a ``robot_state``, you can send commands.
Each command type is shown below.

Using the JointJogCommand
"""""""""""""""""""""""""

.. code-block:: c++

using namespace moveit_servo;

// Create the command.
JointJogCommand command;
command.joint_names = {"panda_link7"};
command.velocities = {0.1};
command.names = {"panda_joint7"};
command.velocities = {0.4};

// Set JointJogCommand as the input type.
servo.setCommandType(CommandType::JOINT_JOG);

// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);
KinematicState next_joint_state = servo.getNextJointState(robot_state, command);

Using the TwistCommand
""""""""""""""""""""""

.. code-block:: c++

using namespace moveit_servo;

// Create the command.
TwistCommand command{"panda_link0", {0.1, 0.0, 0.0, 0.0, 0.0, 0.0};
// Create the command: frame_id and {linear_x, linear_y, linear_z, angular_x, angular_y, angular_z}.
TwistCommand command{"panda_link0", {0.0, 0.0, 0.05, 0.0, 0.0, 0.4}};

// Set the command type.
servo.setCommandType(CommandType::TWIST);

// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);

KinematicState next_joint_state = servo.getNextJointState(robot_state, command);

Using the PoseCommand
"""""""""""""""""""""

.. code-block:: c++

Expand All @@ -185,17 +202,52 @@ Using the PoseCommand

// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);
KinematicState next_joint_state = servo.getNextJointState(robot_state, command);

Publishing the Output
"""""""""""""""""""""

The ``next_joint_state`` result must be published to the robot controller. Servo provides
utility functions for composing trajectory messages and maintaining a sliding window of
recent states:

.. code-block:: c++

using namespace moveit_servo;

std::deque<KinematicState> joint_cmd_rolling_window;

// In your control loop (using the command from one of the sections above):
KinematicState next_joint_state = servo.getNextJointState(robot_state, command);
StatusCode status = servo.getStatus();

The ``next_joint_state`` result can then be used for further steps in the control pipeline.
if (status != StatusCode::INVALID)
{
updateSlidingWindow(next_joint_state, joint_cmd_rolling_window,
servo_params.max_expected_latency, node->now());
if (const auto msg = composeTrajectoryMessage(servo_params, joint_cmd_rolling_window))
{
trajectory_publisher->publish(msg.value());
}
// Update the robot state for the next iteration.
if (!joint_cmd_rolling_window.empty())
{
robot_state->setJointGroupPositions(joint_model_group,
joint_cmd_rolling_window.back().positions);
robot_state->setJointGroupVelocities(joint_model_group,
joint_cmd_rolling_window.back().velocities);
}
}

The status of MoveIt Servo resulting from the last command can be obtained by:

.. code-block:: c++

StatusCode status = servo.getStatus();
std::string status_msg = servo.getStatusMessage();

The user can use status for higher-level decision making.
Use the status for higher-level decision making — for example, to stop sending commands
when the arm reaches a singularity or collision.

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>`.
Expand All @@ -215,7 +267,7 @@ To use MoveIt Servo through the ROS interface, it must be launched as a ``Node``
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 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.)
2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter.
3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter.

Twist and Pose commands require that the ``header.frame_id`` is always specified.
Expand Down
Loading