Getting Started Tutorial 
This notebook provides an introduction to Stretch's MuJoCo simulation. You can run this notebook with either CPU or GPU instance.
Install¶
In [ ]:
Copied!
# Set up Stretch Mujoco repo
%rm -rf ./stretch_mujoco/
# !git clone https://github.com/hello-robot/stretch_mujoco --recurse-submodules
!git clone https://github.com/hello-robot/stretch_mujoco --depth 1
%cd ./stretch_mujoco/
%pip install -e ".[jupyter]"
# Check if we can use GPU rendering
import os
import subprocess
try:
subprocess.run('nvidia-smi')
USE_GPU=True
except:
USE_GPU=False
# Setup rendering
if USE_GPU:
# Add an ICD config so that glvnd can pick up the Nvidia EGL driver.
# This is usually installed as part of an Nvidia driver package, but the Colab
# kernel doesn't install its driver via APT, and as a result the ICD is missing.
# (https://github.com/NVIDIA/libglvnd/blob/master/src/EGL/icd_enumeration.md)
NVIDIA_ICD_CONFIG_PATH = '/usr/share/glvnd/egl_vendor.d/10_nvidia.json'
if not os.path.exists(NVIDIA_ICD_CONFIG_PATH):
with open(NVIDIA_ICD_CONFIG_PATH, 'w') as f:
f.write("""{
"file_format_version" : "1.0.0",
"ICD" : {
"library_path" : "libEGL_nvidia.so.0"
}
}""")
# Configure MuJoCo to use the EGL rendering backend (requires GPU)
print('Setting environment variable to use GPU rendering:')
%env MUJOCO_GL=egl
else:
# Required for OSMesa OpenGL driver
!apt-get update
!apt-get install -y libosmesa6-dev libgl1-mesa-glx libglfw3
print('Setting environment variable to use CPU rendering:')
%env MUJOCO_GL=osmesa
# Other imports and helper functions
import time
import pprint
import itertools
import numpy as np
# Graphics and plotting.
!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)
!pip install -q mediapy
import mediapy as media
import matplotlib.pyplot as plt
# More legible printing from numpy.
np.set_printoptions(precision=3, suppress=True, linewidth=100)
# Set up Stretch Mujoco repo
%rm -rf ./stretch_mujoco/
# !git clone https://github.com/hello-robot/stretch_mujoco --recurse-submodules
!git clone https://github.com/hello-robot/stretch_mujoco --depth 1
%cd ./stretch_mujoco/
%pip install -e ".[jupyter]"
# Check if we can use GPU rendering
import os
import subprocess
try:
subprocess.run('nvidia-smi')
USE_GPU=True
except:
USE_GPU=False
# Setup rendering
if USE_GPU:
# Add an ICD config so that glvnd can pick up the Nvidia EGL driver.
# This is usually installed as part of an Nvidia driver package, but the Colab
# kernel doesn't install its driver via APT, and as a result the ICD is missing.
# (https://github.com/NVIDIA/libglvnd/blob/master/src/EGL/icd_enumeration.md)
NVIDIA_ICD_CONFIG_PATH = '/usr/share/glvnd/egl_vendor.d/10_nvidia.json'
if not os.path.exists(NVIDIA_ICD_CONFIG_PATH):
with open(NVIDIA_ICD_CONFIG_PATH, 'w') as f:
f.write("""{
"file_format_version" : "1.0.0",
"ICD" : {
"library_path" : "libEGL_nvidia.so.0"
}
}""")
# Configure MuJoCo to use the EGL rendering backend (requires GPU)
print('Setting environment variable to use GPU rendering:')
%env MUJOCO_GL=egl
else:
# Required for OSMesa OpenGL driver
!apt-get update
!apt-get install -y libosmesa6-dev libgl1-mesa-glx libglfw3
print('Setting environment variable to use CPU rendering:')
%env MUJOCO_GL=osmesa
# Other imports and helper functions
import time
import pprint
import itertools
import numpy as np
# Graphics and plotting.
!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)
!pip install -q mediapy
import mediapy as media
import matplotlib.pyplot as plt
# More legible printing from numpy.
np.set_printoptions(precision=3, suppress=True, linewidth=100)
Cloning into 'stretch_mujoco'...
remote: Enumerating objects: 1570, done.
remote: Counting objects: 100% (469/469), done.
remote: Compressing objects: 100% (187/187), done.
remote: Total 1570 (delta 350), reused 290 (delta 282), pack-reused 1101 (from 2)
Receiving objects: 100% (1570/1570), 29.74 MiB | 4.79 MiB/s, done.
Resolving deltas: 100% (937/937), done.
/content/stretch_mujoco
--2025-06-09 12:30:40-- https://gist.githubusercontent.com/hello-binit/69ef349e3b55a79bcb3697f41bc92dd7/raw/9fee746d602c9b47e0de624f4d05dd4206169d44/pyproject.toml
Resolving gist.githubusercontent.com (gist.githubusercontent.com)... 185.199.109.133, 185.199.110.133, 185.199.111.133, ...
Connecting to gist.githubusercontent.com (gist.githubusercontent.com)|185.199.109.133|:443... connected.
HTTP request sent, awaiting response... 200 OK
Length: 1231 (1.2K) [text/plain]
Saving to: ‘pyproject.toml’
pyproject.toml 100%[===================>] 1.20K --.-KB/s in 0s
2025-06-09 12:30:40 (78.3 MB/s) - ‘pyproject.toml’ saved [1231/1231]
/content
Obtaining file:///content/stretch_mujoco
Installing build dependencies ... done
Checking if build backend supports build_editable ... done
Getting requirements to build editable ... done
Preparing editable metadata (pyproject.toml) ... done
Requirement already satisfied: click>=8.1.8 in /usr/local/lib/python3.11/dist-packages (from stretch_mujoco==0.0.1) (8.2.1)
Collecting hello-robot-stretch-urdf>=0.1.0 (from stretch_mujoco==0.0.1)
Downloading hello_robot_stretch_urdf-0.1.0-py3-none-any.whl.metadata (1.2 kB)
Collecting inputs>=0.5 (from stretch_mujoco==0.0.1)
Downloading inputs-0.5-py2.py3-none-any.whl.metadata (1.9 kB)
Collecting matplotlib>=3.10.1 (from stretch_mujoco==0.0.1)
Downloading matplotlib-3.10.3-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl.metadata (11 kB)
Collecting mujoco==3.2.6 (from stretch_mujoco==0.0.1)
Downloading mujoco-3.2.6-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl.metadata (44 kB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 44.4/44.4 kB 4.6 MB/s eta 0:00:00
Requirement already satisfied: opencv-python in /usr/local/lib/python3.11/dist-packages (from stretch_mujoco==0.0.1) (4.11.0.86)
Collecting urchin>=0.0.29 (from stretch_mujoco==0.0.1)
Downloading urchin-0.0.29-py3-none-any.whl.metadata (1.4 kB)
Requirement already satisfied: absl-py in /usr/local/lib/python3.11/dist-packages (from mujoco==3.2.6->stretch_mujoco==0.0.1) (1.4.0)
Requirement already satisfied: etils[epath] in /usr/local/lib/python3.11/dist-packages (from mujoco==3.2.6->stretch_mujoco==0.0.1) (1.12.2)
Collecting glfw (from mujoco==3.2.6->stretch_mujoco==0.0.1)
Downloading glfw-2.9.0-py2.py27.py3.py30.py31.py32.py33.py34.py35.py36.py37.py38.p39.p310.p311.p312.p313-none-manylinux_2_28_x86_64.whl.metadata (5.4 kB)
Requirement already satisfied: numpy in /usr/local/lib/python3.11/dist-packages (from mujoco==3.2.6->stretch_mujoco==0.0.1) (2.0.2)
Requirement already satisfied: pyopengl in /usr/local/lib/python3.11/dist-packages (from mujoco==3.2.6->stretch_mujoco==0.0.1) (3.1.9)
Requirement already satisfied: contourpy>=1.0.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (1.3.2)
Requirement already satisfied: cycler>=0.10 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (0.12.1)
Requirement already satisfied: fonttools>=4.22.0 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (4.58.1)
Requirement already satisfied: kiwisolver>=1.3.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (1.4.8)
Requirement already satisfied: packaging>=20.0 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (24.2)
Requirement already satisfied: pillow>=8 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (11.2.1)
Requirement already satisfied: pyparsing>=2.3.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (3.2.3)
Requirement already satisfied: python-dateutil>=2.7 in /usr/local/lib/python3.11/dist-packages (from matplotlib>=3.10.1->stretch_mujoco==0.0.1) (2.9.0.post0)
Requirement already satisfied: lxml in /usr/local/lib/python3.11/dist-packages (from urchin>=0.0.29->stretch_mujoco==0.0.1) (5.4.0)
Requirement already satisfied: networkx in /usr/local/lib/python3.11/dist-packages (from urchin>=0.0.29->stretch_mujoco==0.0.1) (3.5)
Collecting pycollada>=0.6 (from urchin>=0.0.29->stretch_mujoco==0.0.1)
Downloading pycollada-0.9.tar.gz (109 kB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 109.7/109.7 kB 12.6 MB/s eta 0:00:00
Installing build dependencies ... done
Getting requirements to build wheel ... done
Preparing metadata (pyproject.toml) ... done
Collecting pyribbit>=0.1.46 (from urchin>=0.0.29->stretch_mujoco==0.0.1)
Downloading pyribbit-0.1.46-py3-none-any.whl.metadata (1.5 kB)
Requirement already satisfied: scipy in /usr/local/lib/python3.11/dist-packages (from urchin>=0.0.29->stretch_mujoco==0.0.1) (1.15.3)
Requirement already satisfied: six in /usr/local/lib/python3.11/dist-packages (from urchin>=0.0.29->stretch_mujoco==0.0.1) (1.17.0)
Collecting trimesh (from urchin>=0.0.29->stretch_mujoco==0.0.1)
Downloading trimesh-4.6.11-py3-none-any.whl.metadata (18 kB)
Collecting freetype-py (from pyribbit>=0.1.46->urchin>=0.0.29->stretch_mujoco==0.0.1)
Downloading freetype_py-2.5.1-py3-none-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl.metadata (6.3 kB)
Requirement already satisfied: imageio in /usr/local/lib/python3.11/dist-packages (from pyribbit>=0.1.46->urchin>=0.0.29->stretch_mujoco==0.0.1) (2.37.0)
Collecting pyglet>=1.4.10 (from pyribbit>=0.1.46->urchin>=0.0.29->stretch_mujoco==0.0.1)
Downloading pyglet-2.1.6-py3-none-any.whl.metadata (7.7 kB)
Requirement already satisfied: fsspec in /usr/local/lib/python3.11/dist-packages (from etils[epath]->mujoco==3.2.6->stretch_mujoco==0.0.1) (2025.3.2)
Requirement already satisfied: importlib_resources in /usr/local/lib/python3.11/dist-packages (from etils[epath]->mujoco==3.2.6->stretch_mujoco==0.0.1) (6.5.2)
Requirement already satisfied: typing_extensions in /usr/local/lib/python3.11/dist-packages (from etils[epath]->mujoco==3.2.6->stretch_mujoco==0.0.1) (4.14.0)
Requirement already satisfied: zipp in /usr/local/lib/python3.11/dist-packages (from etils[epath]->mujoco==3.2.6->stretch_mujoco==0.0.1) (3.22.0)
Downloading mujoco-3.2.6-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl (6.4 MB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 6.4/6.4 MB 20.2 MB/s eta 0:00:00
Downloading hello_robot_stretch_urdf-0.1.0-py3-none-any.whl (37.0 MB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 37.0/37.0 MB 5.5 MB/s eta 0:00:00
Downloading inputs-0.5-py2.py3-none-any.whl (33 kB)
Downloading matplotlib-3.10.3-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl (8.6 MB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 8.6/8.6 MB 109.2 MB/s eta 0:00:00
Downloading urchin-0.0.29-py3-none-any.whl (27 kB)
Downloading pyribbit-0.1.46-py3-none-any.whl (1.2 MB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 1.2/1.2 MB 57.3 MB/s eta 0:00:00
Downloading glfw-2.9.0-py2.py27.py3.py30.py31.py32.py33.py34.py35.py36.py37.py38.p39.p310.p311.p312.p313-none-manylinux_2_28_x86_64.whl (243 kB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 243.5/243.5 kB 26.6 MB/s eta 0:00:00
Downloading trimesh-4.6.11-py3-none-any.whl (711 kB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 711.7/711.7 kB 53.5 MB/s eta 0:00:00
Downloading pyglet-2.1.6-py3-none-any.whl (983 kB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 984.0/984.0 kB 64.2 MB/s eta 0:00:00
Downloading freetype_py-2.5.1-py3-none-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl (1.0 MB)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 1.0/1.0 MB 67.0 MB/s eta 0:00:00
Building wheels for collected packages: stretch_mujoco, pycollada
Building editable for stretch_mujoco (pyproject.toml) ... done
Created wheel for stretch_mujoco: filename=stretch_mujoco-0.0.1-0.editable-py3-none-any.whl size=8274 sha256=33cde7f329c283fd0c207218843520514150be264212ffa81d7157d9c280478f
Stored in directory: /tmp/pip-ephem-wheel-cache-eeyzgwtw/wheels/00/f2/77/510dfa2b2dfe46fd8f89cdd92447d0bb7a91113a8b1e296877
Building wheel for pycollada (pyproject.toml) ... done
Created wheel for pycollada: filename=pycollada-0.9-py3-none-any.whl size=128524 sha256=9d6f0e58c6916b04b39ad03f95e5f50b2d04be9b72c6b4b2168800b88d073936
Stored in directory: /root/.cache/pip/wheels/d8/f5/25/96914138081f519b3061d82b1306fccf17ea00e00a4b3f8fb2
Successfully built stretch_mujoco pycollada
Installing collected packages: inputs, glfw, trimesh, pyglet, freetype-py, pyribbit, pycollada, matplotlib, urchin, mujoco, hello-robot-stretch-urdf, stretch_mujoco
Attempting uninstall: matplotlib
Found existing installation: matplotlib 3.10.0
Uninstalling matplotlib-3.10.0:
Successfully uninstalled matplotlib-3.10.0
Successfully installed freetype-py-2.5.1 glfw-2.9.0 hello-robot-stretch-urdf-0.1.0 inputs-0.5 matplotlib-3.10.3 mujoco-3.2.6 pycollada-0.9 pyglet-2.1.6 pyribbit-0.1.46 stretch_mujoco-0.0.1 trimesh-4.6.11 urchin-0.0.29
/content/stretch_mujoco
Setting environment variable to use GPU rendering:
env: MUJOCO_GL=egl
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 1.6/1.6 MB 80.4 MB/s eta 0:00:00
Basics¶
In [2]:
Copied!
from stretch_mujoco import StretchMujocoSimulator
from stretch_mujoco import StretchMujocoSimulator
The StretchMujocoSimulator class is used to:
- Start/stop the simulation
- Read camera imagery
- Read lidar scans
- Read joint states
- Position control the robot's ranged joints
- Velocity control the robot's mobile base
In [3]:
Copied!
from stretch_mujoco.enums.stretch_sensors import StretchSensors
from stretch_mujoco.enums.stretch_cameras import StretchCameras
from stretch_mujoco.enums.actuators import Actuators
from stretch_mujoco.enums.stretch_sensors import StretchSensors
from stretch_mujoco.enums.stretch_cameras import StretchCameras
from stretch_mujoco.enums.actuators import Actuators
The enums module defines actuators and sensors that can be accessed through the simulator.
Starting the simulation¶
We create sim, and instance of the simulation class and launch it in the background using the start() method. Since we're in Google Colab, we'll run the simulation "headless", or without the interactive GUI. Headless is also the most performant mode, so it's used most often.
In [4]:
Copied!
sim = StretchMujocoSimulator(cameras_to_use=StretchCameras.all())
sim.start(headless=True)
sim = StretchMujocoSimulator(cameras_to_use=StretchCameras.all())
sim.start(headless=True)
Starting Stretch Mujoco Simulator... Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. Still waiting to connect to the Mujoco Simulatior. The Mujoco Simulatior is connected.
Wall time vs. sim time¶
The rate of time on your clock isn't the same rate of time passing in your simulation. The speed of simulation won't exceed realtime (by default), but can be slower due to hardware (e.g. if you're using a Colab CPU instance instead of a T4 GPU instance, you'll see slower simulation).
In [5]:
Copied!
wall_start = time.time()
sim_start = sim.pull_status().time
for _ in range(10):
time.sleep(0.5)
print(f"Sim is running {(sim.pull_status().time - sim_start)/(time.time() - wall_start):.3f}x as fast as realtime")
wall_start = time.time()
sim_start = sim.pull_status().time
wall_start = time.time()
sim_start = sim.pull_status().time
for _ in range(10):
time.sleep(0.5)
print(f"Sim is running {(sim.pull_status().time - sim_start)/(time.time() - wall_start):.3f}x as fast as realtime")
wall_start = time.time()
sim_start = sim.pull_status().time
Sim is running 0.067x as fast as realtime Sim is running 0.076x as fast as realtime Sim is running 0.104x as fast as realtime Sim is running 0.112x as fast as realtime Sim is running 0.111x as fast as realtime Sim is running 0.119x as fast as realtime Sim is running 0.120x as fast as realtime Sim is running 0.108x as fast as realtime Sim is running 0.108x as fast as realtime Sim is running 0.112x as fast as realtime
The simulation can run faster when there are fewer cameras to simulate. Above, we defined
sim = StretchMujocoSimulator(cameras_to_use=StretchCameras.all())
If we reduced to only the cameras we needed (e.g. cameras_to_use=[StretchCameras.cam_d435i_rgb]), or no cameras if they're not needed (e.g. cameras_to_use=[]), then you'd see a higher speed.
Imagery¶
MuJoCo renders simulated imagery from Stretch's two depth cameras and one wide-angle camera, for a total of 5 different images. The camera intrinsics are also provided.
- RGB and Depth images from Stretch's head Realsense D435if
- RGB and Depth images from Stretch's wrist Realsense D405
- RGB image from Stretch's head Arducam 1MP Wide-angle
In [6]:
Copied!
cams_current = sim.pull_camera_data()
pprint.pprint(cams_current)
cams_current = sim.pull_camera_data()
pprint.pprint(cams_current)
StatusStretchCameras(time=3.201999999999869,
fps=15.507845316206037,
cam_d405_rgb=array([[[169, 224, 255],
[169, 224, 255],
[169, 224, 255],
...,
[169, 224, 255],
[169, 224, 255],
[169, 224, 255]],
[[169, 224, 255],
[169, 224, 255],
[169, 224, 255],
...,
[169, 224, 255],
[169, 224, 255],
[169, 224, 255]],
[[169, 224, 255],
[169, 224, 255],
[169, 224, 255],
...,
[169, 224, 255],
[169, 224, 255],
[169, 224, 255]],
...,
[[160, 133, 100],
[159, 131, 100],
[157, 130, 97],
...,
[160, 135, 101],
[154, 130, 96],
[157, 132, 98]],
[[158, 131, 98],
[157, 130, 98],
[153, 127, 95],
...,
[165, 139, 106],
[160, 135, 102],
[155, 130, 98]],
[[157, 131, 98],
[155, 130, 97],
[154, 128, 97],
...,
[166, 139, 106],
[166, 140, 106],
[162, 136, 103]]], dtype=uint8),
cam_d405_depth=array([[0. , 0. , 0. , ..., 0. , 0. , 0. ],
[0. , 0. , 0. , ..., 0. , 0. , 0. ],
[0. , 0. , 0. , ..., 0. , 0. , 0. ],
...,
[0.445, 0.445, 0.445, ..., 0.449, 0.449, 0.449],
[0.444, 0.444, 0.444, ..., 0.448, 0.448, 0.448],
[0.442, 0.442, 0.442, ..., 0.447, 0.447, 0.447]], dtype=float32),
cam_d405_K=array([[514.682, 0. , 320. ],
[ 0. , 514.682, 240. ],
[ 0. , 0. , 1. ]]),
cam_d435i_rgb=array([[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
...,
[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[147, 216, 255],
[147, 216, 255],
[147, 216, 255],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]]], dtype=uint8),
cam_d435i_depth=array([[0. , 0. , 0. , ..., 1.649, 1.643, 1.638],
[0. , 0. , 0. , ..., 1.649, 1.643, 1.638],
[0. , 0. , 0. , ..., 1.649, 1.643, 1.638],
...,
[0. , 0. , 0. , ..., 1.647, 1.641, 1.636],
[0. , 0. , 0. , ..., 1.647, 1.641, 1.636],
[0. , 0. , 0. , ..., 1.647, 1.641, 1.636]], dtype=float32),
cam_d435i_K=array([[399.427, 0. , 320. ],
[ 0. , 399.427, 240. ],
[ 0. , 0. , 1. ]]),
cam_nav_rgb=array([[[155, 219, 255],
[154, 219, 255],
[154, 219, 255],
...,
[154, 219, 255],
[155, 219, 255],
[155, 219, 255]],
[[155, 219, 255],
[155, 219, 255],
[155, 219, 255],
...,
[155, 219, 255],
[155, 219, 255],
[155, 219, 255]],
[[155, 219, 255],
[155, 219, 255],
[155, 219, 255],
...,
[155, 219, 255],
[155, 219, 255],
[155, 219, 255]],
...,
[[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]],
[[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26],
...,
[ 26, 26, 26],
[ 26, 26, 26],
[ 26, 26, 26]]], dtype=uint8))
In [ ]:
Copied!
import cv2
images = {
'cam_d405_rgb': cams_current.cam_d405_rgb,
'cam_d405_depth': cams_current.cam_d405_depth,
'cam_d435i_rgb': cv2.rotate(cams_current.cam_d435i_rgb, cv2.ROTATE_90_CLOCKWISE),
'cam_d435i_depth': cv2.rotate(cams_current.cam_d435i_depth, cv2.ROTATE_90_CLOCKWISE),
'cam_nav_rgb': cams_current.cam_nav_rgb,
}
media.show_images(images, vmin=0.0, vmax=1.0, border=True, height=200)
import cv2
images = {
'cam_d405_rgb': cams_current.cam_d405_rgb,
'cam_d405_depth': cams_current.cam_d405_depth,
'cam_d435i_rgb': cv2.rotate(cams_current.cam_d435i_rgb, cv2.ROTATE_90_CLOCKWISE),
'cam_d435i_depth': cv2.rotate(cams_current.cam_d435i_depth, cv2.ROTATE_90_CLOCKWISE),
'cam_nav_rgb': cams_current.cam_nav_rgb,
}
media.show_images(images, vmin=0.0, vmax=1.0, border=True, height=200)
cam_d405_rgb  | cam_d405_depth  | cam_d435i_rgb  | cam_d435i_depth  | cam_nav_rgb  |
Just like on the real Stretch robot, the D435if is mounted 90 sideways (to maximize vertical fov). We use cv2.rotate() to correct it.
Note: You can use cams_current.get_all(auto_rotate=True) or cams_current.get_camera_data(StretchCameras.cam_d435i_rgb, auto_rotate=True) to get auto-rotated imagery.
Lidar¶
Stretch has a 2D planar lidar attached to its mobile base, useful for localization and building maps.
In [ ]:
Copied!
scan_data = sim.pull_sensor_data().get_data(StretchSensors.base_lidar)
scan_data[scan_data<0] = 0
degrees = np.array(range(len(scan_data)))
degrees = np.radians(degrees)
x = scan_data * np.cos(degrees) * -1
y = scan_data * np.sin(degrees) * -1
plt.scatter(x, y, color="r", s=5)
max_x = np.abs(x).max()
max_y = np.abs(y).max()
plt.xlim([-max_x-1, max_x+1])
plt.ylim([-max_y-1, max_y+1])
scan_data = sim.pull_sensor_data().get_data(StretchSensors.base_lidar)
scan_data[scan_data<0] = 0
degrees = np.array(range(len(scan_data)))
degrees = np.radians(degrees)
x = scan_data * np.cos(degrees) * -1
y = scan_data * np.sin(degrees) * -1
plt.scatter(x, y, color="r", s=5)
max_x = np.abs(x).max()
max_y = np.abs(y).max()
plt.xlim([-max_x-1, max_x+1])
plt.ylim([-max_y-1, max_y+1])
Joint state¶
Each ranged joint reports its current position (within the joint range) and velocity. The mobile base reports its translational and rotational velocity, as well as an wheel encoder estimate of it's SE2 (x, y, theta) pose in world frame (w.r.t. where the robot woke up). Like the real robot, wheel encoder estimates are noisy, and must be fused with other sensors to better estimate the robot's location.
In [9]:
Copied!
pprint.pprint(sim.pull_status())
pprint.pprint(sim.pull_status())
StatusStretchJoints(time=8.259999999999428,
fps=58.62628889063847,
base=BaseStatus(x=np.float64(-0.012182561444183192),
y=np.float64(0.004419350400411598),
theta=np.float64(-0.06498666843943465),
x_vel=np.float64(-1.1208499528483978e-08),
theta_vel=np.float64(-7.77389340487263e-08)),
lift=PositionVelocity(pos=np.float64(0.5905520090306994),
vel=np.float64(0.00022063552289719744)),
arm=PositionVelocity(pos=np.float64(0.09999622635034094),
vel=np.float64(1.3401941830460996e-10)),
head_pan=PositionVelocity(pos=np.float64(-5.005046374741913e-06),
vel=np.float64(-2.0233485946489055e-13)),
head_tilt=PositionVelocity(pos=np.float64(-0.004519272499335126),
vel=np.float64(-3.8726155029614373e-13)),
wrist_yaw=PositionVelocity(pos=np.float64(9.232975816659571e-05),
vel=np.float64(-3.3150607193177336e-05)),
wrist_pitch=PositionVelocity(pos=np.float64(-0.005324523093874352),
vel=np.float64(-4.013083878656622e-09)),
wrist_roll=PositionVelocity(pos=np.float64(-9.586627571896982e-05),
vel=np.float64(2.510829896829711e-10)),
gripper=PositionVelocity(pos=np.float64(-0.06399746756801022),
vel=np.float64(7.669883691141308e-12)))
Joint limits can be found in the Stretch Robot Overview.
Position control¶
Position control is available for the ranged joints (i.e. not the mobile base). There are two kinds of moves:
- Non-blocking deltas movements
- Blocking absolute movements.
In [ ]:
Copied!
# blocking
sim.move_to('head_tilt', -2.0)
sim.wait_until_at_setpoint('head_tilt')
media.show_image(sim.pull_camera_data().cam_nav_rgb, height=400)
# blocking
sim.move_to('head_tilt', -2.0)
sim.wait_until_at_setpoint('head_tilt')
media.show_image(sim.pull_camera_data().cam_nav_rgb, height=400)
Joint head_tilt did not reach -2.0. Actual: -1.522573472981672
 |
In [ ]:
Copied!
# non-blocking
frames = []
for _ in range(10):
sim.move_by('head_pan', -0.1)
frames.append(sim.pull_camera_data().cam_nav_rgb)
time.sleep(3)
media.show_video(frames, fps=15)
# non-blocking
frames = []
for _ in range(10):
sim.move_by('head_pan', -0.1)
frames.append(sim.pull_camera_data().cam_nav_rgb)
time.sleep(3)
media.show_video(frames, fps=15)
Velocity control¶
Velocity control is available for the mobile base. The API is set_base_velocity(<translation velocity>, <rotational velocity>). Each command is non-blocking.
In [12]:
Copied!
# non-blocking
frames = []
for _ in range(10):
sim.set_base_velocity(0.0, 1.0) # rotate in-place
frames.append(sim.pull_camera_data().cam_nav_rgb)
time.sleep(5)
media.show_video(frames, fps=15)
# non-blocking
frames = []
for _ in range(10):
sim.set_base_velocity(0.0, 1.0) # rotate in-place
frames.append(sim.pull_camera_data().cam_nav_rgb)
time.sleep(5)
media.show_video(frames, fps=15)
Stopping the simulation¶
In [13]:
Copied!
sim.stop()
sim.stop()
Stopping Stretch Mujoco Simulator... simulated runtime= 36.2s Sending signal to stop the Mujoco process... The Mujoco process has ended. Stopping thread 1/3. Thread-2 is not terminating. Make sure to check 'sim.is_running()' in threading loops. Stopping thread 2/3. _colab_inspector_thread is not terminating. Make sure to check 'sim.is_running()' in threading loops. Stopping thread 3/3. Thread-8 is not terminating. Make sure to check 'sim.is_running()' in threading loops. The Stretch Mujoco Simulator has ended. Good-bye!