> For the complete documentation index, see [llms.txt](https://docs.hello-robot.com/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://docs.hello-robot.com/stretch-4-quick-start-guide/writing-code-for-stretch.md).

# 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:

```bash
stretch_body_server --status
```

If it is not running, start it:

```bash
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:

```bash
stretch_body_server --free_up_control
```

**3. Confirm the system is healthy.**

```bash
stretch_system_check
```

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

```bash
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:

```bash
ipython3
```

Import and start the client:

```python
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:

```python
robot.stow()
```

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

Check if the robot is homed:

```python
robot.is_homed()
```

When you are done with a session, always call:

```python
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:

```python
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:

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

Move the arm by a relative amount:

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

Move the arm back:

```python
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:

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

Move the lift up a little:

```python
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:

```python
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):

```python
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):

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

Move sideways to the left (positive Y):

```python
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):

```python
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:

```python
# 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:

```python
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:

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

Move wrist pitch:

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

Move wrist roll by a relative amount:

```python
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):

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

Close the gripper (negative values = closed):

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

Move to neutral (zero):

```python
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):

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

Re-enabling:

```python
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:

```python
robot.routines.routine_wrist_joint_home('wrist_yaw')
```

Or home the entire end of arm at once:

```python
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:

```python
robot.pretty_print()
```

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

```python
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:**

```python
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:**

```python
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):**

```python
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):**

```python
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

```python
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:

```python
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:

```python
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:

```python
# 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:

```python
# 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

```python
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):

```python
# 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

```python
robot.power_periph.trigger_runstop()
robot.push_command()

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

#### Control the Fan

```python
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`:

```python
#!/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:

```bash
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:

```bash
stretch_feetech_reboot
```

After rebooting, re-home the wrist and gripper:

```bash
stretch_dex_wrist_home
stretch_gripper_home
```

You can also check Feetech motor health at any time:

```bash
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**](/stretch-4-quick-start-guide/robot-overview.md) — complete reference for all hardware, sensors, and CLI tools on Stretch SE4
* [**Stretch Safety Guide**](https://docs.hello-robot.com/stretch-4-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.


---

# Agent Instructions
This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com.

## Querying This Documentation
If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter, and the optional `goal` query parameter:

```
GET https://docs.hello-robot.com/stretch-4-quick-start-guide/writing-code-for-stretch.md?ask=<question>&goal=<endgoal>
```

`ask` is the immediate question: it should be specific, self-contained, and written in natural language.
`goal` is optional and describes the broader end goal you are ultimately trying to accomplish on behalf of the user. GitBook uses it to tailor the answer towards what is most useful for that goal.

The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.