Shielding Locomotion Policies with Predictive Safety Filtering
Aditya Shirwatkar, Sebastian Sanokowski, Shishir Kolathaya, Aaron Johnson, Majid Khadiv
This is the code for the paper Shield-Loco. A Unitree Go2 walks with a pre-trained RL policy that follows foot contact locations. The safety filter changes those contact locations when it predicts a collision, so the robot steps around obstacles and stays close to what the user asked for.
This repo is built on top of DOOM, a framework from the Atari lab for running controllers on Unitree robots, in simulation and on the real robot. DOOM handles the robot connection, Docker, ROS2, the joystick and the contact-conditioned policy. The Shield-Loco safety filter is added on top of it, for the Go2 only.
- The Shield-Loco part is everything above the "Base DOOM" line: setup, running the simulation, reproducing the paper, running on the robot. Start here.
- The Base DOOM part is the original DOOM documentation, kept as it was further down. Read it if you want to know how DOOM itself works, or you need the Vicon and robot connection setup for hardware.
- Names you will see in the code: MPS stands for Model Predictive Shielding, which is why the tasks are called
rl-mps-*.MIMPSis the MPPI version,CEMPSthe CEM version, andREMIMPSthe replica exchange version, which is the default and the one used in the paper. - Where the code is is listed near the end of the Shield-Loco part, in Where is the code.
๐ฆ Setup โข ๐งช Simulation โข ๐ Viser โข ๐ Reproducing the paper โข ๐ฆฟ Hardware โข ๐พ Data โข ๐ Where is the code โข ๐ Notes โข ๐ Citation โข ๐ License
You need Linux, Docker and an NVIDIA GPU with nvidia-container-toolkit. The simulation results in the paper were run on an RTX 3090.
git clone --recurse-submodules https://github.com/Atarilab/ShieldLoco.git
cd ShieldLoco
./doom.sh -b # builds the base DOOM image
./doom.sh -s # builds the Shield-Loco image on top of it
./doom.sh -e --safety # enter the container./doom.sh -b is enough for simulation. For the real robot use ./doom.sh -i instead, which also builds the image and then adds a network connection to the robot (it asks for sudo).
Note: All DOOM images share one container called
DOOM, and./doom.sh -ereuses it if it exists. If you ever entered the base container before, run./doom.sh -dfirst (it deletes the old containers), otherwise-e --safetydrops you into one without the Shield-Loco libraries.
Inside the container, run b once to build the ROS workspace, then work from src/. The image also adds a few shortcuts: go2_mps_sim (simulator), kill_eval (stops leftover sim and controller processes).
The first start takes a few minutes while the optimizer compiles. Wait for it.
The simulator serves a 3D view at http://localhost:8080 (open it on the machine running Docker, or use that machine's IP). It shows the robot, the obstacles, the planned contact locations and ghost copies of the predicted robot poses. This works with or without a display, so you can use it over ssh.
Easiest: let the eval launcher drive. No joystick needed. It starts the simulator and controller, waits for the filter to compile, then makes the robot trot forward with the filter on:
cd src/
python3 eval/launch_eval.py --scenario eval/scenarios/scenario_a.yaml --seed 42Open the browser while it runs. The run stops when the robot reaches x = 6.5 m or the time limit, and also saves CSVs (logs/eval/).
Manual: drive it yourself (same as the other DOOM tasks):
# Terminal 1: simulator (add --headless if you have no display, viser still works)
go2_mps_sim
# Terminal 2: attach with ./doom.sh -a, then
ros2 run master_manager master_node --task rl-mps-sim-go2Then follow the joystick table in the Hardware section: Select โ Start โ L1+R1 โ L1+R1, then A turns the filter on and the arrow buttons walk the robot. The terminal UI (--ui) can stand the robot up and send velocities, but the filter on/off switch is only on the joystick (A). With no gamepad, see the virtual joystick part of docs/joystick_debug.md.
These run from src/ inside the container. The launch_eval.py command above is one eval run (Ctrl+C stops both processes). scenario_a.yaml is the robot trotting forward to x = 6.5 m through the cluttered scene. scenario_b.yaml adds a ball thrown at the robot.
Many runs and sweeps over settings:
python3 eval/run_eval.py --scenario eval/scenarios/scenario_a.yaml --runs 10 --seed 0 \
--sweep mps_alg_name=MIMPS,REMIMPS,CEMPS mps_iterations=1,3,6 --out results/my_sweep
python3 eval/analyze_results.py results/my_sweep/*/results.json --out results/my_sweep--sweep accepts any setting from rl_mps_go2_cfg.yaml (nested ones as CBF.clearance=0.35) and runs every combination. Results are written to src/results/ and the summary table shows collisions, tracking cost, optimizer cost and optimization time.
Edit the settings at the top of each script, then run it with bash from src/:
| Study | Script |
|---|---|
| Optimizers, iterations, horizon, samples, momentum | eval.sh |
| CBF, HJ and LIPM baselines against no filter | baselines_ablation.sh |
| Learned value function: off, heuristic, direct, residual | sv_ablation.sh |
Settings used in the paper: horizon 5, 3 iterations, 512 samples, momentum 0.1, 10 seeds. The defaults in the yaml are different (for example 1200 samples and momentum off), so pass the paper values when sweeping, e.g. --sweep mps_num_samples=512 mps_use_momentum=true mps_momentum=0.1.
The learned value function is off by default (safety_value_mode: "off") and no trained weights are included. To train your own:
- In
rl_mps_go2_cfg.yamlsetsafety_value_data_dir: "logs/safety_value_rollouts", so the filter saves its rollouts. - From
src/, runbash collect_data.sh(5 seeds of scenario A). - Run
bash train_terminal.sh. It trains thedirectandresidualversions and writesdirect.msgpackandresidual.msgpacktosafety_filter/quad/safety_value_checkpoint/, which is wheresv_ablation.shlooks.
Then set safety_value_mode to direct or residual to use them.
This needs the Vicon setup from the DOOM docs below, since the robot and the obstacles are tracked with Vicon. Turn off sport mode first (see the warning in Testing Robot Connection).
Launch Vicon client for base state estimation
./doom.sh -e --safety
ros2 launch vicon_receiver client.launch.pyGenerate the obstacles xml, lay the robot down and disable the sport mode of the robot
./doom.sh -a
cd src
python3 scripts/gen_obstacles_xml.py
python3 scripts/safely_lay_down.py
python3 scripts/disable_sport_mode.pyNote: Check that the xml matches the real objects by running
go2_mps_sim --headlessand opening it in the browser. To use the generated obstacles, switch the include inscene.xmlandscene_mjx.xmlfromobstacles.xmltoobstacles_hw.xml.gen_obstacles_xml.pyalso changes the start position ingo2.xmlandgo2_mjx.xml,git checkoutthose files to undo it.
./doom.sh -a
source setup.sh
ros2 run master_manager master_node --task rl-mps-real-go2Record Vicon pose and optimizer metrics to CSV. Start it before the controller so the whole episode is captured, and stop it with Ctrl+C.
./doom.sh -a
source setup.sh
cd src
python3 eval/hw_data_collector.py --label static_nav
python3 eval/hw_data_collector.py --label static_nav --rosbag # with rosbag| Sequence | Button | Action |
|---|---|---|
| Startup | Select |
ZERO mode |
Start |
STAY_DOWN | |
L1+R1 |
STANCE | |
L1+R1 |
RL-MPS (stance) | |
| In RL-MPS | A |
Safety filter on/off |
X |
Toggle trot โ stance | |
Up/Down |
Forward/backward speed | |
Left/Right |
Strafe left/right | |
Y/B |
Turn left/right | |
L1+R1 |
Reset to defaults | |
| Shutdown | Start |
DAMPING |
Note: The filter runs on the GPU in its own process, so the container needs GPU access. It starts OFF, press
Ato turn it on. The paper's hardware runs used a horizon of 6 (about 200 ms per plan) and 8 (about 400 ms).
While the controller runs it publishes /mps_optimizer_metrics every control step. It holds 12 numbers: tracking cost, optimizer cost, optimization time (ms), whether the filter is on, plan number, body-frame velocity, base height and base position. eval/hw_data_collector.py saves these together with the Vicon pose.
In simulation there is no Vicon, so the controller's own position estimate is used:
cd src/
python3 eval/hw_data_collector.py --sim --task rl-mps-eval-go2 --label static_nav
python3 eval/hw_data_collector.py --sim --label static_nav --rosbagEach recording goes to a new folder {alg}_{interface}_{label}_{timestamp}/:
results/hw_eval/REMIMPS_hw_static_nav_20260512_143022/
โโโ configs/
โ โโโ controller_cfg.yaml # copy of rl_mps_go2_cfg.yaml
โ โโโ robot_interface_cfg.yaml # copy of real_go2_cfg.yaml
โโโ xmls/
โ โโโ scene.xml, scene_mjx.xml # scene xmls
โ โโโ go2.xml, go2_mjx.xml # robot xmls
โ โโโ obstacles_hw.xml # obstacle geometry
โ โโโ balls.xml # moving obstacle config
โโโ eval_data.csv # time series
โโโ experiment_config.json # label, task, mode, timestamp
โโโ rosbag/ # only if --rosbag
CSV columns: t, x, y, z, qw, qx, qy, qz, heading, tracking_cost, optimizer_cost, opt_elapsed_ms, mpc_active, plan_seq, ctrl_vx, ctrl_vy, ctrl_height, frame_number
Nothing was moved from the DOOM layout. These are the files added for Shield-Loco:
| Where | What |
|---|---|
src/safety_filter/ |
The safety filter. safety_manager.py runs it in a separate process on the GPU, quad/task.py simulates and scores candidate contact plans, and the other files in quad/ are listed below. |
src/controllers/rl_mps_controller.py |
The Go2 controller that uses the filter. It runs the policy like the normal rl-contact controller, but takes its contact plan from the filter when a collision is predicted. |
src/controllers/config/rl_mps_go2_cfg.yaml |
All the settings. |
src/eval/, src/eval.sh, src/baselines_ablation.sh, src/sv_ablation.sh |
Running experiments and collecting results. |
src/scripts/ |
Hardware helpers, and training of the learned value function. |
src/robots/go2/obstacles.xml |
The obstacle scenes. The cluttered scene is switched on, the navigation scene is commented out. |
Dockerfile.safety |
Extra libraries for the filter (JAX, MJX, MuJoCo Warp). |
The method pieces in src/safety_filter/quad/:
| Piece | File |
|---|---|
Replica exchange optimizer (the default, REMIMPS) |
remimps.py |
Plain MPPI (MIMPS) and CEM (CEMPS) |
mimps.py, cemps.py |
| Projecting contacts out of obstacles | foot_projection.py |
| Momentum | settings mps_use_momentum and mps_momentum |
| Learned value function | safety_value.py |
| Baselines: CBF, HJ reachability, LIPM | cbf_planner.py, hj_planner.py, lipm_planner.py |
cmps.py and dmps.py are unfinished and not used in the paper.
A few DOOM files were edited to hook things in: task_configs.py (the rl-mps-* tasks), simulate.py, sim_robot_interface.py, go2.py, scene.xml and doom.sh (which got the --safety option). Everything else is untouched DOOM, including the G1 robot, which has nothing to do with the paper. The Shield-Loco work starts in February 2026 in the git history.
- The filter is given the obstacle positions (from the xml, or from Vicon on hardware). There is no perception.
- Moving obstacles (the ball in
scenario_b.yaml) were only shown qualitatively in the paper. - The filter reduces collisions, it does not remove them. Some still happen in the dense scenes.
- Simulation speed depends on your GPU. The paper's numbers are from an RTX 3090.
src/obs.datais a leftover from experiments (obstacle sizes thatgen_obstacles_xml.py --obs-datacan read). The tests insrc/safety_filter/tests/were written during development and are not a full test suite.
@article{shirwatkar2026shieldloco,
title = {Shield-Loco: Shielding Locomotion Policies with Predictive Safety Filtering},
author = {Shirwatkar, Aditya and Sanokowski, Sebastian and Kolathaya, Shishir and Johnson, Aaron and Khadiv, Majid},
journal = {arXiv preprint arXiv:2606.07193},
year = {2026}
}BSD 3-Clause, see LICENSE.
Unified Interface for Real Robot Control and MuJoCo Real-time Control
A framework for controlling Unitree robots (Go2, G1) in the real world and in Mujoco real-time using a unified interface.
Everything below is the original DOOM documentation, as it was before Shield-Loco was added. The Shield-Loco tasks are rl-mps-sim-go2, rl-mps-eval-go2 and rl-mps-real-go2.
โ๏ธ Requirements โข ๐ฆ Installation Instructions โข ๐ค Testing Robot Connection โข ๐ How to Use DOOM โข ๐ฎ Joystick Control โข ๐๏ธ Task Library
๐ก Vicon State Estimation โข ๐ PlotJuggler โข ๐๏ธ RViz โข ๐ Debugging โข ๐งฉ DOOM Elements โข ๐ TODO
- docker (ros2 container with unitree_sdk)
- nvidia Graphics card and nvidia-container-toolkit
For installation of the DOOM project, run:
./doom.sh -iThe above script, updates all submodules, builds the docker container, and manually sets up a network interface in the same subnetwork as the robot.
./doom.sh -b # build
./doom.sh -e # enterFor more helpful functions from ./doom.sh, run:
./doom.sh -hOpen VS Code with unitree_mujoco_container as the project directory and build the docker container. Additionally, debuggers for certain tasks are already defined in .vscode/launch.json.
Warning: Before you run anything on the robot, make sure to turn off the sports mode using the Go2 app. Log in to the app using the ATARI Gmail credentials and toggle off Device > Service Status > sport_mode as this will interfere with the additional torque commands passed to the robot. Check: Unitree ros topics should appear by default with 'ros2 topic list'. If not, try restarting your computer.
Once inside the docker container, you can access the Robot's IP address via $ROBOT_IP. You can test the connection using:
ping $ROBOT_IPIf the connection is not established, you might need to manually set the IP for the wired connection. You can do so by following the "Configure Network Environment" section here.
If it still doesn't ping the robot after manually configuring the IP, you can check if the right network interface is chosen. Using the USB-Ethernet Adapter, the network interface should have the ID enx3c4937046061 by default. You can confirm it using ifconfig and checking the network interface ID for the corresponding $ROBOT_IP. If not, you should manually change it in .env.docker, then delete the existing container using ./doom.sh -d, rebuild the container using ./doom.sh -b, enter inside the container using ./doom.sh -e, and update the ROS2/DDS network interface inside setup.sh using the one you found with ifconfig. Don't forget to run source setup.sh to update them.
The various tasks are defined in tasks/task_configs.json. Currently, the following tasks are defined and tested:
rl-velocity-sim-go2(Status: โ )rl-velocity-real-go2(Status: โ )rl-contact-sim-go2(Status: โ )rl-contact-real-go2(Status: โ )rl-velocity-sim-g1(Status: โ )rl-velocity-real-g1(Status: โ )rl-contact-sim-g1(Status:โ ๏ธ )rl-mps-sim-go2(Status: โ )rl-mps-real-go2(Status: โ )
Once you've chosen the task you want to run, you can run the master node to control the robot using:
ros2 run master_manager master_node --task <insert-task-name>Use the --ui argument if you want to use the terminal UI interface. Else, you can also use the joystick to control the robot.
This repository also has a simulation mode, which allows you to run the same scripts with the unitree_sdk to send commands to your robot in MuJoCo. Note that MuJoCo is not used as a visualizer for your real robot interface but rather as a sanity test of the same script that you might run on the real robot. It uses a modified version of MuJoCo to behave real-time.
To launch the simulator, run:
python3 simulate.py --task <insert-task-name>Example Workflow for rl-velocity-sim-go2 task: Standing > Stay_down > Stand_up > Back to Main Menu > Locomotion > RL-Velocity
Before you start, make sure there are no other main processes running on your computer. This could cause jittery movements due to imperfect tracking of the PD controllers introduced by the latency from heavy process in the background. Keep an eye out for your CPU utilisation using htop to validate this. A simple restart can ensure that you start fresh.
./doom.sh -e # enter the container
cd src/
python3 simulate.py --task=rl-velocity-sim-go2./doom.sh -a # attach a terminal to the existing DOOM container
source setup_local.sh
ros2 run master_manager master_node --task rl-velocity-sim-go2 --ui # use ui for the terminal UI interface (additionally, we can also manage commands to the robot via joystick with/without UI)ZERO mode is used to send zero torques to the robot. DAMPING mode is a damping mode to gracefully stop commands to the robot.
STAND modes are used to initialise the robot to its default positions using simple PD controllers. Click on STAND, and then STAY_DOWN. This makes the robot stay in a crouched position close to the ground. After it stabilises, click on STAND_UP, which is a phase-based PD controller that makes the robot stand up to the default joint configuration. STAND_DOWN is also a phase-based PD controller that moves from the standing up joint configuration to the crouched joint position, as in STAY_DOWN.
Note: Since these
STANDmodes are phase-based PD controllers, allow them to stabilise before switching to other modes.
Note: The UI terminal window may need to be resized to see the different modes. If using Terminator, you can use
Ctrl+Shift+Xfor full screen andCtrl + MouseScrollto adjust the window dimensions to fit your screen
Now that the robot is in the STAND_UP configuration, go back to the Main Menu from the UI, choose LOCOMOTION, and then RL-VELOCITY. This will start the velocity command based blind RL locomotion policy at zero velocity. From the UI, you can now enter the X, Y velocities and the yaw rates for fixed command velocities.
The only change for running on the real robot is launching the Vicon client to get the base states instead of launching the simulator, and using the correct task name for the real experiments. The rest is taken care of by DOOM to maintain the same interface to run experiments in sim (MuJoCo) and on the real robot.
Launch Vicon client that publishes the base states
./doom.sh -e # enter the container
ros2 launch vicon_receiver client.launch.py./doom.sh -a # attach a terminal to the existing DOOM container
source setup.sh
ros2 topic list # view available topics, confirm if you can view topics published by the robot and by the vicon
ros2 run master_manager master_node --task rl-velocity-real-go2 --ui # use ui for the terminal UI interface (additionally, we can also manage commands to the robot via joystick with/without UI)Use plotjuggler to view topics in real time plots
./doom.sh -a # attach a terminal to the existing DOOM container
source setup.sh
ros2 run plotjuggler plotjugglerRobot Visualization in RViz
./doom.sh -a # attach a terminal to the existing DOOM container
source setup.sh
ros2 launch go2_description go2_visualization.launch.pyAlternatively, to send commands to the robot, users can also use the joystick. Make sure that the Docker container is launched with the joystick already connected. Otherwise, you will need to rebuild the container and enter again. There are some common joystick configurations to handle the mode-switches:
Select: Any -> ZERO
Start: ZERO/DAMPING -> STAY_DOWN
Start: AnyElse -> IDLE
Up: STAY_DOWN/STAND_DOWN -> STAND_UP
Down: STAND_UP -> STAND_DOWN
Down: STAND_DOWN -> STAY_DOWN
L1+R1: STAND_UP -> RL-VELOCITY
You can further define controller-specific joystick mappings by defining get_joystick_mappings (See RL-Contact Controller for reference).
The Vicon receiver client is already installed in the Docker container. You can launch it in a new terminal inside the existing container (./doom.sh -a) using:
ros2 launch vicon_receiver client.launch.pyros2 run plotjuggler plotjugglerVisualizing in RViz is useful, especially for debugging, to see what the world looks like to the robot. Once the master node is launched in a terminal, you can launch RViz in another terminal using:
ros2 launch go2_description go2_visualization.launch.pyThis project uses black as the code formatter and flake7 as additional linter to ensure consistent code style across the codebase. You can run the formatter inside the container in VS Code (or other forks like Cursor, Windsurf etc.) using the keyboard shortcut Ctrl + Shift + B and running the task Format and Lint.
A logger is available throughout almost every part of the project. You can print debug logs using logger.debug() or info logs using logger.info(). Unless in debug mode, the debug logs won't appear on the terminal. It will only be saved to src/logs. The debug mode can be turned on using --debug argument in launching the master node, such as
ros2 run master_manager master_node --task rl-velocity-real-go2 --log velocity_real --debug
When running in debug mode, it is also possible to add breakpoints inside threaded functions (we set daemon=False). We highly recommend using a debugger such as the one existing in VSCode (debugpy) to add breakpoints and debug the code.
๐ง Master Manager
The master manager is the entry point of DOOM. It loads up the necessary configurations based on the arguments you provide to it, the main one being the task, used to resolve the task, robot and interface (sim/real). For example, rl-velocity-sim-go2 is used to resolve the robot: Go2, the interface: simulation, the controller type: rl, and the method: contact. It follows the convention: <controller-type>-<method>-<interface>-<robot>
. The available configs are defined in task_configs.py and can be appended with new ones for new tasks.
LowLevelCmdPublisher is the ROS2 node inside the master_manager that runs the main program loop inside the callback. Essentially, it updates the states and passes them to the controller that is active, which returns low-level commands which could be in the form of PD targets or torques. The low-level commands are then published through the unitree communication channel (which uses DDS), to either the simulation interface or real robot interface (which are automatically resolved from the task name). Optionally, the UI Interface can be run concurrently with the LowLevelCmdPublisher inside master_manager using the ui argument.
๐ State Manager
The state manager is responsible for listening to different ROS2/DDS topics. Each subscriber also has callbacks/handlers which are defined in state_manager/msg_handlers.py. The state manager then makes these states available to your controllers in the form of a dictionary.
๐ Mode Manager
The mode manager is used to switch between different modes or controllers which are available to the robot based on the task. By default, there are the ZERO and DAMPING modes available across DOOM for all tasks, which are used to send zero torques and damping torques, respectively. When starting the robot, it is useful to have these modes to initialise the robot to some default safe poses, and then switch to the mode/controller that you developed. For example, for the task rl-velocity-sim-go2, apart from the default zero and damping modes, also has standing controllers. These are phase-based PD controllers that can stabilise and bring the robot to default positions. Once ready, you can then switch to the RL-VELOCITY mode/controller.
๐ Observation Manager
Each controller also has an optional observation manager. This is different from the state manager. The states are what you directly subscribe to through ROS2/DDS topics. However, you may sometimes need to create additional observations based on the states you have. For example, in RL routines, it is common to have a projected gravity vector as an observation instead of using quaternions for the base orientation. These can be defined as a function that computes the projected gravity vector from the states. You need to register the required observations in your controller to use this. Additionally, you can also pass additional params to the observation functions. The observation manager is especially useful for RL since you can register the observations in your RL Controller class, and it can directly compute a vector of observations (respecting the order in your registration) that can be directly given to your policy to compute the action. If you are familiar with the ObservationManager in IsaacLab, this is exactly what that does. Additionally, the RL routines have separate threads for policy inference and observation processing and gets the latest policy action inside the compute_lowlevelcmd().
๐ฆพ RobotBase
The robot class defines robot-specific data. This is also where you define the available controllers and the subscribers for your task and robot, based on the task name. You also have access to a MuJoCo wrapper with robot.mj_model. The robots supported now are Unitree Go2 and G1. You can check out robots/<robot-name>/<robot-name>.py for more robot-specific information.
๐ฏ ControllerBase
This is a base controller class that needs to be inherited if you need to define your own controller. In the barest form for creating a new controller, all you need is to inherit the ControllerBase and complete the compute_lowlevelcmd function, that has access to the states that you subscribe to, and you can compute the desired motor commands in the form of PD targets or torques and pass return it. An example of this can be seen in the ZeroController or the DampingController. Then you need to make the controller available in your robot class in its corresponding available_controllers.
Note: By default, it is not a ROS2 node. However, you can convert it into one by also inheriting from Node in rclpy.node. Usually, this is only required if you want to visualise something from inside your controller. If you need more info on doing this, check out the RLControllerBase.
๐ฎ Joystick Interface
The joystick interface is used to switch between different modes/controllers and also to send commands to the controller. There are already some common joystick transitions defined to switch across the different modes. Additionally, you can add your own joystick mappings inside your controller by adding them in get_joystick_mappings(). Pay attention to not overriding existing joystick mappings for damping and zero modes for safety reasons.
๐ฅ๏ธ RobotControlUI
This is an optional UI Interface that allows you to choose different modes and send commands to the robot from Terminal UI. In DOOM, we recommend using the joystick instead, since the UI contributes to additional CPU overhead.
Note: This is a research prototype. Use at your own risk.
- Test g1 bimanual manipulation policy
- Add support for AlienGo/Allegro
- Remove ROS2 Node from
LowLevelCmdPublisherand publish robot states instead fromstate_manager. - Make StateManager a ROS2 Node.
- Remove low level cmd spin code from master node
- Remove ROS2 Node to just Subscriber inside
ROS2StateSubscriber. - Add controller for tuning the PD gains of G1
- Add RViz visualization for G1
- Add g1 velocity locomotion Policy from unitree example (rl-unitree-sim-g1) and IsaacLab (rl-velocity-sim-g1)
- Create table with box env for G1
- Implement simple stand up, stand down controllers for G1
- Add G1 in sim
- Optional Terminal UI Interface
- Test RViz on the real robot for contact-conditioned policy (stance, trot in place)
- Implement joystick control for controller-specific commands
- Implement joystick control for switching between common modes
- Implement a real-time High-Level Contact Planner
- Publish robot and joint states to corresponding topics from master node
- Visualize Robot in RViz
- Implement interface to change commands from UI
- Implement safety mechanisms (soft dof pos limits, dof torque limits)
- Get vicon frame from Vicon SDK and transform to robot base (directly using base position from vicon after @victorDD1's update)
- Add mechanism for real-time state logger and plotter (debug logger in console and file, plotjuggler for ros topics)
- Test Velocity-conditioned policy
Unitree Guide: https://support.unitree.com/home/en/developer/Quick_start


