Build a Simple Visual Servoing Objective

Tutorial Level: Advanced

In this tutorial you will learn how to make an objective using the Web app to track an AprilTag fiducial with a robot’s end effector.

Prerequisites for completing this tutorial:

1. Familiarity building and running objectives through the MoveIt Studio user interface, see the Tutorials.

2. A calibrated camera publishing synchronized CameraInfo and Image to ROS 2 topics. The camera frame should be included in the robot’s URDF for retrieving frame transforms from TF2.

3. A configuration for Cartesian endpoint jogging of your arm using MoveIt Servo. This is verifiable through the endpoing tab in the MoveIt Studio user interface.

4. Access to an AprilTag Fiducial Marker. We generally recommend markers from 36h11 to start, though the detection node will work with any tag from the standard library.

1. Overview

Visual servoing is a technique for using camera feedback to control the motion of a robot. This tutorial provides steps for building an Objective that identifies the pose of an AprilTag in a camera feed, determines the desired grasp point relative to the detected tag, filters the results, then computes and publishes Cartesian twists to drive the tool towards the goal pose.

The default configuration assumes a wrist mounted camera and AprilTag to do localization of a grasp point. There are four frames that are relevant to understanding this configuration,

1. F The camera frame. Used for localization of the AprilTag.

2. T The tool frame, in the case of MoveIt Studio we generally refer to this as the manual_grasp_link.

3. A The detected AprilTag frame. Determined by image feedback and from F.

4. G The goal frame, or the desired pose for the end effector. For now we will hard-code this as a transform relative A.

2. Adding Required Behaviors

There are a handful of Behaviors specific to this objective that are not included in MoveIt Studio’s core Behavior loader.

To ensure MoveIt Studio has access to the required Behaviors, modify the robot’s site configuration ensure the following are in the list behavior_loader_plugins:

behavior_loader_plugins:
  # Plugin for core behaviors provided with MoveIt Studio
  core:
    - "moveit_studio::behaviors::CoreBehaviorsLoader"
  # Plugin for prototyped Behaviors to run Visual Servoing objectives
  visual_servo_behaviors:
    - "moveit_visual_servo::behaviors::VisualServoBehaviorLoader"

Also add the Behaviors to the tree_nodes_model.xml file,

<!-- This Behavior is included in the core Behaviors plugin and may already be present in your tree_nodes_model.xml -->
<Action ID="GetApriltagPose">
    <metadata subcategory="Perception"/>
    <description>
        <p>
            Provides functions for realtime tracking of AprilTag in published image topics.
        </p>
        <p>
            Parameters for the tag detector are configured through YAML files.
            Raw images are processed from the specified camera stream topic, and detections are published as a vector of TransformStamped to the specified output topic.
        </p>
        <p>
            NOTE: This Behavior will never terminate on its own. You must wrap it in a sequence that will handle halting the node.
        </p>
    </description>
    <input_port name="parameters" default="{parameters}">Parameters for the AprilTag bt node.</input_port>
    <input_port name="camera_stream_topic" default="/wrist_mounted_camera/color/image_raw">Target camera stream topic.</input_port>
    <input_port name="detection_transform_stamped_topic" default="/apriltag_detections">Output transforms topic.</input_port>
</Action>

<!-- These two Behaviors have been added to support Visual Servoing, requires the visual servoing plugin! -->
<Action ID="VisualServo">
    <description>
        <p>
            PROTOTYPE ONLY. USE WITH CAUTION.
        </p>
        <p>
            Sample Behavior for servoing towards a streaming pose.
            Ingests a streaming goal pose, and implements a basic proportional controller around MoveIt Servo to jog the end effector towards the pose.
            If the goal pose is moving with constant velocity, can optionally include a velocity feedforward term to the controller to drive steady state error to 0.

        </p>
    </description>
    <input_port name="parameters" default="{parameters}">Parameters for the visual servo bt node.</input_port>
    <input_port name="controller_name" default="streaming_controller">Name of the controller activate.</input_port>
    <input_port name="goal_pose_topic" default="/visual_servo_goal_pose_filtered">The streaming goal pose topic for the end effector.</input_port>
    <input_port name="pose_error_topic" default="/visual_servo_pose_error">The error between the gripper and the goal pose will be published to this topic.</input_port>
