Pick Object Demo

This walkthrough covers the pick_object_example behavior tree, which is the most complete demo in DelibFlow. It combines navigation, visual detection via ArUco markers, inverse kinematics planning, and actual arm execution to pick up an object.

Before you start

Make sure the full stack is running with the world world (the full home environment), since the behavior tree navigates to the Bedroom waypoint:

ros2 launch tiago_demos tiago_bringup.launch.py world:=world

You will also need an ArUco marker placed somewhere TiAGo can see from the Bedroom location. Check out ArUco Markers for details on the marker setup.

Running the demo

ros2 launch tiago_demos tiago_demo.launch.py tree:=pick_object_example

What happens step by step

Once launched, the behavior tree runs through this sequence:

  1. Setup - Initializes the action controller names for the arm, head, gripper, and torso. Also loads the pre-defined joint trajectories from the tiago_grab package.

  2. Arm ready - Moves the arm to a safe tucked position before navigating.

  3. Navigate to Bedroom - Uses Nav2 to drive to the Bedroom waypoint defined in map_locations.yaml.

  4. Head down - Points the head downward so the camera can see objects on a surface.

  5. Gripper open - Opens the gripper in preparation for grasping.

  6. ArUco detection - Reads the ArUco marker transform and computes the grasp target pose, with a 0.32m approach offset and a 0.05m pre-grasp offset.

  7. Pre-grasp motion - Plans and executes an IK trajectory to move the arm just above the object.

  8. Grasp motion - Plans and executes the final downward motion to the object.

  9. Gripper close - Closes the gripper to secure the object.

  10. Recover - Raises the torso and moves the arm back to the ready position with the object in hand.

The whole tree is wrapped in a RetryUntilSuccessful node, so if anything fails (for example, the marker is not detected yet) it will keep trying.

Monitoring with Groot2

The BT executor publishes real-time tree state on port 5555. If you have Groot2 installed, connect it to localhost:5555 to watch the nodes tick live.

Tweaking the demo

The predefined locations are in tiago_demos/config/map_locations.yaml. The Bedroom entry looks like this:

Bedroom:
  frame_id: map
  pose:
    x: 6.0
    y: -2.57
    z: 0.0
    yaw_deg: 135.0

Edit the coordinates to match your world setup, then relaunch the demo. No rebuild needed since the config is installed with symlinks.

Launch arguments reference

Argument

Default

Description

tree

pick_object_example

Name of the XML behavior tree file in tiago_demos/trees/

locations

map_locations.yaml

YAML file with named waypoints to load into the BT blackboard

use_sim_time

true

Set to false when running on a real robot

log_level

info

Logging verbosity (debug, info, warn, error)