Introducing MoveIt Servo in ROS 2

AdamPettinger · 2020-09-15T14:05:59.833Z

This is a cross-post of a MoveIt blog post I wrote after my Google Summer of Code work with MoveIt this summer. A video demonstrating and summarizing the features of Servo can be seen here.

Servo Introduction

MoveIt Servo (formerly Jog Arm) is now available in ROS 2 Foxy Fitzroy. It includes all of the great features from ROS 1, including singularity and collision safety, joint limit enforcement, and agnostic input. New for ROS 2 is support for running Servo as a composable node, increased test coverage, and ready-made demonstrations and examples.

MoveIt Servo allows you to send end effector velocity commands directly to the robot, and is part of MoveIt 2’s focus on realtime performance. As with the ROS 1 version, using Servo does not preclude you from using MoveIt’s planning and execution framework, and using both alongside each other remains a powerful tool for controlling your manipulator. The remainder of this article summarizes the new features of Servo and gives ideas for its use.

Servo Possibilities

Varied input devices. Because the input to Servo is a geometry_msgs/TwistStamped, the source of the input has unlimited options. Gamepads, VR hand controllers, and 6 DoF CAD mice all work great, but you could also send commands via another ROS node to enable voice-to-command control, visual servoing, or virtual fixture control. Additionally, any number of these input devices can be used together to give your robot different input modes.
Command frame choice. The TwistStamped input includes frame information which is up to the user to set. Changing this during run-time is nice while teleoperating and allows you to pick whichever frame is easiest to control in. Common choices include the End Effector and base frame. If performing a task with a defined frame, you can also servo in the task frame!

Pick and place, mobile manipulation, and contact tasks are the kinds of things Servo can help you with, and now those capabilities are available in ROS 2! Additionally, Servo’s capabilities are continuing to grow with ongoing work to use Servo to move a robot to a target Pose and make some servoing tasks easier.

Interface Options

Servo can still be run through its C++ API, and this remains a great option for using Servo in your projects. A demonstration of the C++ API in action was included with the ROS 2 effort, and the hope is that you can get MoveIt Servo running within minutes using this demo, similar to the available moveit_cpp demonstration included in the Foxy release. See the Servo demonstration page for details on getting started.

Servo also makes use of the ROS 2 composable node framework and offers a component that can be run by itself or in a component container with the rest of your project, allowing intra-process communication. The component (named moveit_servo::ServoServer) takes care of all of the run-time details you would need to manage if using the C++ API interface: loading the parameters, setting up the planning scene, and starting Servo.

The example of the component interface shows how to launch Servo as a component, including enabling intra-process communications to avoid unnecessary message copies. See the demonstrations page to get started.

Regardless of how you launch Servo, interacting with it is largely the same and done through ROS topics and services. Publishing commands is as simple as:

auto twist_pub = node->create_publisher<geometry_msgs::msg::TwistStamped>(“command_topic”, 10);

auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
msg->header.stamp = node->now();
msg->header.frame_id = "command_frame_name";
msg->twist.linear.x = 0.3;
msg->twist.angular.z = 0.5;

twist_pub->publish(std::move(msg));

Also available through the ROS interface for both the C++ API and component:

Sending joint commands
Changing the “control” dimensions to fil
Changing the “drift” dimensions to avoid singularities

Additional services for starting and stopping Servo are available with the component method. See the tutorial page for a detailed overview of MoveIt Servo.

Achllle · 2020-09-16T00:22:06.642Z

Very cool, great work! MoveIt Servo also has a better ring to it than Jog Arm I think

Question: Since this seems to be using the inverse Jacobian, I can see this getting stuck in a few scenarios, or unstable in cases where you need lots of tuning to get it right (at least in my experience with this). Think of obstacles, singularities, cuspidal robots, joint limits. Is there anything about the implementation of the core (robotics related) algorithm that’s different from the ROS1 version? Did you test this on (m)any real robots?

AdamPettinger · 2020-09-16T21:14:02.397Z

Yes, it is an inverse Jacobian method. The core algorithm is the same across ROS 1 and ROS 2.

It definitely does get stuck at times. Mostly with kinematic singularities. Servo handles these in a way that is safe but can be frustrating (slows down as the condition number increases, eventually coming to a stop). With obstacles, the behavior is fairly intuitive and the tuning parameter for it is just a threshold distance. Joint limits can cause problems as well (I’ve noticed this with UR5’s mostly).

The ROS 2 version was not tested on a real robot (but we have run the ROS 1 version extensively on multiple robots, HEBI, UR5). I am thinking about trying it out with ROS 2 on that same HEBI arm (because it would require little/no other ROS 2 parts), but I haven’t done it yet.

Fermi · 2020-09-18T19:37:26.990Z

First of all Thank You for MoveIt Servo, really cool.
Would there be a release for Ros2 Eloquent? some of us are using Ubuntu 18.04 and Foxy is for newer Ubuntu release 20.
Or can we build it ourselves for Eloquent.

I am noticing most of the packages are coming for Foxy but some of us have 18.04 and cannot install Foxy.
Thanks a lot

AdamPettinger · 2020-09-21T00:58:31.060Z

I don’t think there are plans to release it in Eloquent, as the main branch of moveit 2 is targeting Foxy.