Documentation Version

You're reading the documentation for a stable version of MoveIt that is not being developed further. For information on the recommended version, please have a look at Main.

Realtime Arm Servoing

MoveIt Servo allows you to stream End Effector (EEF) velocity commands to your manipulator and have it execute them concurrently. 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.

This tutorial shows how to use MoveIt Servo to send real-time servo commands to a ROS-enabled robot. Some nice features of the servo node are singularity handling and collision checking that prevents the operator from breaking the robot.

Getting Started

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

Launching a Servo Node

MoveIt Servo can be launched as a “node component” or a standalone node. The launch file, moveit_servo/servo_example.launch.py, launches a standalone node by default but also contains a commented component node. Commands are sent via ROS topics. The commands can come from anywhere, such as a joystick, keyboard, or other controller.

This demo was written for an Xbox 1 controller, but can be easily modified to use any controller compatible with the Joy package by modifying the joystick_servo_example.cpp file.

To run the demo, make sure your controller is plugged in and can be detected by ros2 run joy joy_node. Usually this happens automatically after plugging the controller in. Then launch with

ros2 launch moveit_servo servo_example.launch.py

Make a service request to start Servo

ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}

You should be able to control the arm with your controller now, with MoveIt Servo automatically avoiding singularities and collisions.

Without a Controller

If you do not have a joystick or game controller, you can still try the demo using your keyboard. With the demo still running, in a new terminal, run

ros2 run moveit2_tutorials servo_keyboard_input

You will be able to use your keyboard to servo the robot. Send Cartesian commands with arrow keys and the . and ; keys. Change the Cartesian command frame with W for world and E for End-Effector. Send joint jogging commands with keys 1-7 (use R to reverse direction)

Note that the controller overlay here is just for demonstration purposes and is not actually included

Introspection

Here are some tips for inspecting and/or debugging the system.

1. View the ros2_controllers that are currently active with ros2 control list_controllers. You will see a JointTrajectoryController that receives the joint position commands from Servo and handles them in the simulated robot driver. The JointTrajectoryController is very flexible; it can handle any combination of position/velocity/(position-and-velocity) input. Servo is also compatible with JointGroupPosition or JointGroupVelocity-type controllers.

2. ros2 topic echo /servo_node/status shows the current state of the Servo node. If 0 is published, all is well. The definition for all enums can be seen here.

3. ros2 node list shows the following. ros2 node info can be used to get more information about any of these nodes.

   • /joy_node handles commands from the XBox controller

   • /moveit_servo_demo_container holds several ancillary ROS2 “component nodes” that are placed in a container for faster intra-process communication

   • /servo_node which does the calculations and collision checking for this demo. servo_node may be moved into the demo container in the future

Using the C++ Interface

Instead of launching Servo as its own component, you can include Servo in your own nodes via the C++ interface. Sending commands to the robot is very similar in both cases, but for the C++ interface a little bit of setup for Servo is necessary. In exchange, you will be able to directly interact with Servo through its C++ API.

This basic C++ interface demo moves the robot in a predetermined way and can be launched with

ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py

An Rviz window should appear with a Panda arm and collision object. The arm will joint-jog for a few seconds before switching to a Cartesian movement. As the arm approaches the collision object, it slows and stops.

Entire Code

The entire code is available here

Setup

First we declare pointers to the node and publisher that will publish commands to Servo

rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
size_t count_ = 0;

Next we will set up the node, planning_scene_monitor, and collision object

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;

This is false for now until we fix the QoS settings in moveit to enable intra process comms

node_options.use_intra_process_comms(false);
node_ = std::make_shared<rclcpp::Node>("servo_demo_node", node_options);

Pause for RViz to come up. This is necessary in an integrated demo with a single launch file

rclcpp::sleep_for(std::chrono::seconds(4));

Create the planning_scene_monitor. We need to pass this to Servo’s constructor, and we should set it up first before initializing any collision objects

auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
    node_, "robot_description", tf_buffer, "planning_scene_monitor");

Here we make sure the planning_scene_monitor is updating in real time from the joint states topic

if (planning_scene_monitor->getPlanningScene())
{
  planning_scene_monitor->startStateMonitor("/joint_states");
  planning_scene_monitor->setPlanningScenePublishingFrequency(25);
  planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                       "/moveit_servo/publish_planning_scene");
  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->providePlanningSceneService();
}
else
{
  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
  return EXIT_FAILURE;
}

These are the publishers that will send commands to MoveIt Servo. Two command types are supported: JointJog messages which will directly jog the robot in the joint space, and TwistStamped messages which will move the specified link with the commanded Cartesian velocity. In this demo, we jog the end effector link.

joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("servo_demo_node/delta_joint_cmds", 10);
twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("servo_demo_node/delta_twist_cmds", 10);

Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down and stop before colliding

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0";
collision_object.id = "box";

Make a box and put it in the way

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.6;
box_pose.position.y = 0.0;
box_pose.position.z = 0.6;

Add the box as a collision object

collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

Create the message to publish the collision object

moveit_msgs::msg::PlanningSceneWorld psw;
psw.collision_objects.push_back(collision_object);
moveit_msgs::msg::PlanningScene ps;
ps.is_diff = true;
ps.world = psw;

Publish the collision object to the planning scene

auto scene_pub = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10);
scene_pub->publish(ps);

Initializing Servo

Servo requires a number of parameters to dictate its behavior. These can be read automatically by using the makeServoParameters helper function

auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_);
if (!servo_parameters)
{
  RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
  return EXIT_FAILURE;
}

Initialize the Servo C++ interface by passing a pointer to the node, the parameters, and the PSM

auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);

You can start Servo directly using the C++ interface. If launched using the alternative ServoNode, a ROS service is used to start Servo. Before it is started, MoveIt Servo will not accept any commands or move the robot

servo->start();

Sending Commands

For this demo, we will use a simple ROS timer to send joint and twist commands to the robot

rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

Here is the timer callback for publishing commands. The C++ interface sends commands through internal ROS topics, just like if Servo was launched using ServoNode.

void publishCommands()
{

First we will publish 100 joint jogging commands. The joint_names field allows you to specify individual joints to move, at the velocity in the corresponding velocities field. It is important that the message contains a recent timestamp, or Servo will think the command is stale and will not move the robot.

if (count_ < 100)
{
  auto msg = std::make_unique<control_msgs::msg::JointJog>();
  msg->header.stamp = node_->now();
  msg->joint_names.push_back("panda_joint1");
  msg->velocities.push_back(0.3);
  joint_cmd_pub_->publish(std::move(msg));
  ++count_;
}

After a while, we switch to publishing twist commands. The provided frame is the frame in which the twist is defined, not the robot frame that will follow the command. Again, we need a recent timestamp in the message

  else
  {
    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = "panda_link0";
    msg->twist.linear.x = 0.3;
    msg->twist.angular.z = 0.5;
    twist_cmd_pub_->publish(std::move(msg));
  }
}

We use a multithreaded executor here because Servo has concurrent processes for moving the robot and avoiding collisions

auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
executor->add_node(node_);
executor->spin();

Servo Overview

The following sections give some background information about MoveIt Servo and describe the first steps to set it up on your robot.

Servo includes a number of nice features:

1. Cartesian End-Effector twist commands

2. Joint commands

3. Collision checking

4. Singularity checking

5. Joint position and velocity limits enforced

6. Inputs are generic ROS messages

Setup on a New Robot

Preliminaries

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 from a ROS topic

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.

Input Devices

The two primary inputs to MoveIt Servo are Cartesian commands and joint commands. These come into Servo as TwistStamped and JointJog messages respectively. The source of the commands can be almost anything including: gamepads, voice commands, a SpaceNav mouse, or PID controllers (e.g. for visual servoing).

Requirements for incoming command messages, regardless of input device are:

1. TwistStamped and JointJog: need a timestamp in the header that is updated when the message is published

2. JointJog: must have valid joint names in the joint_names field that correspond with the commands given in the displacements or velocities fields

3. (Optional) TwistStamped: can provide an arbitrary frame_id in the header that the twist will be applied to. If empty, the default from the configs is used

Servo Configs

The demo config file shows the parameters needed for MoveIt Servo and is well documented.

Start with the parameters from the demo file, but some must be changed for your specific setup:

1. robot_link_command_frame: Update this to be a valid frame in your robot, recommended as the planning frame or EEF frame

2. command_in_type: Set to “unitless” if your input comes from a joystick, “speed_units” if the input will be in meters/second or radians/second

3. command_out_topic: Change this to be the input topic of your controller

4. command_out_type: Change this based on the type of message your controller needs

5. publish_joint_positions and publish_joint_velocities: Change these based on what your controller needs. Note if command_out_type == std_msgs/Float64MultiArray, only one of these can be True

6. joint_topic: Change this to be the joint_state topic for your arm, usually /joint_states

7. move_group_name: Change this to be the name of your move group, as defined in your SRDF

8. planning_frame: This should be the planning frame of your group