</Action>
<Action ID="PoseStreamFilter">
    <description>
        <p>
            Ingests a PoseStamped message, applies a filter, and publishes the result.
        </p>
    </description>
    <input_port name="input_pose_topic" default="/visual_servo_goal_pose">PoseStamped topic to filter.</input_port>
    <input_port name="output_pose_topic" default="/visual_servo_goal_pose_filtered">PoseStamped topic to publish filtered data to.</input_port>
    <input_port name="filter_coefficient" default="0.1">Filter coefficient for the smoothing filte.r</input_port>
</Action>
<Action ID="GraspPoseStreamer">
    <description>
        <p>
            PROTOTYPE ONLY. USE WITH CAUTION.
        </p>
        <p>
            Sample Behavior for use in visual servoing objectives.
            Ingests published AprilTag poses relative to the camera, and determines a fixed pose for grasping an object relative the detected tag.

            Once the measured error published by the servo node drops below the specified threshold, this Behavior will return success.
        </p>
        <p>
            This Behavior could be replaced by a more intelligent system that uses transforms or other image features to determine an ideal grasp pose on the fly.
        </p>
    </description>
    <input_port name="target_frame_id" default="">The frame id for the detected object.</input_port>
    <input_port name="detections_streaming_topic" default="/apriltag_detections">Object detections topic name, generally from the AprilTag.</input_port>
    <input_port name="goal_pose_streaming_topic" default="/visual_servo_goal_pose">Topic on which to publish the goal pose, generally subscribed to by the visual servo.</input_port>
    <input_port name="pose_error_topic" default="/visual_servo_pose_error">Pose error topic to subscribe to, generally published from visual servo.</input_port>
    <input_port name="goal_frame_id" default="base_link">Target frame to transform detections into.</input_port>
    <input_port name="goal_linear_threshold" default="0.03">Maximum error to say the visual servo has reached the linear goal with the gripper.</input_port>
    <input_port name="goal_angular_threshold" default="0.05">Maximum error to say the visual servo has reached the angular goal with the gripper.</input_port>
    <input_port name="goal_pose_translation_x" default="0">X offset  between the AprilTag and the grasp pose.</input_port>
    <input_port name="goal_pose_translation_y" default="0">Y offset between the AprilTag and the grasp pose.</input_port>
    <input_port name="goal_pose_translation_z" default="0.1">Z offset between the AprilTag and the grasp pose.</input_port>
    <input_port name="goal_pose_orientation_x" default="1.0">X-component of the quaternion describing the orientation of the grasp pose relative to the AprilTag frame.</input_port>
    <input_port name="goal_pose_orientation_y" default="0.0">Y-component of the quaternion describing the orientation of the grasp pose relative to the AprilTag frame.</input_port>
    <input_port name="goal_pose_orientation_z" default="0.0">Z-component of the quaternion describing the orientation of the grasp pose relative to the AprilTag frame.</input_port>
    <input_port name="goal_pose_orientation_w" default="0.0">W-component of the quaternion describing the orientation of the grasp pose relative to the AprilTag frame.</input_port>
</Action>

3. Build and Run the Objective

With the Behaviors now added to the site configuration, you are ready to build and configure the objective through the MoveIt Studio user interface! Launch MoveIt Studio and open the objective editor, and create a new objective:

The first Behavior added to the objective is to load the objectives parameters file. Add a LoadObjectivesParameters node, and set the config_file_name to apriltag_visual_servoing_params.yaml. Leave the parameters output port as {parameters}.

For more information, refer to Parameterizing Behaviors using Configuration Parameters.

Next, create the yaml file for the Behavior using default values. Using a text editor, copy the yaml below into your objectives directory, to $HOME/.config/moveit_studio/objectives/apriltag_visual_servoing_params.yaml.

# These are default for the AprilTag detection node. These can be overridden depending on your
# chosen tag's family and parameters. For more information, refer to
# https://github.com/christianrauch/apriltag_ros#configuration
GetApriltagPose:
  apriltag_family_name: 36h11
  # Note that more than one tag can be detected per image.
  apriltag_ids:
    - 0
  apriltag_sizes:
    - 0.028
  apriltag_names:
    - frame_id0
  max_hamming: 0
  nthreads: 1
  quad_decimate: 1
  quad_sigma: 0
  refine_edges: true
  z_up: true
