By integrating the RapidCode API and the RMP with ROS 2 control, we commanded a Huayan Robotics Elfin5 collaborative robot (6-axis, 5 kg payload) to draw. These drawings came in two forms:
- Cartesian recipes
- Live drawings (with a non-drawing “follow” mode)
The Cartesian recipes were generated by converting SVG files from Wikimedia Commons into lines, arcs, and curves consisting of points sampled at a configurable interval. The live drawings were created on the fly by having the Elfin5 robot mimic the user’s strokes on a drawing plane (drawn with a mouse cursor or drawing tablet). While creating live drawings, users may also command the robot to “follow” the mouse cursor. While following, the robot takes the shortest path towards the current position of the cursor, which indicates a pose in Cartesian space, to allow users to easily reposition the arm without making unwanted strokes.
To accomplish this, we implemented two major components in the ROS 2 control node:
- The passthrough trajectory controller
- The RapidCode System Hardware
The code for the RMP and ROS 2 integration can be found at the GitHub repository roboticsys/rmp-ros2_control.
The Passthrough Trajectory Controller
The passthrough trajectory controller is intended to function similarly to the stock ROS 2 Joint Trajectory Controller (JTC) and have a similar interface for commanding motion. It accepts trajectory goals via the action ~/follow_joint_trajectory, and can take in continuous streams of joint trajectory messages such as those provided by MoveIt Servo via the topic ~/joint_trajectory. However, unlike the JTC, which only sends a single point sampled from its stored trajectories every control cycle, this passthrough trajectory controller samples the trajectories at the RMP firmware’s sample rate, then sends “chunks” of these sampled trajectories to the RapidCode System Hardware. The passthrough trajectory controller also provides some additional services such as ~/stop, which halts the robot’s motion and discards any queued goals, and the ~/reset_fault service to clear any latched faults on the RapidCode side.
The passthrough trajectory controller does not perform any kinematics. It consumes joint trajectories, typically from stock MoveIt components. The /compute_cartesian_path service solves the inverse kinematics at each point within a commanded stroke. Pilz plans trajectories in joint space to move the robot to the ready position. MoveIt Servo supplies the live joint stream for the “follow” mode.
The RapidCode System Hardware
The RapidCode System Hardware uses the RapidCode API to configure, command motion on, and read state from the robot’s drives. It handles the initialization of the RMP firmware while also configuring and maintaining ownership of the RapidCode objects. The RMP firmware is the process that cyclically exchanges EtherCAT packets containing the command and state information from the physical drives. The RapidCode System Hardware receives its commands from the passthrough trajectory controller in the form of trajectory points, containing times, positions, and velocities, which are then fed to the RapidCode streaming call MovePVT(). Users can observe the progress of the streamed motion and which trajectory chunk is currently being executed, as well as the state of the RMP itself, using the RapidCode System Hardware’s state interface. If users want to test motion without the physical robot, they may also use RapidCode phantom axes to command a phantom robot. The reporting from the RapidCode System Hardware’s state interfaces remains the same, so visualization tools like RViz can still display the pose of the robot accurately.
Why not use the stock Joint Trajectory Controller?
It is possible to use the stock JTC directly with the RapidCode System Hardware, but we found it more error-prone as the JTC’s default real-time configuration was not as stringent as the RMP’s, which led to either choppy motion or an “out of frames” error that stops the robot entirely. Running the JTC while matching its cycle rate to the RMP’s sample rate and sending a single point to the RapidCode API’s MovePVT() call every cycle left the system at the mercy of the ROS 2 Controller Manager’s jitter, and also was simply a case of unnecessarily overlapping responsibilities. The RMP can already queue up the motion frames given to it via MovePVT() and send the correct positions to the drives every sample, so there was no need for the controller to withhold the trajectory points until immediately before the required time. When in “follow” mode, the passthrough trajectory controller commands only a limited number of motion frames though to allow users to cancel their motion and potentially alter course.
Bridging the User Interface to MoveIt
In addition to the two core components for commanding motion with RapidCode and ROS 2, we added some supporting Python scripts to orchestrate the transfer of the Cartesian recipes and live drawings to MoveIt and then to the passthrough trajectory controller. Users will interact mostly with the surface client Python program, which can run on a system without a ROS 2 installation, where they can construct recipes, load existing ones, or perform the live drawings. This surface client connects to a communication bridge (running on a system with ROS 2) where it sends all of the user’s commanded robot poses. The communication bridge then dispatches the robot poses to an appropriate MoveIt component based on the type of motion. Most commonly, paths consisting of goal Cartesian poses from a recipe or from a live drawing are split into short, contiguous segments. Each segment is fed to MoveIt’s /compute_cartesian_path for inverse kinematics solving. The outputted joint trajectories then have their times and velocities adjusted to maintain continuity before being sent to the passthrough trajectory controller. The broken-up trajectories allow for planning upcoming motion in parallel while the already planned motion is executing.