Writing Code for Stretch

This tutorial introduces the two primary ways to develop software with Stretch SE4 — Python and ROS 2 — and walks you through writing your first Python programs using the Stretch 4 Body API. By the end, you will be able to command every joint on the robot, read sensor status, control the omnibase holonomically, and use the client/server architecture.

---

Background

Stretch 4 supports two approaches to software development:

Python (Stretch 4 Body) is the low-level direct interface to the robot hardware. It gives you fine-grained control over every joint and sensor, and is the fastest way to get started. The stretch4_body package is pre-installed on every Stretch SE4.

ROS 2 (Robot Operating System 2) is a robotics middleware framework providing a collection of tools, libraries, and conventions for building robot applications. Stretch SE4 ships with ROS 2 Jazzy and a full driver stack. It is well suited for navigation, SLAM, MoveIt, and multi-node architectures.

You can learn more about when to use each approach in the Developing with Stretch guide. This tutorial focuses on the Python API. ROS 2 examples are covered in the Demos section.

---

Software Architecture

All application code on Stretch SE4 uses the RobotClient class to communicate with a background stretch_body_server process running a 100 Hz control loop on the NUC.

The 100 Hz control loop follows this sequence every tick:

1. Pulls status from all hardware devices
2. Updates safety sentries (watchdogs)
3. Ingests commands from the Robot Client
4. Runs active controllers and behaviors
5. Computes safe motion limits
6. Pushes safe commands to motor controllers

Your code connects via RobotClient, which queues commands that the server loop ingests on the next tick. This means motion is asynchronous by default — your code keeps running while the robot moves. Use push_command() to flush the command queue and wait_on_motion_finish() to block until motion is done.

---

Prerequisites

Before running any code, the robot must be set up and ready.

1. Make sure the body server is running.

The body server is launched automatically at startup. Verify it is active:

stretch_body_server --status

If it is not running, start it:

stretch_body_server --daemon

2. Free up robot control from the gamepad demo.

If the Xbox gamepad teleoperation demo is running at startup, your code cannot control the robot. Free the robot:

stretch_body_server --free_up_control

3. Confirm the system is healthy.

stretch_system_check

4. Home the robot (required once per power cycle).

stretch_robot_home

The robot will beep when homing is complete. Joints will not accept motion commands until they are homed. Once homed, the motors remember their homed state as long as the robot remains powered on.

---

Your First Program with RobotClient

Open a terminal and launch iPython, an interactive Python console where each line runs immediately:

ipython3

Import and start the client:

from stretch4_body.robot.robot_client import RobotClient

robot = RobotClient()
robot.startup()

startup() connects to the running body server. If successful, you now have full access to all robot subsystems.

Stow the robot to its compact travel pose:

robot.stow()

This is a blocking call — it returns only when the robot is fully stowed.

Check if the robot is homed:

robot.is_homed()

When you are done with a session, always call:

robot.stop()

This cleanly disconnects from the server. Skipping stop() can leave stale connections that block other processes from controlling the robot.

Using RobotClient as a Context Manager

For scripts, the cleanest pattern is to use RobotClient as a context manager. This ensures stop() is always called, even if an exception occurs:

from stretch4_body.robot.robot_client import RobotClient

with RobotClient() as robot:
    robot.stow()
    # ... your code ...
# robot.stop() is called automatically here

---

Moving Individual Joints

Arm

The arm telescopes horizontally, extending up to ~0.52 m (21.6 inches) beyond the base. Position is in meters (0.0 = fully retracted, ~0.52 = fully extended).

Move the arm to an absolute position:

robot.arm.move_to(0.25)
robot.push_command()

Move the arm by a relative amount:

robot.arm.move_by(0.05)
robot.push_command()

Move the arm back:

robot.arm.move_by(-0.05)
robot.push_command()

Lift

The lift translates the arm vertically, from floor level up to ~1.20 m (47 inches). Position is in meters.

Move the lift to mid height:

robot.lift.move_to(0.6)
robot.push_command()

Move the lift up a little:

robot.lift.move_by(0.1)
robot.push_command()

Commanding Multiple Joints Simultaneously

You can queue commands to several joints before pushing. They will execute simultaneously:

robot.lift.move_to(0.7)
robot.arm.move_to(0.3)
robot.push_command()

