This repository contains the official implementation of the SM$^2$ITH framework (Safe Mobile Manipulation with Interactive Human Prediction via Task-Hierarchical Bilevel Model Predictive Control).
SM$^2$ITH enables a robot to simultaneously perform multiple hierarchically defined tasks while safely navigating around humans. It achieves this by combining Hierarchical Task Model Predictive Control (HTMPC) with Safe and Interactive Crowd Navigation (SICNav) frameworks, utilizing bilevel optimization to jointly predict both human and robot motions.
stmith_bmpc: Contains the core algorithmic framework, the Pybullet simulation environment, and the ROS2 node used to control the Stretch3 robot.stretch_interface: Provides a collection of utility functions and scripts for running experiments on the hardware.
You need to create your workspace folder, for example
mkdir -p ~/sm2ith_ws/src
then you can move into the src folder and clone this repo
Important
You can give any name to the workspace folder however, to prevent parsing errors, ensure that it doesn't share a name with the repository sm2ith_bmpc.
❌ Do not use: ~/sm2ith_bmpc/src/sm2ith/sm2ith_bmpc
✅ Instead use: ~/sm2ith_ws/src/sm2ith/sm2ith_bmpc
in the following we are assuming that you called the workspace folder sm2ith_ws
if not in an Ubuntu 22.04 native system you have to install docker. Here is presented a way (without Dockerfiles)
Docker Engine: Run the following command to uninstall all conflicting packages.
for pkg in docker.io docker-doc docker-compose docker-compose-v2 podman-docker containerd runc; do sudo apt-get remove $pkg; done
Set up docker apt repository.
# Add Docker's official GPG key:
sudo apt-get update
sudo apt-get install ca-certificates curl
sudo install -m 0755 -d /etc/apt/keyrings
sudo curl -fsSL https://download.docker.com/linux/ubuntu/gpg -o /etc/apt/keyrings/docker.asc
sudo chmod a+r /etc/apt/keyrings/docker.asc
# Add the repository to Apt sources:
echo \
"deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.asc] https://download.docker.com/linux/ubuntu \
$(. /etc/os-release && echo "$VERSION_CODENAME") stable" | \
sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
sudo apt-get update
Install the latest version of Docker packages.
sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin
Verify that the Docker Engine installation is successful by running the hello-world image.
sudo docker run hello-world
By default, you’re required to use sudo to run Docker commands. Shortly, it is because you need to add Docker to the groups. More information can be found here.
- Create the
dockergroup
sudo groupadd docker
- Add your
userto the docker group.
sudo usermod -aG docker $USER
- Log-out and log-back to activate so that the group membership is re-evaluated (may be necessary to restart the PC).
- Verify that you can run
dockercommands withoutsudo.
docker run hello-world
This repository contains the tool to run docker images with customized local support injected for things like NVIDIA support. And user ID specific files for cleaner mounting file permissions.
Rocker is available via pip you can install it via pip using
pip install rocker
After the installation you need to restart your PC
Pull the pre-compiled docker image from docker-hub with
docker pull francescod98/ros:humble
To create a container from the downloaded image, we'll use the rocker command. While it offers several options, the most important flags for this setup are:
--home: Mounts the user's home directory inside the container.--user: Mounts the user's ID and runs the container as that user.--network{bridge,none,host}: Specifies the network mode. NOTE: This must be set to host when running experiments with the real robot.--nocleanup: Prevents the container from being automatically removed after it is stopped.--nvidia: Enables NVIDIA Docker support for GPU acceleration.
In order to maintain the container alive, some persistent docker files need to be stored. First create a folder to store them, e.g.
mkdir ~/persistent_docker_files
and then export the environmental variable
export TMPDIR=~/persistent_docker_files
Finally, the container is created using the command
rocker --home --user --x11 --privileged --nocleanup francescod98/ros:humble --name <NAME>
Important
In case of NVIDIA graphic support mounted on the PC, you would need first to install the Nvidia Docker Support with the following commands (installation with apt)
curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
&& curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo sed -i -e '/experimental/ s/^#//g' /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo apt-get update
sudo apt-get install -y nvidia-container-toolkit
and then you need to configure docker running
sudo nvidia-ctk runtime configure --runtime=docker
sudo systemctl restart docker
then, create the container appending the --nvidia flag
rocker --home --user --nvidia --x11 --privileged --nocleanup francescod98/ros:humble --name <NAME>
If the procedure ends successfully, you will be inside the container's terminal.
Useful Docker Commands:
exit: exits from the container in the current terminal. If the container is open in only one terminal, it is stoppeddocker ps -a: lists all the container that runs in the PCdocker help: to see all the basic commands of docker such as rename, delete, or list containers or imagesdocker start -ia <CONTAINER_NAME>: to start the container and enters inside its terminal. <CONTAINER_ID> can be used also, that can be seen from thedocker ps -acommanddocker exec -it <CONTAINER_NAME> bash: to attach the terminal of a running container to the current one, such as from another terminal. In any case is better to use theterminator -uinside a container to open a terminal that can be split at will to manage multiple terminals simultaneously.
First, install the following
sudo apt update
sudo apt install ros-humble-tf-transformations ros-humble-control-msgs ros-humble-xacro python3-ament-package ros-humble-rosidl-typesupport-c ros-humble-rviz2 -y
then, from the root repo folder, install python requirements
python3 -m pip install -r requirements.txt
All explanations can be found in the official guide, however pinocchio is already installed in the image francescod98/ros:humble. Please add the following to your ~/.bashrc file
export PATH=/opt/openrobots/bin:$PATH
export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH # Adapt your desired python version here
export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH
The full installation guide can be found at this page. Clone acados and its submodules by running:
git clone https://github.com/acados/acados.git
cd acados
git checkout b4bf5cd640cd8e55863e41e06509435025f9c20b
git submodule update --recursive --init
then create the build package
cd acados
mkdir -p build
cd build
compile and install
cmake -DACADOS_PYTHON=ON ..
make -j4
sudo make install -j4
for the python interface run the following commands from the build folder
pip install -e ../interfaces/acados_template
Note
The option -e makes the installation editable, so you can seamlessly switch to a later acados version and make changes in the Python interface yourself.
Add the path to the compiled shared libraries libacados.so, libblasfeo.so, libhpipm.so to LD_LIBRARY_PATH (default path is <acados_root/lib>) by running:
export LD_LIBRARY_PATH=<acados_root>/lib:$LD_LIBRARY_PATH
export ACADOS_SOURCE_DIR=<acados_root>
export PYTHONPATH=<acados_root>:$PYTHONPATH
You can put them directly in the ~/.bashrc file.
Clone the Python-RVO2 install it
git clone git@github.com:sybrenstuvel/Python-RVO2.git
cd Python-RVO2
pip install . --no-build-isolation
Note
Required for Hardware Experiments
If you want to use the VICON system with the Stretch interface, clone and build the VICON-to-ROS2 bridge in your workspace:
git clone git@github.com:dasc-lab/ros2-vicon-bridge.git
Make sure the following Boost libraries and the diagnostic-updater package are installed:
sudo apt-get install libboost-thread-dev libboost-date-time-dev ros-${ROS_DISTRO}-diagnostic-updater
Note
Suggested for Hardware Experiments
To avoid network congestion caused by ROS2 default broadcasting, we used the Zenoh middleware for experiments.
First, check which Zenoh version is installed on the robot (if any):
sudo apt list ros-${ROS_DISTRO}-zenoh-cpp-vendor --installed
Note
If the robot has already a distribution installed which differs from the latest distribution, build the matching version from source by following the official Zenoh instructions.
Otherwise, install it on both your machine and the robot:
sudo apt-get install ros-${ROS_DISTRO}-rmw-zenoh-cpp
Start the Zenoh router with:
RUST_LOG=zenoh=info ros2 run rmw_zenoh_cpp rmw_zenohd
The terminal output will show your machine TCP endpoint, for example tcp/123.456.789.000:1111. Add this endpoint to the robot-side zenoh_router_config.json5 endpoints list. Also add the robot TCP endpoint to the same configuration file on your machine.
Move to your workspace
cd ~/sm2ith_ws
source ROS2 workspace
source /opt/ros/humble/setup.bash
build the environment
colcon build --symlink-install
Then if the building process succeeds you can source your workspace with
source ~/sm2ith_ws/install/setup.bash
First, source the workspace setup.bash file from the workspace directory and from opt/ros/humble/ folder
source /opt/ros/humble/setup.bash
source ~/sm2ith_ws/install/setup.bash
then export the following environmental variable for convenience
export WORKSPACE=~/sm2ith_ws/src/sm2ith
If this is the first time you download the repo, to start the simulation you need to generate acados code by running
cd ${WORKSPACE}/sm2ith_bmpc/sm2ith_control/scripts && python3 generate_acados_code.py -c ${WORKSPACE}/sm2ith_bmpc/sm2ith_run/config/simulation_stretch.yaml
To start the simulation, run
cd ${WORKSPACE}/sm2ith_bmpc/sm2ith_run/scripts && python3 simulations.py --config ${WORKSPACE}/sm2ith_bmpc/sm2ith_run/config/simulation_stretch.yaml --GUI
First ssh into the robot
ssh -X hello-robot@<ROBOT_IP>
If you just turned the robot on, run the calibration start the drivers
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
stretch_robot_home.py # run calibration
ros2 launch stretch_core stretch_driver.launch.py
Note
in every terminal that you are going to use from now on you will need to export the following environmental variable
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
You need two additional terminals: one on the robot and one on your local machine, both running a Zenoh router.
On your local machine terminal, export the environment variable and start the router:
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
RUST_LOG=zenoh=info ros2 run rmw_zenoh_cpp rmw_zenohd
If needed, restart the ROS2 daemon first with ros2 daemon stop.
On the robot terminal, it is recommended to export the environment variable in .bashrc, then start the router with the same command:
RUST_LOG=zenoh=info ros2 run rmw_zenoh_cpp rmw_zenohd
in another terminal on the local machine, once you sourced the environment, check if the ip address in the src/ros2-VICON-bridge/launch/all_segments.launch.py is the one of the VICON system present in the lab and run
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
ros2 launch sm2ith_run vicon_bridge.launch.py
The expected topic is vicon/stretch_base/stretch_base, where the base pose is published. If the topic name is different, update it here for stretch_interface. For the agent side, check the name assigned to each agent_vicon_interface, which expects the /vicon/tum_<color>_hat/tum_<color>_hat pattern.
Note
this launch command will spawn a rviz window with the robot model which is expecting to find the robot_description package copied in the src folder from the robot ament_ws. To do so first install rsync with
sudo apt install rsync
with a ssh connection in the robot registered in the .ssh/config file named as stretch3, you can run
rsync -r --verbose --progress --update -t stretch3:~/ament_ws/src/stretch_description <YOUR_WS>/src/
To launch the experiment open a new terminal, source the workspace, export the sm2ith folder export WORKSPACE=<PATH_TO_SM2ITH_FOLDER>, then run
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
cd ${WORKSPACE}/sm2ith_bmpc/sm2ith_run/src/sm2ith_run/
python3 controller_ros.py --config ${WORKSPACE}/sm2ith_bmpc/sm2ith_run/config/experiment_stretch.yaml