# Parameters for the visual servo detection node.
VisualServo:
  # Values to clamp commanded angular and linear velocities
  max_angular_output: 1
  max_linear_output: 1
  # Linear and angular gains on the Cartesian error between the start and target pose
  # We start these really low and go up!
  proportional_gain_angular: 1
  proportional_gain_linear: 1
  # The rate to publish twist commands
  servo_spin_rate: 0.02
  # The desired tool frame to align with the goal pose, must match MoveIt Servo's tool frame.
  target_servo_frame: 'manual_grasp_link'
  # The fixed world frame to do velocity computations in
  fixed_servo_frame: 'base_link'
  # Whether or not to include a velocity feed-forward term.
  # In general you do not want these.
  include_ff: false
  ff_velocity_gain_angular: 0
  ff_velocity_gain_linear: 0

With that in place, return to the objective editor. Now, when selecting the LoadObjectiveParameters node, the yaml parameters from the file should be configurable in the sidebar (as seen in the image below). For now, configure the parameters under GetApriltagPose to match whatever tag you have selected for this tutorial. In particular you will need the apriltag_family_name, apriltag_ids, and apriltag_sizes. All the other parameters can remain with the default values.

With the parameters in place, use a Parallel node to execute all remaining nodes simultaneously. Add the GetApriltagPose, GraspPoseStreamer, PoseStreamFilter, and VisualServo nodes all as children of the Parallel node, in order. The structure of the tree should be the following:

Here is a run-down on the configurable parameters for each node:

• Parallel

  Refer to the node description for more information.

  All children are executed concurrently in the same thread. In this objective, if a single node returns success we wish to report success, as it will indicate that the end effector has arrived at the goal pose.

• GetApriltagPose

  Configuration for the detection logic in this node is done through YAML parameters.

  The camera_stream_topic parameter specifies the image topic that will be be subscribed to and processed by the AprilTag detector. The detection_transform_stamped_topic specifies the topic name to publish tag detections. This will be a list of stamped transforms from frame F to frame A, given in the camera’s frame F, as defined above.

• GraspPoseStreamer

  As mentioned previously, this node determines the target grasp pose, G, for the tool based on a given tag detection. As of now G is a hard-coded pose defined relative the AprilTag’s frame, A, given in frame, A. Many of the defaults here should be sufficient for a basic use case. The default goal pose is set to a 10cm offset from the AprilTag, but this can be modified by changing the goal_pose_translation_* and goal_pose_orientation_* values accordingly. The *_threshold parameters define the success threshold for the linear and angular distances between the current tool pose and the goal pose, or |G - T|.

• PoseStreamFilter

  Applies a simple smoothing filter to the specified incoming pose topic, and publishes to the specified output topic. The filter_coefficient is configurable, though in general some smoothing is recommended.

• VisualServo

  Activates and publishes twist commands to MoveIt Servo to drive the tool, T, to the goal pose, G. Most parameters for the servo node itself are configured in the objective’s YAML file, apriltag_visual_servoing_params.yaml. You must set the controller_name to the same controller used in your MoveIt Servo configuration (and so should match the teleoperate objective). The goal_pose_topic should match the output topic from the filter node. The pose_error_topic publishes the current measured error between the current and target poses for the tool tip, namely |G - T|. It is subscribed to by the GraspPoseStreamer and used to measure success.

To summarize the data flow between these nodes:

flowchart TD GetApriltagPose --apriltag transforms--> GraspPoseStreamer GraspPoseStreamer --target goal pose--> PoseStreamFilter PoseStreamFilter --filtered target goal pose--> VisualServo VisualServo --target pose error--> GraspPoseStreamer VisualServo --delta twists--> MoveItServo{{MoveIt Servo}}

The objective is now ready to be run! Fix (or hold) an AprilTag in the camera frame and watch the arm try to chase it.

Note

By default, the proportional gains in the Visual Servo node are small.

As you become more familiar with your MoveIt Servo configuration and the tunable parameters in this objective, we recommend bumping the proportional_gain_angular and proportional_gain_linear up to get faster response from the Behavior.

Note

Once you have your objective running with well-tuned parameters, remember to copy it into your site configuration to save it forever!

4. Building Out the Behavior Tree

With the basic objective in place, there are many ways to expand the functionality!

For this demonstration in the PickNik office, we configured the left arm to translate with the AprilTag and fixture, with the right arm running the Visual Servoing objective. We have set the grasp pose relative the detected tag so that the tool aligns with the handle.

This objective includes additional Behaviors for opening and closing the gripper around the target handle, as well as an additional GraspPoseStreamer node to support alignment with the target before “plunging” towards it. We also have built out error handling, and additional safety logic around the objective as fallbacks for controller or other errors.

The full Behavior tree for this setup is given below,

We hope you enjoy experimenting with this objective and Behaviors!