[!NOTE] If you queue two motion commands for the same joint before pushing, only the last one executes. Earlier commands are overwritten.

Velocity and Acceleration Limits

Every motion command accepts optional velocity and acceleration limits (in m/s and m/s² respectively):

robot.arm.move_to(0.2, v_m=0.05, a_m=0.1)
robot.push_command()

If not specified, the robot uses its default motion profile.

---

The Omnibase (Holonomic Base)

Stretch SE4 has a triangular holonomic omnibase — three holonomic closed-loop stepper motors each driving one omnidirectional wheel. Unlike a differential drive, the omnibase can move forward, sideways, diagonally, and rotate in place — all simultaneously.

Wheel numbering increases counter-clockwise, with wheel 0 to the left of the forward direction. This is consistent with the ROS convention where X+ is forward and Y+ is left.

The base is accessed as robot.base or robot.omnibase.

Translate by a Relative Amount

Move the base 0.3 m forward (X):

robot.base.translate_by(x_m=0.3, y_m=0.0)
robot.push_command()

Move sideways to the left (positive Y):

robot.base.translate_by(x_m=0.0, y_m=0.15)
robot.push_command()

Rotate in Place

Rotate counter-clockwise by 90 degrees (π/2 radians):

import math
robot.base.rotate_by(w_r=math.pi / 2)
robot.push_command()

Set Continuous Velocity

Set a continuous velocity (useful for teleoperation or reactive control). Units: m/s for linear, rad/s for rotation:

# Drive forward at 0.2 m/s while rotating at 0.1 rad/s
robot.base.set_velocity(vx_m=0.2, vy_m=0.0, w_r=0.1)
robot.push_command()

# Stop
robot.base.set_velocity(vx_m=0.0, vy_m=0.0, w_r=0.0)
robot.push_command()

Hard Stop

Immediately stop all base motion:

robot.base.hard_stop()
robot.push_command()

---

The Dexterous Wrist and Gripper

Stretch SE4 has a 3-DOF dexterous wrist (yaw, pitch, roll) driven by Feetech servo motors over a TTL serial bus, plus a compliant spring-mechanism gripper.

All wrist and gripper joints are accessed through robot.end_of_arm. Positions are in radians unless noted.

Joint	Range
wrist_yaw	±170 deg (340 deg total)
wrist_pitch	~100 deg
wrist_roll	±170 deg (340 deg total)
stretch_gripper	-100 to +100 (percent, where positive = open)

Move Wrist Joints

Move wrist yaw to 0.5 rad:

robot.end_of_arm.move_to('wrist_yaw', 0.5)
robot.push_command()

Move wrist pitch:

robot.end_of_arm.move_to('wrist_pitch', -0.5)
robot.push_command()

Move wrist roll by a relative amount:

robot.end_of_arm.move_by('wrist_roll', 1.0)
robot.push_command()

robot.end_of_arm.move_by('wrist_roll', -1.0)
robot.push_command()

Gripper

Open the gripper (positive values = open):

robot.end_of_arm.move_to('stretch_gripper', 100)
robot.push_command()

Close the gripper (negative values = closed):

robot.end_of_arm.move_to('stretch_gripper', -100)
robot.push_command()

Move to neutral (zero):

robot.end_of_arm.move_to('stretch_gripper', 0)
robot.push_command()

Disable/Enable Torque on a Wrist Joint

Making a joint backdrivable (torque off):

robot.end_of_arm.disable_torque('wrist_yaw')
robot.push_command()

Re-enabling:

robot.end_of_arm.enable_torque('wrist_yaw')
robot.push_command()

Homing the Wrist

If a wrist joint loses its homed state (e.g., after a Feetech motor error and reboot), home it:

robot.routines.routine_wrist_joint_home('wrist_yaw')

Or home the entire end of arm at once:

robot.routines.routine_end_of_arm_home()

[!NOTE] After a Feetech error: If a wrist or gripper motor enters an error state (LED blinks red, motor becomes limp), clear it with stretch_feetech_reboot from a terminal, then re-home the wrist.

---

Reading Robot Status

Print Full Robot Status

Print all subsystem statuses in a human-readable format:

robot.pretty_print()

This outputs a lot of data. To read individual subsystem statuses, access robot.status directly:

print(robot.status['lift'])
print(robot.status['arm'])
print(robot.status['omnibase'])
print(robot.status['power_periph'])
print(robot.status['end_of_arm'])

Key Status Fields

Lift/Arm:

robot.status['lift']['pos']                     # Current position (m)
robot.status['lift']['vel']                     # Current velocity (m/s)
robot.status['arm']['pos']                      # Current position (m)
robot.status['arm']['motor']['pos_calibrated']  # True if homed

Omnibase:

robot.status['omnibase']['x']      # X position estimate (m)
robot.status['omnibase']['y']      # Y position estimate (m)
robot.status['omnibase']['theta']  # Heading (rad)
robot.status['omnibase']['x_vel']  # X velocity (m/s)
robot.status['omnibase']['y_vel']  # Y velocity (m/s)

Power Periph (IMU, battery, runstop):

robot.status['power_periph']['voltage_cpu']       # NUC supply voltage (V)
robot.status['power_periph']['current_cpu']       # NUC current (A)
robot.status['power_periph']['battery_soc']       # Battery state of charge (%)
robot.status['power_periph']['runstop_event']     # True if runstop active
robot.status['power_periph']['imu']['ax']         # IMU accelerometer X (m/s²)
robot.status['power_periph']['imu']['gravity_tilt']  # Gravity tilt angle (rad)
robot.status['power_periph']['over_tilt_type']    # Tilt direction if tilted (e.g. 'Left Tilt')

Wrist Joint (via end_of_arm):

robot.status['end_of_arm']['wrist_yaw']['pos']             # Position (rad)
robot.status['end_of_arm']['wrist_yaw']['effort']          # Effort (%)
robot.status['end_of_arm']['wrist_yaw']['temp']            # Motor temperature
robot.status['end_of_arm']['wrist_yaw']['overtemp_error']  # Overtemp flag
robot.status['end_of_arm']['wrist_yaw']['pos_calibrated']  # True if homed

---

Waiting for Motion to Complete

Motion commands are asynchronous. Use these methods to synchronize:

Wait for Specific Subsystems to Finish Moving

robot.arm.move_to(0.4)
robot.push_command()
robot.wait_on_motion_finish(['arm'], timeout=10.0)
print('Arm done moving')

Wait for multiple joints at once:

robot.lift.move_to(0.8)
robot.arm.move_to(0.2)
robot.push_command()
robot.wait_on_motion_finish(['lift', 'arm'], timeout=15.0)

Wait for All Motion to Complete

To wait for the arm, lift, base, and end of arm to all finish moving:

robot.arm.move_to(0.3)
robot.push_command()
robot.wait_command(timeout=10.0)

---

Guarded Contact Sensitivity

Stretch SE4's lift, arm, and omnibase joints have a Guarded Contact safety system. This uses current sensing to detect when actuator effort exceeds a configurable threshold. When triggered, the joint halts until a new command is received.

This protects people and objects from excessive force. By default, contacts are set to a moderate sensitivity. You can change this per use case:

# See available modes
print(robot.get_guarded_contact_modes())

# Set a robot-wide mode
robot.set_guarded_contact_sensitivity('high_sensitivity_manipulation')

# Or set per-joint
robot.arm.set_guarded_contact_sensitivity('high_sensitivity_manipulation')
robot.base.set_guarded_contact_sensitivity('default')

You can also pass per-move contact thresholds directly:

# Move arm with high contact sensitivity in the positive direction
robot.arm.move_to(0.3, contact_sensitivity_pos=0.9)
robot.push_command()

---

Power Periph: Beeps, Eyes, and Fan

The Power Periph board controls the speaker buzzer, LED eye animations, RGB lightbar, and chassis fans.

Trigger a Beep

robot.power_periph.trigger_beep()
robot.push_command()

Control LED Eyes

Set animations on the left and right eye rings. Animation indices are defined in the firmware (0 = off, see stretch_eye_animations for the full set):

# Set left eye animation index 3, right eye animation index 3
robot.power_periph.set_eye_animation(left_idx=3, right_idx=3)
robot.push_command()

Toggle the Runstop Programmatically

robot.power_periph.trigger_runstop()
robot.push_command()

# Later, clear it
robot.power_periph.clear_runstop()
robot.push_command()

Control the Fan

robot.power_periph.set_fan_on()
robot.push_command()

robot.power_periph.set_fan_off()
robot.push_command()

---

Writing a Complete Script

Here is a complete standalone Python script that demonstrates a sequence of moves on Stretch SE4. Save this as my_first_stretch_script.py:

#!/usr/bin/env python3
"""
Simple Stretch SE4 demo script.
Run with: python3 my_first_stretch_script.py

Prerequisites:
  - stretch_body_server must be running
  - Robot must be homed
"""

import time
import math
from stretch4_body.robot.robot_client import RobotClient

def main():
    with RobotClient() as robot:

        if not robot.is_homed():
            print('Robot is not homed. Homing now...')
            robot.home()

        print('Stowing robot...')
        robot.stow()

        # --- Lift ---
        print('Raising lift to 0.6 m...')
        robot.lift.move_to(0.6)
        robot.push_command()
        robot.wait_on_motion_finish(['lift'], timeout=15.0)

        # --- Arm ---
        print('Extending arm to 0.3 m...')
        robot.arm.move_to(0.3)
        robot.push_command()
        robot.wait_on_motion_finish(['arm'], timeout=10.0)

        # --- Simultaneous move ---
        print('Moving lift and arm together...')
        robot.lift.move_to(0.8)
        robot.arm.move_by(0.1)
        robot.push_command()
        robot.wait_on_motion_finish(['lift', 'arm'], timeout=15.0)

        # --- Wrist ---
        print('Moving wrist yaw...')
        robot.end_of_arm.move_to('wrist_yaw', 0.5)
        robot.push_command()

        print('Opening gripper...')
        robot.end_of_arm.move_to('stretch_gripper', 100)
        robot.push_command()
        time.sleep(1.0)

        print('Closing gripper...')
        robot.end_of_arm.move_to('stretch_gripper', -50)
        robot.push_command()
        time.sleep(1.0)

        # --- Base ---
        print('Translating base forward 0.2 m...')
        robot.base.translate_by(x_m=0.2, y_m=0.0)
        robot.push_command()
        robot.wait_on_motion_finish(['omnibase'], timeout=10.0)

        print('Rotating base 90 degrees...')
        robot.base.rotate_by(w_r=math.pi / 2)
        robot.push_command()
        robot.wait_on_motion_finish(['omnibase'], timeout=10.0)

        # --- Beep to signal completion ---
        robot.power_periph.trigger_beep()
        robot.push_command()

        print('Demo complete. Stowing...')
        robot.stow()

if __name__ == '__main__':
    main()

Run it with:

python3 my_first_stretch_script.py

---

Feetech Motor Errors

Wrist and gripper Feetech motors can enter an error state after over-force or over-temperature events. When this happens:

• The motor becomes limp and backdrivable
• The LED on the motor body blinks red
• Motion commands are ignored

Clear the error without powering down:

stretch_feetech_reboot

After rebooting, re-home the wrist and gripper:

stretch_dex_wrist_home
stretch_gripper_home

You can also check Feetech motor health at any time:

stretch_feetech_monitor

---

ROS 2 with Stretch SE4

Stretch SE4 ships with ROS 2 Jazzy. The ROS 2 driver communicates with stretch_body_server via the same RobotClient interface, making Python and ROS 2 development fully interoperable.

Key ROS 2 topics published by the driver include:

• /joint_states — positions and velocities for all joints
• /wheel_odom — odometry from the omnibase
• /scan_filtered — point clouds from the Hesai QT128 lidars
• /color/image_raw — RGB images from the head cameras

Key ROS 2 services include:

• /home — trigger full robot homing
• /stow — stow the robot

The Jetson add-on (when present) subscribes to camera topics and returns inference results via Zenoh topic bridging. This enables GPU-accelerated perception (e.g., YOLO, pose estimation) without burdening the NUC.

For full ROS 2 examples, refer to the ROS 2 with Stretch tutorial track on Stretch Docs.

---

Learn More

• Robot Overview — complete reference for all hardware, sensors, and CLI tools on Stretch SE4
• Stretch Safety Guide — essential safety information before operating the robot

---

Next Steps

In the next tutorial, Demo #1 - Mapping & Navigation, we will look at two ways that Stretch can navigate within a map.