commit ffe9f7faeacbd308c06eac46ac264ddbc594dd01 Author: Dennis Da Date: Wed Nov 12 23:13:39 2025 -0800 Initial commit diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..264e058 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,23 @@ +# Machine learning models and data files +*.pt filter=lfs diff=lfs merge=lfs -text +*.onnx filter=lfs diff=lfs merge=lfs -text +*.pkl filter=lfs diff=lfs merge=lfs -text +# 3D assets and models +*.usd filter=lfs diff=lfs merge=lfs -text +*.usda filter=lfs diff=lfs merge=lfs -text +*.STL filter=lfs diff=lfs merge=lfs -text +*.stl filter=lfs diff=lfs merge=lfs -text +# Shared libraries +*.a filter=lfs diff=lfs merge=lfs -text +*.so* filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.gif filter=lfs diff=lfs merge=lfs -text +*.safetensors filter=lfs diff=lfs merge=lfs -text +*.deb filter=lfs diff=lfs merge=lfs -text +# Collect demo in sim +*.hdf5 filter=lfs diff=lfs merge=lfs -text +*.parquet filter=lfs diff=lfs merge=lfs -text +*.obj filter=lfs diff=lfs merge=lfs -text +*.dae filter=lfs diff=lfs merge=lfs -text +*.so filter=lfs diff=lfs merge=lfs -text +*.so.* filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..920382b --- /dev/null +++ b/.gitignore @@ -0,0 +1,175 @@ + # Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions + + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# UV +# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +#uv.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/latest/usage/project/#working-with-version-control +.pdm.toml +.pdm-python +.pdm-build/ + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# IDE +.idea/ +.vscode/ + +# log +outputs/ + +# Ruff stuff: +.ruff_cache/ + +# PyPI configuration file +.pypirc + +outputs/ + +.DS_Store + + +# Mujoco +MUJOCO_LOG.TXT + +# IsaacDeploy +external_dependencies/isaac_teleop_app/isaac-deploy + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..7f9b118 --- /dev/null +++ b/LICENSE @@ -0,0 +1,38 @@ +NVIDIA License + +1. Definitions + +“Licensor” means any person or entity that distributes its Work. +“Work” means (a) the original work of authorship made available under this license, which may include software, documentation, or other files, and (b) any additions to or derivative works thereof that are made available under this license. +The terms “reproduce,” “reproduction,” “derivative works,” and “distribution” have the meaning as provided under U.S. copyright law; provided, however, that for the purposes of this license, derivative works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work. +Works are “made available” under this license by including in or with the Work either (a) a copyright notice referencing the applicability of this license to the Work, or (b) a copy of this license. + +2. License Grant + +2.1 Copyright Grant. Subject to the terms and conditions of this license, each Licensor grants to you a perpetual, worldwide, non-exclusive, royalty-free, copyright license to use, reproduce, prepare derivative works of, publicly display, publicly perform, sublicense and distribute its Work and any resulting derivative works in any form. + +3. Limitations + +3.1 Redistribution. You may reproduce or distribute the Work only if (a) you do so under this license, (b) you include a complete copy of this license with your distribution, and (c) you retain without modification any copyright, patent, trademark, or attribution notices that are present in the Work. + +3.2 Derivative Works. You may specify that additional or different terms apply to the use, reproduction, and distribution of your derivative works of the Work (“Your Terms”) only if (a) Your Terms provide that the use limitation in Section 3.3 applies to your derivative works; (b) you comply with Other Licenses, and (c) you identify the specific derivative works that are subject to Your Terms and Other Licenses, as applicable. Notwithstanding Your Terms, this license (including the redistribution requirements in Section 3.1) will continue to apply to the Work itself. + +3.3 Use Limitation. The Work and any derivative works thereof only may be used or intended for use non-commercially. As used herein, “non-commercially” means for non-commercial research purposes only, and excludes any military, surveillance, service of nuclear technology or biometric processing purposes. + +3.4 Patent Claims. If you bring or threaten to bring a patent claim against any Licensor (including any claim, cross-claim or counterclaim in a lawsuit) to enforce any patents that you allege are infringed by any Work, then your rights under this license from such Licensor (including the grant in Section 2.1) will terminate immediately. + +3.5 Trademarks. This license does not grant any rights to use any Licensor’s or its affiliates’ names, logos, or trademarks, except as necessary to reproduce the notices described in this license. + +3.6 Termination. If you violate any term of this license, then your rights under this license (including the grant in Section 2.1) will terminate immediately. + +3.7 Components Under Other Licenses. The Work may include or be distributed with components provided with separate legal notices or terms that accompany the components, such as open source software licenses and other license terms, including but not limited to the Meta OPT-IML 175B License Agreement (“Other Licenses”). The components are subject to the applicable Other Licenses, including any proprietary notices, disclaimers, requirements and extended use rights; except that this Agreement will prevail regarding the use of third-party software, unless a third-party software license requires it license terms to prevail. + +4. Disclaimer of Warranty. + +THE WORK IS PROVIDED “AS IS” WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WARRANTIES OR CONDITIONS OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE OR NON-INFRINGEMENT. YOU BEAR THE RISK OF UNDERTAKING ANY ACTIVITIES UNDER THIS LICENSE. + +5. Limitation of Liability. + +EXCEPT AS PROHIBITED BY APPLICABLE LAW, IN NO EVENT AND UNDER NO LEGAL THEORY, WHETHER IN TORT (INCLUDING NEGLIGENCE), CONTRACT, OR OTHERWISE SHALL ANY LICENSOR BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR RELATED TO THIS LICENSE, THE USE OR INABILITY TO USE THE WORK (INCLUDING BUT NOT LIMITED TO LOSS OF GOODWILL, BUSINESS INTERRUPTION, LOST PROFITS OR DATA, COMPUTER FAILURE OR MALFUNCTION, OR ANY OTHER DAMAGES OR LOSSES), EVEN IF THE LICENSOR HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..09d1141 --- /dev/null +++ b/Makefile @@ -0,0 +1,16 @@ +.PHONY : run-checks +run-checks : + isort --check . + black --check . + ruff check . + # mypy . + +.PHONY : format +format : + isort . + black . + +.PHONY : build +build : + rm -rf *.egg-info/ + python -m build diff --git a/README.md b/README.md new file mode 100644 index 0000000..b593832 --- /dev/null +++ b/README.md @@ -0,0 +1,140 @@ +# gr00t_wbc + +Software stack for loco-manipulation experiments across multiple humanoid platforms, with primary support for the Unitree G1. This repository provides whole-body control policies, a teleoperation stack, and a data exporter. + +--- + +## System Installation + +### Prerequisites +- Ubuntu 22.04 +- NVIDIA GPU with a recent driver +- Docker and NVIDIA Container Toolkit (required for GPU access inside the container) + +### Repository Setup +Install Git and Git LFS: +```bash +sudo apt update +sudo apt install git git-lfs +git lfs install +``` + +Clone the repository: +```bash +mkdir -p ~/Projects +cd ~/Projects +git clone https://github.com/NVlabs/gr00t_wbc.git +cd gr00t_wbc +``` + +### Docker Environment +We provide a Docker image with all dependencies pre-installed. + +Install a fresh image and start a container: +```bash +./docker/run_docker.sh --install --root +``` +This pulls the latest `gr00t_wbc` image from `docker.io/nvgear`. + +Start or re-enter a container: +```bash +./docker/run_docker.sh --root +``` + +Use `--root` to run as the `root` user. To run as a normal user, build the image locally: +```bash +./docker/run_docker.sh --build +``` +--- + +## Running the Control Stack + +Once inside the container, the control policies can be launched directly. + +- Simulation: + ```bash + python gr00t_wbc/control/main/teleop/run_g1_control_loop.py + ``` +- Real robot: Ensure the host machine network is configured per the [G1 SDK Development Guide](https://support.unitree.com/home/en/G1_developer) and set a static IP at `192.168.123.222`, subnet mask `255.255.255.0`: + ```bash + python gr00t_wbc/control/main/teleop/run_g1_control_loop.py --interface real + ``` + +Keyboard shortcuts (terminal window): +- `]`: Activate policy +- `o`: Deactivate policy +- `9`: Release / Hold the robot +- `w` / `s`: Move forward / backward +- `a` / `d`: Strafe left / right +- `q` / `e`: Rotate left / right +- `z`: Zero navigation commands +- `1` / `2`: Raise / lower the base height +- `backspace` (viewer): Reset the robot in the visualizer + +--- + +## Running the Teleoperation Stack + +The teleoperation policy primarily uses Pico controllers for coordinated hand and body control. It also supports other teleoperation devices, including LeapMotion and HTC Vive with Nintendo Switch Joy-Con controllers. + +Keep `run_g1_control_loop.py` running, and in another terminal run: + +```bash +python gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py --hand_control_device=pico --body_control_device=pico +``` + +### Pico Setup and Controls +Configure the teleop app on your Pico headset by following the [XR Robotics guidelines](https://github.com/XR-Robotics). + +The necessary PC software is pre-installed in the Docker container. Only the [XRoboToolkit-PC-Service](https://github.com/XR-Robotics/XRoboToolkit-PC-Service) component is needed. + +Prerequisites: Connect the Pico to the same network as the host computer. + +Controller bindings: +- `menu + left trigger`: Toggle lower-body policy +- `menu + right trigger`: Toggle upper-body policy +- `Left stick`: X/Y translation +- `Right stick`: Yaw rotation +- `L/R triggers`: Control hand grippers + +Pico unit test: +```bash +python gr00t_wbc/control/teleop/streamers/pico_streamer.py +``` + +--- + +## Running the Data Collection Stack + +Run the full stack (control loop, teleop policy, and camera forwarder) via the deployment helper: +```bash +python scripts/deploy_g1.py \ + --interface sim \ + --camera_host localhost \ + --sim_in_single_process \ + --simulator robocasa \ + --image-publish \ + --enable-offscreen \ + --env_name PnPBottle \ + --hand_control_device=pico \ + --body_control_device=pico +``` + +The `tmux` session `g1_deployment` is created with panes for: +- `control_data_teleop`: Main control loop, data collection, and teleoperation policy +- `camera`: Camera forwarder +- `camera_viewer`: Optional live camera feed + +Operations in the `controller` window (`control_data_teleop` pane, left): +- `]`: Activate policy +- `o`: Deactivate policy +- `k`: Reset the simulation and policies +- `` ` ``: Terminate the tmux session +- `ctrl + d`: Exit the shell in the pane + +Operations in the `data exporter` window (`control_data_teleop` pane, right top): +- Enter the task prompt + +Operations on Pico controllers: +- `A`: Start/Stop recording +- `B`: Discard trajectory diff --git a/docker/.bashrc b/docker/.bashrc new file mode 100644 index 0000000..ecd18c6 --- /dev/null +++ b/docker/.bashrc @@ -0,0 +1,160 @@ +# ~/.bashrc: executed by bash(1) for non-login shells. +# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) +# for examples + +# If not running interactively, don't do anything +case $- in + *i*) ;; + *) return;; +esac + +# don't put duplicate lines or lines starting with space in the history. +# See bash(1) for more options +HISTCONTROL=ignoreboth + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=1000 +HISTFILESIZE=2000 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# If set, the pattern "**" used in a pathname expansion context will +# match all files and zero or more directories and subdirectories. +#shopt -s globstar + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color|*-256color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + #alias dir='dir --color=auto' + #alias vdir='vdir --color=auto' + + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# colored GCC warnings and errors +export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01' + +# Set terminal type for color support +export TERM=xterm-256color + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' + +# Add an "alert" alias for long running commands. Use like so: +# sleep 10; alert +alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' + +# Alias definitions. +# You may want to put all your additions into a separate file like +# ~/.bash_aliases, instead of adding them here directly. +# See /usr/share/doc/bash-doc/examples in the bash-doc package. + +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +if ! shopt -oq posix; then + if [ -f /usr/share/bash-completion/bash_completion ]; then + . /usr/share/bash-completion/bash_completion + elif [ -f /etc/bash_completion ]; then + . /etc/bash_completion + fi +fi + +# useful commands +bind '"\e[A": history-search-backward' +bind '"\e[B": history-search-forward' + +# Store the last 10 directories in a history file +CD_HISTFILE=~/.cd_history +CD_HISTSIZE=10 + +cd() { + local histfile="${CD_HISTFILE:-$HOME/.cd_history}" + local max="${CD_HISTSIZE:-10}" + + case "$1" in + --) [ -f "$histfile" ] && tac "$histfile" | nl -w2 -s' ' || echo "No directory history yet."; return ;; + -[0-9]*) + local idx=${1#-} + local dir=$(tac "$histfile" 2>/dev/null | sed -n "${idx}p") + [ -n "$dir" ] && builtin cd "$dir" || echo "Invalid selection: $1" + return ;; + esac + + builtin cd "$@" || return + + [[ $(tail -n1 "$histfile" 2>/dev/null) != "$PWD" ]] && echo "$PWD" >> "$histfile" + tail -n "$max" "$histfile" > "${histfile}.tmp" && mv "${histfile}.tmp" "$histfile" +} + +# Manus to LD_LIBRARY_PATH +export LD_LIBRARY_PATH=$GR00T_WBC_DIR/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib:$LD_LIBRARY_PATH + +# CUDA support +export LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:/usr/lib:/lib/x86_64-linux-gnu:/lib64:/lib:$LD_LIBRARY_PATH + +# gr00t_wbc aliases +alias dg="python $GR00T_WBC_DIR/scripts/deploy_g1.py" +alias rsl="python $GR00T_WBC_DIR/gr00t_wbc/control/main/teleop/run_sim_loop.py" +alias rgcl="python $GR00T_WBC_DIR/gr00t_wbc/control/main/teleop/run_g1_control_loop.py" +alias rtpl="python $GR00T_WBC_DIR/gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py" +alias tgcl="pytest $GR00T_WBC_DIR/tests/control/main/teleop/test_g1_control_loop.py -s" diff --git a/docker/.tmux.conf b/docker/.tmux.conf new file mode 100644 index 0000000..0da4a31 --- /dev/null +++ b/docker/.tmux.conf @@ -0,0 +1,84 @@ +# Enable mouse mode +set -g mouse on + +# Start window numbering at 0 (default) +set -g base-index 0 +setw -g pane-base-index 0 + +# Increase scrollback buffer size +set -g history-limit 50000 + +# Use Alt-arrow keys without prefix key to switch panes +bind -n M-Left select-pane -L +bind -n M-Right select-pane -R +bind -n M-Up select-pane -U +bind -n M-Down select-pane -D + +# Use Alt-1,2,3... to switch windows +bind -n M-1 select-window -t 1 +bind -n M-2 select-window -t 2 +bind -n M-3 select-window -t 3 +bind -n M-4 select-window -t 4 +bind -n M-5 select-window -t 5 +bind -n M-6 select-window -t 6 +bind -n M-7 select-window -t 7 +bind -n M-8 select-window -t 8 +bind -n M-9 select-window -t 9 + +# Split panes using Alt-| and Alt-- +bind -n M-| split-window -h +bind -n M-- split-window -v + +# Easy config reload +bind -n M-r source-file ~/.tmux.conf \; display-message "Config reloaded!" + +# Status bar customization +set -g status-style bg=colour240,fg=colour255 +set -g status-left "#[fg=colour255,bg=colour240] #S #[fg=colour240,bg=colour238]" +set -g status-right "#[fg=colour255,bg=colour240] %H:%M #[fg=colour240,bg=colour238]" + +# Window status format +setw -g window-status-format "#[fg=colour255,bg=colour238] #I:#W " +setw -g window-status-current-format "#[fg=colour238,bg=colour255]#[fg=colour238,bg=colour255] #I:#W #[fg=colour255,bg=colour238]" + +# Pane border colors +set -g pane-border-style fg=colour240 +set -g pane-active-border-style fg=colour255 + +# Message text +set -g message-style bg=colour238,fg=colour255 + +# Clock mode +setw -g clock-mode-colour colour255 + +# Enable focus events +set -g focus-events on + +# Increase escape time +set -sg escape-time 0 + +# Enable true color support +set -ga terminal-overrides ",*256col*:Tc" + +# Set default terminal mode to 256 colors +set -g default-terminal "screen-256color" + +# Display a message when a window is created +set -g display-time 4000 + +# Automatically set window title +setw -g automatic-rename on +set -g set-titles on +set -g set-titles-string "#T" + +# Enable clipboard integration +set -g @plugin 'tmux-plugins/tmux-yank' + +# List of plugins +set -g @plugin 'tmux-plugins/tpm' +set -g @plugin 'tmux-plugins/tmux-sensible' +set -g @plugin 'tmux-plugins/tmux-resurrect' +set -g @plugin 'tmux-plugins/tmux-continuum' + +# Initialize TMUX plugin manager (keep this line at the very bottom of tmux.conf) +run '~/.tmux/plugins/tpm/tpm' \ No newline at end of file diff --git a/docker/70-manus-hid.rules b/docker/70-manus-hid.rules new file mode 100644 index 0000000..3c2bb32 --- /dev/null +++ b/docker/70-manus-hid.rules @@ -0,0 +1,5 @@ + # HIDAPI/libusb + SUBSYSTEMS=="usb", ATTRS{idVendor}=="3325", MODE:="0666" + + # HIDAPI/hidraw + KERNEL=="hidraw*", ATTRS{idVendor}=="3325", MODE:="0666" \ No newline at end of file diff --git a/docker/Dockerfile.deploy b/docker/Dockerfile.deploy new file mode 100644 index 0000000..7ba55e3 --- /dev/null +++ b/docker/Dockerfile.deploy @@ -0,0 +1,127 @@ +FROM nvgear/ros-2:latest + +# Accept build argument for username +ARG USERNAME +ARG USERID +ARG HOME_DIR +ARG WORKTREE_NAME + +# Create user with the same name as host +RUN if [ "$USERID" != "0" ]; then \ + useradd -m -u ${USERID} -s /bin/bash ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers && \ + # Add user to video and render groups for GPU access + usermod -a -G video,render ${USERNAME} || true; \ + fi + +# Copy .bashrc with color settings before switching user +COPY --chown=${USERNAME}:${USERNAME} docker/.bashrc ${HOME_DIR}/.bashrc + +# Install Manus udev rules +COPY --chown=${USERNAME}:${USERNAME} docker/70-manus-hid.rules /etc/udev/rules.d/70-manus-hid.rules + +# Copy tmux configuration +COPY --chown=${USERNAME}:${USERNAME} docker/.tmux.conf ${HOME_DIR}/.tmux.conf + +# Switch to user +USER ${USERNAME} + +# Install tmux plugin manager and uv in parallel +RUN git clone https://github.com/tmux-plugins/tpm ${HOME_DIR}/.tmux/plugins/tpm & \ + curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=${HOME_DIR}/.cargo/bin sh & \ + wait + +# Install tmux plugins automatically +RUN ${HOME_DIR}/.tmux/plugins/tpm/bin/install_plugins || true + +# Add uv to PATH +ENV PATH="${HOME_DIR}/.cargo/bin:$PATH" +ENV UV_PYTHON=${HOME_DIR}/venv/bin/python + +# Create venv +RUN uv venv --python 3.10 ${HOME_DIR}/venv + +# Install hardware-specific packages (x86 only - not available on ARM64/Orin) +USER root +COPY --chown=${USERNAME}:${USERNAME} gr00t_wbc/control/teleop/device/pico/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb ${HOME_DIR}/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb +COPY --chown=${USERNAME}:${USERNAME} gr00t_wbc/control/teleop/device/pico/roboticsservice_1.0.0.0_arm64.deb ${HOME_DIR}/roboticsservice_1.0.0.0_arm64.deb + +RUN if [ "$(dpkg --print-architecture)" = "amd64" ]; then \ + # Ultra Leap setup + wget -qO - https://repo.ultraleap.com/keys/apt/gpg | gpg --dearmor | tee /etc/apt/trusted.gpg.d/ultraleap.gpg && \ + echo 'deb [arch=amd64] https://repo.ultraleap.com/apt stable main' | tee /etc/apt/sources.list.d/ultraleap.list && \ + apt-get update && \ + echo "yes" | DEBIAN_FRONTEND=noninteractive apt-get install -y ultraleap-hand-tracking libhidapi-dev && \ + # Space Mouse udev rules + echo 'KERNEL=="hidraw*", SUBSYSTEM=="hidraw", MODE="0664", GROUP="plugdev"' > /etc/udev/rules.d/99-hidraw-permissions.rules && \ + usermod -aG plugdev ${USERNAME}; \ + # Pico setup + apt-get install -y xdg-utils && \ + dpkg -i ${HOME_DIR}/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb; \ + else \ + echo "Skipping x86-only hardware packages on $(dpkg --print-architecture)"; \ + fi + +USER ${USERNAME} +# Install hardware Python packages (x86 only) with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + if [ "$(dpkg --print-architecture)" = "amd64" ]; then \ + # Ultra Leap Python bindings + git clone https://github.com/ultraleap/leapc-python-bindings ${HOME_DIR}/leapc-python-bindings && \ + cd ${HOME_DIR}/leapc-python-bindings && \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -r requirements.txt && \ + MAKEFLAGS="-j$(nproc)" ${HOME_DIR}/venv/bin/python -m build leapc-cffi && \ + uv pip install leapc-cffi/dist/leapc_cffi-0.0.1.tar.gz && \ + uv pip install -e leapc-python-api && \ + # Space Mouse Python package + uv pip install pyspacemouse && \ + # Pico Python bindings + git clone https://github.com/XR-Robotics/XRoboToolkit-PC-Service-Pybind.git ${HOME_DIR}/XRoboToolkit-PC-Service-Pybind && \ + cd ${HOME_DIR}/XRoboToolkit-PC-Service-Pybind && \ + uv pip install setuptools pybind11 && \ + sed -i "s|pip install|uv pip install|g" setup_ubuntu.sh && \ + sed -i "s|pip uninstall|uv pip uninstall|g" setup_ubuntu.sh && \ + sed -i "s|python setup.py install|${HOME_DIR}/venv/bin/python setup.py install|g" setup_ubuntu.sh && \ + bash setup_ubuntu.sh; \ + fi + +# Install Python dependencies using uv with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install --upgrade pip ipython jupyter notebook debugpy + + +# Copy entire project to the workspace directory where it will be mounted at runtime +# NOTE: The build context must be the project root for this to work +# Use dynamic worktree name to match runtime mount path +COPY --chown=${USERNAME}:${USERNAME} . ${HOME_DIR}/Projects/${WORKTREE_NAME} + +# Install Python dependencies inside the venv with caching - split into separate commands +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install \ + -e ${HOME_DIR}/Projects/${WORKTREE_NAME}/external_dependencies/unitree_sdk2_python + +# Unlike pip, uv downloads LFS files by default. There's a bug in uv that causes LFS files +# to fail to download (https://github.com/astral-sh/uv/issues/3312). So we need to set +# UV_GIT_LFS=1 to prevent uv from downloading LFS files. +# Install project dependencies (VLA extras only on x86) with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + GIT_LFS_SKIP_SMUDGE=1 UV_CONCURRENT_DOWNLOADS=8 uv pip install -e "${HOME_DIR}/Projects/${WORKTREE_NAME}[dev]" + +# Clone and install robosuite with specific branch +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + git clone https://github.com/xieleo5/robosuite.git ${HOME_DIR}/robosuite && \ + cd ${HOME_DIR}/robosuite && \ + git checkout leo/support_g1_locomanip && \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -e . + +# Install gr00trobocasa +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -e ${HOME_DIR}/Projects/${WORKTREE_NAME}/gr00t_wbc/dexmg/gr00trobocasa + +# Configure bash environment with virtual environment and ROS2 setup +RUN echo "source ${HOME_DIR}/venv/bin/activate" >> ${HOME_DIR}/.bashrc && \ + echo "source /opt/ros/humble/setup.bash" >> ${HOME_DIR}/.bashrc && \ + echo "export ROS_LOCALHOST_ONLY=1" >> ${HOME_DIR}/.bashrc + +# Default command (can be overridden at runtime) +CMD ["/bin/bash"] diff --git a/docker/Dockerfile.deploy.base b/docker/Dockerfile.deploy.base new file mode 100644 index 0000000..360d2c5 --- /dev/null +++ b/docker/Dockerfile.deploy.base @@ -0,0 +1,155 @@ +# Multi-architecture Dockerfile for NVIDIA CUDA +# Supports linux/amd64 and linux/arm64 +FROM nvidia/cuda:12.4.0-runtime-ubuntu22.04 + +# Build info - ARGs need to be redeclared after FROM to use in RUN commands +ARG TARGETPLATFORM +ARG BUILDPLATFORM +RUN echo "Building for $TARGETPLATFORM on $BUILDPLATFORM" + +# Avoid prompts from apt +ENV DEBIAN_FRONTEND=noninteractive + +# Install minimal system dependencies +RUN apt-get update && \ + apt-get install -y \ + # Basic tools + build-essential \ + curl \ + gdb \ + git \ + git-lfs \ + net-tools \ + sudo \ + wget \ + iputils-ping \ + vim \ + unzip \ + # System services + udev \ + # Graphics and X11 + libgl1-mesa-dri \ + libgl1-mesa-glx \ + libglu1-mesa \ + mesa-utils \ + libxcb-cursor0 \ + x11-apps \ + xauth \ + # EGL and GPU access + libegl1 \ + libegl1-mesa \ + libegl1-mesa-dev \ + libgl1-mesa-dev \ + libgles2-mesa-dev \ + libglvnd-dev \ + mesa-common-dev \ + # XCB and Qt platform dependencies + libxcb-icccm4 \ + libxcb-image0 \ + libxcb-keysyms1 \ + libxcb-randr0 \ + libxcb-render-util0 \ + libxcb-shape0 \ + libxcb-xfixes0 \ + libxcb-xinerama0 \ + libxcb-xinput0 \ + libxcb-xkb1 \ + libxkbcommon-x11-0 \ + # D-Bus and system dependencies + libdbus-1-3 \ + # Other dependencies + libncurses5-dev \ + libudev-dev \ + libusb-1.0-0-dev \ + # Python 3.10 and pip + python3.10 \ + python3.10-venv \ + python3.10-distutils \ + python3-pip \ + expect \ + # ffmpeg and related libraries + ffmpeg \ + libavcodec-dev \ + libavformat-dev \ + libavdevice-dev \ + libavfilter-dev \ + libavutil-dev \ + libswresample-dev \ + libswscale-dev \ + # opencv + libgtk2.0-dev \ + # Clean up + && rm -rf /var/lib/apt/lists/* + +# --- Install ROS 2 Humble (following official instructions) --- +# Enable required repositories +RUN apt-get update && apt-get install -y software-properties-common \ + && add-apt-repository universe \ + # Add ROS 2 GPG key and repository + && apt-get install -y curl gnupg lsb-release \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" > /etc/apt/sources.list.d/ros2.list \ + # Upgrade system to avoid removal of critical packages (see ROS 2 docs) + && apt-get update \ + && apt-get upgrade -y \ + # Install ROS 2 Humble desktop + && apt-get install -y ros-humble-desktop \ + # (Optional) Install development tools + && apt-get install -y ros-dev-tools \ + # Install Eclipse Cyclone DDS RMW implementation + && apt-get install -y ros-humble-rmw-cyclonedds-cpp \ + # Clean up + && rm -rf /var/lib/apt/lists/* + +# Source ROS 2 setup in bashrc for all users +RUN echo 'source /opt/ros/humble/setup.bash' >> /etc/bash.bashrc + +# Clone, build, and install CycloneDDS 0.10.x +RUN git clone --branch releases/0.10.x https://github.com/eclipse-cyclonedds/cyclonedds /opt/cyclonedds && \ + mkdir -p /opt/cyclonedds/build /opt/cyclonedds/install && \ + cd /opt/cyclonedds/build && \ + cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ + cmake --build . --target install && \ + # Clean up build files to reduce image size + rm -rf /opt/cyclonedds/build + +# Set CYCLONEDDS_HOME for all users +ENV CYCLONEDDS_HOME=/opt/cyclonedds/install + +# Install uv +RUN curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=/opt/uv sh + +# Add uv to PATH +ENV PATH="/opt/uv:$PATH" +ENV UV_PYTHON=/opt/venv/bin/python + +# Fix dpkg state and install tmux +RUN dpkg --configure -a && apt-get update && apt-get install -y tmux && rm -rf /var/lib/apt/lists/* + +# Create NVIDIA ICD config for EGL if it doesn't exist +# Note: This might need platform-specific handling for ARM64 +RUN if [ ! -f /usr/share/glvnd/egl_vendor.d/10_nvidia.json ]; then \ + mkdir -p /usr/share/glvnd/egl_vendor.d && \ + printf '{\n "file_format_version" : "1.0.0",\n "ICD" : {\n "library_path" : "libEGL_nvidia.so.0"\n }\n}' | tee /usr/share/glvnd/egl_vendor.d/10_nvidia.json > /dev/null; \ + fi + +# Platform-specific configurations +RUN case "$TARGETPLATFORM" in \ + "linux/arm64") \ + echo "Configuring for ARM64 platform" && \ + # Add any ARM64-specific configurations here + echo "export GPU_FORCE_64BIT_PTR=1" >> /etc/environment \ + ;; \ + "linux/amd64") \ + echo "Configuring for AMD64 platform" && \ + # Add any AMD64-specific configurations here + echo "AMD64 platform configured" \ + ;; \ + *) \ + echo "Unknown platform: $TARGETPLATFORM" \ + ;; \ + esac + +# Add labels for better image management +LABEL org.opencontainers.image.title="Multi-Arch CUDA Runtime with ROS 2 Humble" +LABEL org.opencontainers.image.description="Multi-architecture Docker image with CUDA runtime and ROS 2 Humble" diff --git a/docker/build_deploy_base.sh b/docker/build_deploy_base.sh new file mode 100644 index 0000000..6c1ddb6 --- /dev/null +++ b/docker/build_deploy_base.sh @@ -0,0 +1,55 @@ +#!/bin/bash + +# Multi-architecture Docker build script +# Supports linux/amd64 + +set -e + +# Configuration +IMAGE_NAME="nvgear/ros-2" +TAG="${1:-latest}" +DOCKERFILE="docker/Dockerfile.deploy.base" + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +echo -e "${GREEN}Building multi-architecture Docker image: ${IMAGE_NAME}:${TAG}${NC}" + +# Ensure we're using the multiarch builder +echo -e "${YELLOW}Setting up multiarch builder...${NC}" +sudo docker buildx use multiarch-builder 2>/dev/null || { + echo -e "${YELLOW}Creating multiarch builder...${NC}" + sudo docker buildx create --name multiarch-builder --use --bootstrap +} + +# Show supported platforms +echo -e "${YELLOW}Supported platforms:${NC}" +sudo docker buildx inspect --bootstrap | grep Platforms + +# Build for multiple architectures +echo -e "${GREEN}Starting multi-arch build...${NC}" +sudo docker buildx build \ + --platform linux/amd64 \ + --file "${DOCKERFILE}" \ + --tag "${IMAGE_NAME}:${TAG}" \ + --push \ + . + +# Alternative: Build and load locally (only works for single platform) +# docker buildx build \ +# --platform linux/amd64 \ +# --file "${DOCKERFILE}" \ +# --tag "${IMAGE_NAME}:${TAG}" \ +# --load \ +# . + +echo -e "${GREEN}Multi-arch build completed successfully!${NC}" +echo -e "${GREEN}Image: ${IMAGE_NAME}:${TAG}${NC}" +echo -e "${GREEN}Platforms: linux/amd64${NC}" + +# Verify the manifest +echo -e "${YELLOW}Verifying multi-arch manifest...${NC}" +sudo docker buildx imagetools inspect "${IMAGE_NAME}:${TAG}" \ No newline at end of file diff --git a/docker/entrypoint/bash.sh b/docker/entrypoint/bash.sh new file mode 100755 index 0000000..1142dd8 --- /dev/null +++ b/docker/entrypoint/bash.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -e # Exit on error + +echo "Dependencies installed successfully. Starting interactive bash shell..." +exec /bin/bash \ No newline at end of file diff --git a/docker/entrypoint/deploy.sh b/docker/entrypoint/deploy.sh new file mode 100755 index 0000000..216fbc2 --- /dev/null +++ b/docker/entrypoint/deploy.sh @@ -0,0 +1,20 @@ +#!/bin/bash +set -e # Exit on error + +# Run the deployment script +# Check for script existence before running +DEPLOY_SCRIPT="scripts/deploy_g1.py" +if [ -f "$DEPLOY_SCRIPT" ]; then + echo "Running deployment script at $DEPLOY_SCRIPT" + echo "Using python from $(which python)" + echo "Deploy args: $@" + exec python "$DEPLOY_SCRIPT" "$@" +else + echo "ERROR: Deployment script not found at $DEPLOY_SCRIPT" + echo "Current directory structure:" + find . -type f -name "*.py" | grep -i deploy + echo "Available script options:" + find . -type f -name "*.py" | sort + echo "Starting a bash shell for troubleshooting..." + exec /bin/bash +fi \ No newline at end of file diff --git a/docker/entrypoint/install_deps.sh b/docker/entrypoint/install_deps.sh new file mode 100755 index 0000000..cb602fb --- /dev/null +++ b/docker/entrypoint/install_deps.sh @@ -0,0 +1,23 @@ +#!/bin/bash +set -e + +# Source virtual environment and ROS2 +source ${HOME}/venv/bin/activate +source /opt/ros/humble/setup.bash +export ROS_LOCALHOST_ONLY=1 + +# Install external dependencies +echo "Current directory: $(pwd)" +echo "Installing dependencies..." + +# Install Unitree SDK and LeRobot +if [ -d "external_dependencies/unitree_sdk2_python" ]; then + cd external_dependencies/unitree_sdk2_python/ + uv pip install -e . --no-deps + cd ../.. +fi + +# Install project +if [ -f "pyproject.toml" ]; then + UV_GIT_LFS=1 uv pip install -e ".[dev]" +fi diff --git a/docker/image_name.txt b/docker/image_name.txt new file mode 100644 index 0000000..10b5635 --- /dev/null +++ b/docker/image_name.txt @@ -0,0 +1 @@ +nvcr.io/nvidian/gr00t_wbc:base diff --git a/docker/kill_all_containers.sh b/docker/kill_all_containers.sh new file mode 100755 index 0000000..1043070 --- /dev/null +++ b/docker/kill_all_containers.sh @@ -0,0 +1,79 @@ +#!/bin/bash + +# Script to kill all running Docker containers +# Usage: ./kill_all_containers.sh [--force] + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Function to print colored output +print_info() { + echo -e "${GREEN}[INFO]${NC} $1" +} + +print_warning() { + echo -e "${YELLOW}[WARNING]${NC} $1" +} + +print_error() { + echo -e "${RED}[ERROR]${NC} $1" +} + +# Check if Docker is running +if ! sudo docker info >/dev/null 2>&1; then + print_error "Docker is not running or not accessible. Please start Docker first." + exit 1 +fi + +# Get list of running containers +RUNNING_CONTAINERS=$(sudo docker ps -q) + +if [ -z "$RUNNING_CONTAINERS" ]; then + print_info "No running containers found." + exit 0 +fi + +# Count running containers +CONTAINER_COUNT=$(echo "$RUNNING_CONTAINERS" | wc -l | tr -d ' ') + +print_info "Found $CONTAINER_COUNT running container(s):" +sudo docker ps --format "table {{.ID}}\t{{.Image}}\t{{.Names}}\t{{.Status}}" + +# Check for --force flag +FORCE_KILL=false +if [ "$1" = "--force" ]; then + FORCE_KILL=true + print_warning "Force mode enabled. Containers will be killed without confirmation." +fi + +# Ask for confirmation unless --force is used +if [ "$FORCE_KILL" = false ]; then + echo + read -p "Are you sure you want to kill all running containers? (y/N): " -n 1 -r + echo + if [[ ! $REPLY =~ ^[Yy]$ ]]; then + print_info "Operation cancelled." + exit 0 + fi +fi + +# Kill all running containers +print_info "Killing all running containers..." +if sudo docker kill $RUNNING_CONTAINERS; then + print_info "Successfully killed all running containers." +else + print_error "Failed to kill some containers. You may need to run with sudo or check Docker permissions." + exit 1 +fi + +# Optional: Remove stopped containers (commented out by default) +# Uncomment the following lines if you also want to remove the stopped containers +# print_info "Removing stopped containers..." +# sudo docker container prune -f + +print_info "Done!" \ No newline at end of file diff --git a/docker/kill_gr00t_wbc_processors.sh b/docker/kill_gr00t_wbc_processors.sh new file mode 100755 index 0000000..03d0819 --- /dev/null +++ b/docker/kill_gr00t_wbc_processors.sh @@ -0,0 +1,192 @@ +#!/bin/bash + +# kill_gr00t_wbc_processors.sh +# Kill gr00t_wbc processes in current container to prevent message passing conflicts + +# Note: Don't use 'set -e' as tmux/pgrep commands may return non-zero exit codes + +# Configuration +DRY_RUN=false +FORCE=false +QUIET=false +declare -A FOUND_PROCESSES + +# Default to verbose mode if no arguments +[[ $# -eq 0 ]] && { QUIET=false; DRY_RUN=false; } + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + --dry-run) DRY_RUN=true ;; + --force) FORCE=true ;; + --verbose|-v) VERBOSE=true ;; + --help|-h) + echo "Usage: $0 [--dry-run] [--force] [--verbose] [--help]" + echo "Kill gr00t_wbc processes to prevent message passing conflicts" + exit 0 ;; + *) echo "Unknown option: $1"; exit 1 ;; + esac + shift +done + +# Colors (only if not quiet) +if [[ "$QUIET" != true ]]; then + RED='\033[0;31m'; GREEN='\033[0;32m'; YELLOW='\033[1;33m'; BLUE='\033[0;34m'; NC='\033[0m' +else + RED=''; GREEN=''; YELLOW=''; BLUE=''; NC='' +fi + +# Show processes by pattern (for preview) +show_processes_by_pattern() { + local pattern="$1" desc="$2" + local pids=$(pgrep -f "$pattern" 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + echo -e "${YELLOW}$desc processes:${NC}" + + for pid in $pids; do + local cmd=$(ps -p $pid -o cmd= 2>/dev/null || echo "Process not found") + echo " PID $pid: $cmd" + done +} + +# Kill processes by pattern (silent killing) +kill_by_pattern() { + local pattern="$1" desc="$2" signal="${3:-TERM}" + local pids=$(pgrep -f "$pattern" 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + for pid in $pids; do + # Kill if not dry run + [[ "$DRY_RUN" != true ]] && kill -$signal $pid 2>/dev/null + done +} + +# Show tmux sessions (for preview) +show_tmux() { + local pattern="$1" + local sessions=$(tmux list-sessions 2>/dev/null | grep "$pattern" | cut -d: -f1 || true) + + [[ -z "$sessions" ]] && return 0 + + echo -e "${YELLOW}Tmux sessions:${NC}" + + for session in $sessions; do + echo " Session: $session" + done +} + +# Kill tmux sessions (silent killing) +kill_tmux() { + local pattern="$1" + local sessions=$(tmux list-sessions 2>/dev/null | grep "$pattern" | cut -d: -f1 || true) + + [[ -z "$sessions" ]] && return 0 + + for session in $sessions; do + [[ "$DRY_RUN" != true ]] && tmux kill-session -t "$session" 2>/dev/null + done +} + +# Show processes by port (for preview) +show_processes_by_port() { + local port="$1" desc="$2" + local pids=$(lsof -ti:$port 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + echo -e "${YELLOW}$desc (port $port):${NC}" + + for pid in $pids; do + local cmd=$(ps -p $pid -o cmd= 2>/dev/null || echo "Process not found") + echo " PID $pid: $cmd" + done +} + +# Kill processes by port (silent killing) +kill_by_port() { + local port="$1" desc="$2" + local pids=$(lsof -ti:$port 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + for pid in $pids; do + [[ "$DRY_RUN" != true ]] && kill -TERM $pid 2>/dev/null + done +} + +# Check if any processes exist +has_processes() { + # Check for processes + local has_tmux=$(tmux list-sessions 2>/dev/null | grep "g1_deployment" || true) + local has_control=$(pgrep -f "run_g1_control_loop.py" 2>/dev/null || true) + local has_teleop=$(pgrep -f "run_teleop_policy_loop.py" 2>/dev/null || true) + local has_camera=$(pgrep -f "camera_forwarder.py" 2>/dev/null || true) + local has_rqt=$(pgrep -f "rqt.*image_view" 2>/dev/null || true) + local has_port=$(lsof -ti:5555 2>/dev/null || true) + + [[ -n "$has_tmux" || -n "$has_control" || -n "$has_teleop" || -n "$has_camera" || -n "$has_rqt" || -n "$has_port" ]] +} + +# Main execution +main() { + # Check if any processes exist first + if ! has_processes; then + # No processes to kill, exit silently + exit 0 + fi + + # Show header and processes to be killed + if [[ "$QUIET" != true ]]; then + echo -e "${BLUE}=== gr00t_wbc Process Killer ===${NC}" + [[ "$DRY_RUN" == true ]] && echo -e "${BLUE}=== DRY RUN MODE ===${NC}" + + # Show what will be killed + show_tmux "g1_deployment" + show_processes_by_pattern "run_g1_control_loop.py" "G1 control loop" + show_processes_by_pattern "run_teleop_policy_loop.py" "Teleop policy" + show_processes_by_pattern "camera_forwarder.py" "Camera forwarder" + show_processes_by_pattern "rqt.*image_view" "RQT viewer" + show_processes_by_port "5555" "Inference server" + + # Ask for confirmation + if [[ "$FORCE" != true && "$DRY_RUN" != true ]]; then + echo + echo -e "${RED}WARNING: This will terminate the above gr00t_wbc processes!${NC}" + read -p "Continue? [Y/n]: " -n 1 -r + echo + # Default to Y - only abort if user explicitly types 'n' or 'N' + [[ $REPLY =~ ^[Nn]$ ]] && { echo "Aborted."; exit 0; } + fi + echo + fi + + # Kill processes (silently) + kill_tmux "g1_deployment" + kill_by_pattern "run_g1_control_loop.py" "G1 control loop" + kill_by_pattern "run_teleop_policy_loop.py" "Teleop policy" + kill_by_pattern "camera_forwarder.py" "Camera forwarder" + kill_by_pattern "rqt.*image_view" "RQT viewer" + kill_by_port "5555" "Inference server" + + # Force kill remaining (SIGKILL) + [[ "$DRY_RUN" != true ]] && { + sleep 1 + kill_by_pattern "run_g1_control_loop.py" "G1 control loop" "KILL" + kill_by_pattern "run_teleop_policy_loop.py" "Teleop policy" "KILL" + kill_by_pattern "camera_forwarder.py" "Camera forwarder" "KILL" + } + + # Summary (unless quiet) + [[ "$QUIET" != true ]] && { + if [[ "$DRY_RUN" == true ]]; then + echo -e "${BLUE}=== DRY RUN COMPLETE ===${NC}" + else + echo -e "${GREEN}All gr00t_wbc processes terminated${NC}" + fi + } +} + +main "$@" diff --git a/docker/publish.sh b/docker/publish.sh new file mode 100644 index 0000000..a9d909e --- /dev/null +++ b/docker/publish.sh @@ -0,0 +1,3 @@ +#!/bin/bash +image_name=$(cat image_name.txt) +docker push "$@" $image_name \ No newline at end of file diff --git a/docker/run_docker.sh b/docker/run_docker.sh new file mode 100755 index 0000000..cca4621 --- /dev/null +++ b/docker/run_docker.sh @@ -0,0 +1,454 @@ +#!/bin/bash + +# Docker run script for gr00t_wbc with branch-based container isolation +# +# Usage: +# ./docker/run_docker.sh [OPTIONS] +# +# Options: +# --build Build Docker image +# --clean Clean containers +# --deploy Run in deploy mode +# --install Pull prebuilt Docker image +# --push Push built image to Docker Hub +# --branch Use branch-specific container names +# +# Branch-based Container Isolation (when --branch flag is used): +# - Each git branch gets its own isolated containers +# - Container names include branch identifier (e.g., gr00t_wbc-deploy-user-main) +# - Works with git worktrees, separate clones, or nested repositories +# - Clean and build operations only affect the current branch + +# Exit on error +set -e + +# Default values +BUILD=false +CLEAN=false +DEPLOY=false +INSTALL=false +# Flag to push the built Docker image to Docker Hub +# This should be used when someone updates the Docker image dependencies +# because this image is used for CI/CD pipelines +# When true, the image will be tagged and pushed to docker.io/nvgear/gr00t_wbc:latest (lowercased in practice) +DOCKER_HUB_PUSH=false +# Flag to build the docker with root user +# This could cause some of your local files to be owned by root +# If you get error like "PermissionError: [Errno 13] Permission denied:" +# You can run `sudo chown -R $USER:$USER .` in local machine to fix it +ROOT=false +BRANCH_MODE=false +EXTRA_ARGS=() +PROJECT_NAME="gr00t_wbc" +PROJECT_SLUG=$(echo "$PROJECT_NAME" | tr '[:upper:]' '[:lower:]') +REMOTE_IMAGE="nvgear/${PROJECT_SLUG}:latest" + +# Parse command line arguments +while [[ $# -gt 0 ]]; do + case $1 in + --build) + BUILD=true + shift + ;; + --clean) + CLEAN=true + shift + ;; + --deploy) + DEPLOY=true + shift + ;; + --install) + INSTALL=true + shift + ;; + --push) + DOCKER_HUB_PUSH=true + shift + ;; + --root) + ROOT=true + shift + ;; + --branch) + BRANCH_MODE=true + shift + ;; + *) + # Collect all unknown arguments as extra args for the deployment script + EXTRA_ARGS+=("$1") + shift + ;; + esac +done + +if [ "$INSTALL" = true ] && [ "$BUILD" = true ]; then + echo "Cannot use --install and --build together. Choose one." + exit 1 +fi + + +# Function to get branch name for container naming +function get_branch_id { + # Check if we're in a git repository + if git rev-parse --is-inside-work-tree > /dev/null 2>&1; then + # Get current branch name (returns "HEAD" in detached state) + local branch_name=$(git rev-parse --abbrev-ref HEAD) + # Replace forward slashes with dashes for valid container names + echo "${branch_name//\//-}" + else + # Default: no branch identifier (not in git repo) + echo "" + fi +} + +# Architecture detection helpers +is_arm64() { [ "$(dpkg --print-architecture)" = "arm64" ]; } +is_amd64() { [ "$(dpkg --print-architecture)" = "amd64" ]; } + +# Get current user's username and UID +if [ "$ROOT" = true ]; then + USERNAME=root + USERID=0 + DOCKER_HOME_DIR=/root + CACHE_FROM=${PROJECT_SLUG}-deploy-cache-root +else + USERNAME=$(whoami) + USERID=$(id -u) + DOCKER_HOME_DIR=/home/${USERNAME} + CACHE_FROM=${PROJECT_SLUG}-deploy-cache +fi +# Get input group ID for device access +INPUT_GID=$(getent group input | cut -d: -f3) + +# Get script directory for path calculations +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# Function to get the actual project directory (worktree-aware) +function get_project_dir { + # For worktrees, use the actual worktree root path + if git rev-parse --is-inside-work-tree > /dev/null 2>&1; then + git rev-parse --show-toplevel + else + # Fallback to script-based detection + dirname "$SCRIPT_DIR" + fi +} + +# Get branch identifier +BRANCH_ID=$(get_branch_id) + +# Set project directory (needs to be after branch detection) +PROJECT_DIR="$(get_project_dir)" + +# Function to generate container name with optional branch support +function get_container_name { + local container_type="$1" + if [[ -n "$BRANCH_ID" ]] && [[ "$BRANCH_MODE" = true ]]; then + echo "${PROJECT_SLUG}-${container_type}-${USERNAME}-${BRANCH_ID}" + else + echo "${PROJECT_SLUG}-${container_type}-${USERNAME}" + fi +} + +# Set common variables used throughout the script +DEPLOY_CONTAINER=$(get_container_name "deploy") +BASH_CONTAINER=$(get_container_name "bash") +WORKTREE_NAME=$(basename "$PROJECT_DIR") + +# Debug output for branch detection +if [[ -n "$BRANCH_ID" ]] && [[ "$BRANCH_MODE" = true ]]; then + echo "Branch mode enabled - using branch: $BRANCH_ID" + echo "Project directory: $PROJECT_DIR" +elif [[ -n "$BRANCH_ID" ]]; then + echo "Branch mode disabled - using default containers" + echo "Project directory: $PROJECT_DIR" +else + echo "Running outside git repository" + echo "Project directory: $PROJECT_DIR" +fi + +# Get host's hostname and append -docker +HOSTNAME=$(hostname)-docker + +function clean_container { + echo "Cleaning up Docker containers..." + + # Stop containers + sudo docker stop $DEPLOY_CONTAINER 2>/dev/null || true + sudo docker stop $BASH_CONTAINER 2>/dev/null || true + # Remove containers + echo "Removing containers..." + sudo docker rm $DEPLOY_CONTAINER 2>/dev/null || true + sudo docker rm $BASH_CONTAINER 2>/dev/null || true + echo "Containers cleaned!" +} + + +# Function to install Docker Buildx if needed +function install_docker_buildx { + # Check if Docker Buildx is already installed + if sudo docker buildx version &> /dev/null; then + echo "Docker Buildx is already installed." + return 0 + fi + + echo "Installing Docker Buildx..." + + # Create directories and detect architecture + mkdir -p ~/.docker/cli-plugins/ && sudo mkdir -p /root/.docker/cli-plugins/ + ARCH=$(dpkg --print-architecture) + [[ "$ARCH" == "arm64" ]] && BUILDX_ARCH="linux-arm64" || BUILDX_ARCH="linux-amd64" + + # Get version (with fallback) + BUILDX_VERSION=$(curl -s https://api.github.com/repos/docker/buildx/releases/latest | grep tag_name | cut -d '"' -f 4) + BUILDX_VERSION=${BUILDX_VERSION:-v0.13.1} + + # Download and install for both user and root + curl -L "https://github.com/docker/buildx/releases/download/${BUILDX_VERSION}/buildx-${BUILDX_VERSION}.${BUILDX_ARCH}" -o ~/.docker/cli-plugins/docker-buildx + sudo cp ~/.docker/cli-plugins/docker-buildx /root/.docker/cli-plugins/docker-buildx + chmod +x ~/.docker/cli-plugins/docker-buildx && sudo chmod +x /root/.docker/cli-plugins/docker-buildx + + # Create builder + sudo docker buildx create --use --name mybuilder || true + sudo docker buildx inspect --bootstrap + + echo "Docker Buildx installation complete!" +} + +# Function to install NVIDIA Container Toolkit if needed +function install_nvidia_toolkit { + # Check if NVIDIA Container Toolkit is already installed + if command -v nvidia-container-toolkit &> /dev/null; then + echo "NVIDIA Container Toolkit is already installed." + return 0 + fi + + echo "Installing NVIDIA Container Toolkit..." + + # Add the package repositories + distribution=$(. /etc/os-release;echo $ID$VERSION_ID) + + # Check if GPG key exists and remove it if it does + if [ -f "/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg" ]; then + echo "Removing existing NVIDIA GPG key..." + sudo rm /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg + fi + + # Add new GPG key + curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg + + # Add repository + curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.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-runtime.list + + # Install nvidia-container-toolkit and docker if needed + sudo apt-get update + sudo apt-get install -y nvidia-container-toolkit + + # Install docker if not already installed + if ! command -v docker &> /dev/null; then + sudo apt-get install -y docker.io + fi + + # Configure Docker to use the NVIDIA runtime + sudo nvidia-ctk runtime configure --runtime=docker + + # Restart the Docker daemon + sudo systemctl restart docker + + echo "NVIDIA Container Toolkit installation complete!" +} + + +# Function to build Docker image for current branch +function build_docker_image { + echo "Building Docker image: $DEPLOY_CONTAINER" + + sudo docker buildx build \ + --build-arg USERNAME=$USERNAME \ + --build-arg USERID=$USERID \ + --build-arg HOME_DIR=$DOCKER_HOME_DIR \ + --build-arg WORKTREE_NAME=$WORKTREE_NAME \ + --cache-from $CACHE_FROM \ + -t $DEPLOY_CONTAINER \ + -f docker/Dockerfile.deploy \ + --load \ + . + + # Tag for persistent cache + # sudo docker tag $DEPLOY_CONTAINER $CACHE_FROM + echo "Docker image build complete!" +} + +# Build function +function build_with_cleanup { + echo "Building Docker image..." + echo "Removing existing containers and images..." + clean_container + # Tag for persistent cache before deleting the image + sudo docker tag $DEPLOY_CONTAINER $CACHE_FROM 2>/dev/null || true + sudo docker rmi $DEPLOY_CONTAINER 2>/dev/null || true + echo "Images cleaned!" + + install_docker_buildx + install_nvidia_toolkit + build_docker_image +} + +function install_remote_image { + echo "Installing Docker image from remote registry: $REMOTE_IMAGE" + echo "Removing existing containers to ensure a clean install..." + clean_container + sudo docker pull "$REMOTE_IMAGE" + sudo docker tag "$REMOTE_IMAGE" "$DEPLOY_CONTAINER" + sudo docker tag "$REMOTE_IMAGE" "$CACHE_FROM" 2>/dev/null || true + echo "Docker image install complete!" +} + +# Clean up if requested +if [ "$CLEAN" = true ]; then + clean_container + exit 0 +fi + +# Build if requested +if [ "$BUILD" = true ]; then + build_with_cleanup +fi + +if [ "$INSTALL" = true ]; then + install_remote_image +fi + +if [ "$DOCKER_HUB_PUSH" = true ]; then + echo "Pushing Docker image to Docker Hub: docker.io/nvgear/${PROJECT_SLUG}:latest" + sudo docker tag $DEPLOY_CONTAINER docker.io/nvgear/${PROJECT_SLUG}:latest + sudo docker push docker.io/nvgear/${PROJECT_SLUG}:latest + echo "Docker image pushed to Docker Hub!" + exit 0 +fi + +# Setup X11 display forwarding +setup_x11() { + # Set display if missing and X server available + if [ -z "$DISPLAY" ] && command -v xset >/dev/null 2>&1 && xset q >/dev/null 2>&1; then + export DISPLAY=:1 + echo "No DISPLAY set, using :1" + fi + + # Enable X11 forwarding if possible + if [ -n "$DISPLAY" ] && command -v xhost >/dev/null 2>&1 && xhost +local:docker 2>/dev/null; then + echo "X11 forwarding enabled" + return 0 + else + echo "Headless environment - X11 disabled" + export DISPLAY="" + return 1 + fi +} + +X11_ENABLED=false +setup_x11 && X11_ENABLED=true + +# Mount entire /dev directory for dynamic device access (including hidraw for joycon) +# This allows JoyCon controllers to be detected even when connected after container launch +sudo chmod g+r+w /dev/input/* + +# Detect GPU setup and set appropriate environment variables +echo "Detecting GPU setup..." +GPU_ENV_VARS="" + +# Check if we have both integrated and discrete GPUs (hybrid/Optimus setup) +HAS_AMD_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i amd | wc -l) +HAS_INTEL_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i intel | wc -l) +HAS_NVIDIA_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i nvidia | wc -l) + +if [[ "$HAS_INTEL_GPU" -gt 0 ]] || [[ "$HAS_AMD_GPU" -gt 0 ]] && [[ "$HAS_NVIDIA_GPU" -gt 0 ]]; then + echo "Detected hybrid GPU setup (Intel/AMD integrated + NVIDIA discrete)" + echo "Setting NVIDIA Optimus environment variables for proper rendering offload..." + GPU_ENV_VARS="-e __NV_PRIME_RENDER_OFFLOAD=1 \ + -e __VK_LAYER_NV_optimus=NVIDIA_only" +else + GPU_ENV_VARS="" +fi + +# Set GPU runtime based on architecture +if is_arm64; then + echo "Detected ARM64 architecture (Jetson Orin), using device access instead of nvidia runtime..." + GPU_RUNTIME_ARGS="--device /dev/nvidia0 --device /dev/nvidiactl --device /dev/nvidia-modeset --device /dev/nvidia-uvm --device /dev/nvidia-uvm-tools" +else + GPU_RUNTIME_ARGS="--gpus all --runtime=nvidia" +fi + +# Common Docker run parameters +DOCKER_RUN_ARGS="--hostname $HOSTNAME \ + --user $USERNAME \ + --group-add $INPUT_GID \ + $GPU_RUNTIME_ARGS \ + --ipc=host \ + --network=host \ + --privileged \ + --device=/dev \ + $GPU_ENV_VARS \ + -p 5678:5678 \ + -e DISPLAY=$DISPLAY \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=graphics,compute,utility \ + -e __GLX_VENDOR_LIBRARY_NAME=nvidia \ + -e USERNAME=$USERNAME \ + -e GR00T_WBC_DIR="$DOCKER_HOME_DIR/Projects/$WORKTREE_NAME" \ + -v /dev/bus/usb:/dev/bus/usb \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -v $HOME/.ssh:$DOCKER_HOME_DIR/.ssh \ + -v $HOME/.gear:$DOCKER_HOME_DIR/.gear \ + -v $HOME/.Xauthority:$DOCKER_HOME_DIR/.Xauthority \ + -v $PROJECT_DIR:$DOCKER_HOME_DIR/Projects/$(basename "$PROJECT_DIR") + --device /dev/snd \ + --group-add audio \ + -e PULSE_SERVER=unix:/run/user/$(id -u)/pulse/native \ + -v /run/user/$(id -u)/pulse/native:/run/user/$(id -u)/pulse/native \ + -v $HOME/.config/pulse/cookie:/home/$USERNAME/.config/pulse/cookie" + +# Check if RL mode first, then handle container logic +if [ "$DEPLOY" = true ]; then + # Deploy mode - use gr00t_wbc-deploy-${USERNAME} container + + # Always clean up old processes and create a new container + # Kill all gr00t_wbc processes across containers to prevent message passing conflicts + "$SCRIPT_DIR/kill_gr00t_wbc_processors.sh" + echo "Creating new deploy container..." + + # Clean up old processes and create a fresh deploy container + # Remove existing deploy container if it exists + if sudo docker ps -a --format '{{.Names}}' | grep -q "^$DEPLOY_CONTAINER$"; then + echo "Removing existing deploy container..." + sudo docker rm -f $DEPLOY_CONTAINER + fi + sudo docker run -it --rm $DOCKER_RUN_ARGS \ + -w $DOCKER_HOME_DIR/Projects/$WORKTREE_NAME \ + --name $DEPLOY_CONTAINER \ + $DEPLOY_CONTAINER \ + /bin/bash -ic 'exec "$0" "$@"' \ + "${DOCKER_HOME_DIR}/Projects/${WORKTREE_NAME}/docker/entrypoint/deploy.sh" \ + "${EXTRA_ARGS[@]}" +else + # Bash mode - use gr00t_wbc-bash-${USERNAME} container + if sudo docker ps -a --format '{{.Names}}' | grep -q "^$BASH_CONTAINER$"; then + echo "Bash container exists, starting it..." + sudo docker start $BASH_CONTAINER > /dev/null + sudo docker exec -it $BASH_CONTAINER /bin/bash + else + echo "Creating new bash container with auto-install gr00t_wbc..." + sudo docker run -it $DOCKER_RUN_ARGS \ + -w $DOCKER_HOME_DIR/Projects/$WORKTREE_NAME \ + --name $BASH_CONTAINER \ + $DEPLOY_CONTAINER \ + /bin/bash -ic 'exec "$0"' \ + "${DOCKER_HOME_DIR}/Projects/${WORKTREE_NAME}/docker/entrypoint/bash.sh" + fi +fi + +# Cleanup X11 permissions +$X11_ENABLED && xhost -local:docker 2>/dev/null diff --git a/external_dependencies/unitree_sdk2_python/.gitignore b/external_dependencies/unitree_sdk2_python/.gitignore new file mode 100644 index 0000000..001de9a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/.gitignore @@ -0,0 +1,39 @@ +# Generated by MacOS +.DS_Store + +# Generated by Windows +Thumbs.db + +# Applications +*.app +*.exe +*.war + +# Large media files +*.mp4 +*.tiff +*.avi +*.flv +*.mov +*.wmv +*.jpg +*.png + +# VS Code +.vscode + +# other +*.egg-info +__pycache__ + +# IDEs +.idea + +# cache +.pytest_cache + +# JetBrains IDE +.idea/ + +# python +dist/ \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/LICENSE b/external_dependencies/unitree_sdk2_python/LICENSE new file mode 100644 index 0000000..42d2e64 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2016-2024 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/external_dependencies/unitree_sdk2_python/README zh.md b/external_dependencies/unitree_sdk2_python/README zh.md new file mode 100644 index 0000000..a7e6a8c --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/README zh.md @@ -0,0 +1,121 @@ +# unitree_sdk2_python +unitree_sdk2 python 接口 + +# 安装 +## 依赖 +- python>=3.8 +- cyclonedds==0.10.2 +- numpy +- opencv-python + +## 安装 unitree_sdk2_python +在终端中执行： +```bash +cd ~ +sudo apt install python3-pip +git clone https://github.com/unitreerobotics/unitree_sdk2_python.git +cd unitree_sdk2_python +pip3 install -e . +``` +## FAQ +##### 1. `pip3 install -e .` 遇到报错 +```bash +Could not locate cyclonedds. Try to set CYCLONEDDS_HOME or CMAKE_PREFIX_PATH +``` +该错误提示找不到 cyclonedds 路径。首先编译安装cyclonedds： +```bash +cd ~ +git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x +cd cyclonedds && mkdir build install && cd build +cmake .. -DCMAKE_INSTALL_PREFIX=../install +cmake --build . --target install +``` +进入 unitree_sdk2_python 目录，设置 `CYCLONEDDS_HOME` 为刚刚编译好的 cyclonedds 所在路径，再安装 unitree_sdk2_python +```bash +cd ~/unitree_sdk2_python +export CYCLONEDDS_HOME="~/cyclonedds/install" +pip3 install -e . +``` + +详细见： +https://pypi.org/project/cyclonedds/#installing-with-pre-built-binaries + +# 使用 +python sdk2 接口与 unitree_skd2的接口保持一致，通过请求响应或订阅发布topic实现机器人的状态获取和控制。相应的例程位于`/example`目录下。在运行例程前，需要根据文档 https://support.unitree.com/home/zh/developer/Quick_start 配置好机器人的网络连接。 +## DDS通讯 +在终端中执行： +```bash +python3 ./example/helloworld/publisher.py +``` +打开新的终端，执行： +```bash +python3 ./example/helloworld/subscriber.py +``` +可以看到终端输出的数据信息。`publisher.py` 和 `subscriber.py` 传输的数据定义在 `user_data.py` 中，用户可以根据需要自行定义需要传输的数据结构。 + +## 高层状态和控制 +高层接口的数据结构和控制方式与unitree_sdk2一致。具体可见：https://support.unitree.com/home/zh/developer/sports_services +### 高层状态 +终端中执行： +```bash +python3 ./example/high_level/read_highstate.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。 +### 高层控制 +终端中执行： +```bash +python3 ./example/high_level/sportmode_test.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。 +该例程提供了几种测试方法，可根据测试需要选择: +```python +test.StandUpDown() # 站立趴下 +# test.VelocityMove() # 速度控制 +# test.BalanceAttitude() # 姿态控制 +# test.TrajectoryFollow() # 轨迹跟踪 +# test.SpecialMotions() # 特殊动作 + +``` +## 底层状态和控制 +底层接口的数据结构和控制方式与unitree_sdk2一致。具体可见：https://support.unitree.com/home/zh/developer/Basic_services +### 底层状态 +终端中执行： +```bash +python3 ./example/low_level/lowlevel_control.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。程序会输出右前腿hip关节的状态、IMU和电池电压信息。 + +### 底层电机控制 +首先使用 app 关闭高层运动服务(sport_mode)，否则会导致指令冲突。 +终端中执行： +```bash +python3 ./example/low_level/lowlevel_control.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。左后腿 hip 关节会保持在0角度 (安全起见，这里设置 kp=10, kd=1)，左后腿 calf 关节将持续输出 1Nm 的转矩。 + +## 遥控器状态获取 +终端中执行： +```bash +python3 ./example/wireless_controller/wireless_controller.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。 +终端将输出每一个按键的状态。对于遥控器按键的定义和数据结构可见： https://support.unitree.com/home/zh/developer/Get_remote_control_status + +## 前置摄像头 +使用opencv获取前置摄像头(确保在有图形界面的系统下运行, 按 ESC 退出程序): +```bash +python3 ./example/front_camera/camera_opencv.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。 + +## 避障开关 +```bash +python3 ./example/obstacles_avoid_switch/obstacles_avoid_switch.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。机器人将循环开启和关闭避障功能。关于避障服务，详细见 https://support.unitree.com/home/zh/developer/ObstaclesAvoidClient + +## 灯光音量控制 +```bash +python3 ./example/vui_client/vui_client_example.py enp2s0 +``` +其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。机器人将循环调节音量和灯光亮度。该接口详细见 https://support.unitree.com/home/zh/developer/VuiClient diff --git a/external_dependencies/unitree_sdk2_python/README.md b/external_dependencies/unitree_sdk2_python/README.md new file mode 100644 index 0000000..fb799c2 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/README.md @@ -0,0 +1,118 @@ +# unitree_sdk2_python +Python interface for unitree sdk2 + +# Installation +## Dependencies +- Python >= 3.8 +- cyclonedds == 0.10.2 +- numpy +- opencv-python +## Install unitree_sdk2_python + +```bash +pip install unitree_sdk2py +``` + +### Installing from source +Execute the following commands in the terminal: +```bash +cd ~ +sudo apt install python3-pip +git clone https://github.com/unitreerobotics/unitree_sdk2_python.git +cd unitree_sdk2_python +pip3 install -e . +``` +## FAQ +##### 1. Error when `pip3 install -e .`: +```bash +Could not locate cyclonedds. Try to set CYCLONEDDS_HOME or CMAKE_PREFIX_PATH +``` +This error mentions that the cyclonedds path could not be found. First compile and install cyclonedds: + +```bash +cd ~ +git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x +cd cyclonedds && mkdir build install && cd build +cmake .. -DCMAKE_INSTALL_PREFIX=../install +cmake --build . --target install +``` +Enter the unitree_sdk2_python directory, set `CYCLONEDDS_HOME` to the path of the cyclonedds you just compiled, and then install unitree_sdk2_python. +```bash +cd ~/unitree_sdk2_python +export CYCLONEDDS_HOME="~/cyclonedds/install" +pip3 install -e . +``` +For details, see: https://pypi.org/project/cyclonedds/#installing-with-pre-built-binaries + +# Usage +The Python sdk2 interface maintains consistency with the unitree_sdk2 interface, achieving robot status acquisition and control through request-response or topic subscription/publishing. Example programs are located in the `/example` directory. Before running the examples, configure the robot's network connection as per the instructions in the document at https://support.unitree.com/home/en/developer/Quick_start. +## DDS Communication +In the terminal, execute: +```bash +python3 ./example/helloworld/publisher.py +``` +Open a new terminal and execute: +```bash +python3 ./example/helloworld/subscriber.py +``` +You will see the data output in the terminal. The data structure transmitted between `publisher.py` and `subscriber.py` is defined in `user_data.py`, and users can define the required data structure as needed. +## High-Level Status and Control +The high-level interface maintains consistency with unitree_sdk2 in terms of data structure and control methods. For detailed information, refer to https://support.unitree.com/home/en/developer/sports_services. +### High-Level Status +Execute the following command in the terminal: +```bash +python3 ./example/high_level/read_highstate.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected,. +### High-Level Control +Execute the following command in the terminal: +```bash +python3 ./example/high_level/sportmode_test.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. This example program provides several test methods, and you can choose the required tests as follows: +```python +test.StandUpDown() # Stand up and lie down +# test.VelocityMove() # Velocity control +# test.BalanceAttitude() # Attitude control +# test.TrajectoryFollow() # Trajectory tracking +# test.SpecialMotions() # Special motions +``` +## Low-Level Status and Control +The low-level interface maintains consistency with unitree_sdk2 in terms of data structure and control methods. For detailed information, refer to https://support.unitree.com/home/en/developer/Basic_services. +### Low-Level Status +Execute the following command in the terminal: +```bash +python3 ./example/low_level/lowlevel_control.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. The program will output the state of the right front leg hip joint, IMU, and battery voltage. +### Low-Level Motor Control +First, use the app to turn off the high-level motion service (sport_mode) to prevent conflicting instructions. +Execute the following command in the terminal: +```bash +python3 ./example/low_level/lowlevel_control.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. The left hind leg hip joint will maintain a 0-degree position (for safety, set kp=10, kd=1), and the left hind leg calf joint will continuously output 1Nm of torque. +## Wireless Controller Status +Execute the following command in the terminal: +```bash +python3 ./example/wireless_controller/wireless_controller.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. The terminal will output the status of each key. For the definition and data structure of the remote control keys, refer to https://support.unitree.com/home/en/developer/Get_remote_control_status. +## Front Camera +Use OpenCV to obtain the front camera (ensure to run on a system with a graphical interface, and press ESC to exit the program): +```bash +python3 ./example/front_camera/camera_opencv.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. + +## Obstacle Avoidance Switch +```bash +python3 ./example/obstacles_avoid_switch/obstacles_avoid_switch.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected. The robot will cycle obstacle avoidance on and off. For details on the obstacle avoidance service, see https://support.unitree.com/home/en/developer/ObstaclesAvoidClient + +## Light and volume control +```bash +python3 ./example/vui_client/vui_client_example.py enp2s0 +``` +Replace `enp2s0` with the name of the network interface to which the robot is connected.T he robot will cycle the volume and light brightness. The interface is detailed at https://support.unitree.com/home/en/developer/VuiClient \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/b2/camera/camera_opencv.py b/external_dependencies/unitree_sdk2_python/example/b2/camera/camera_opencv.py new file mode 100644 index 0000000..3fafcef --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2/camera/camera_opencv.py @@ -0,0 +1,51 @@ +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient +from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient +import cv2 +import numpy as np +import sys + +def display_image(window_name, data): + # If data is a list, we need to convert it to a bytes object + if isinstance(data, list): + data = bytes(data) + + # Now convert to numpy image + image_data = np.frombuffer(data, dtype=np.uint8) + image = cv2.imdecode(image_data, cv2.IMREAD_COLOR) + if image is not None: + cv2.imshow(window_name, image) + +if __name__ == "__main__": + if len(sys.argv) > 1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + frontCameraClient = FrontVideoClient() # Create a front camera video client + frontCameraClient.SetTimeout(3.0) + frontCameraClient.Init() + + backCameraClient = BackVideoClient() # Create a back camera video client + backCameraClient.SetTimeout(3.0) + backCameraClient.Init() + + # Loop to continuously fetch images + while True: + # Get front camera image + front_code, front_data = frontCameraClient.GetImageSample() + if front_code == 0: + display_image("Front Camera", front_data) + + # Get back camera image + back_code, back_data = backCameraClient.GetImageSample() + if back_code == 0: + display_image("Back Camera", back_data) + + # Press ESC to stop + if cv2.waitKey(20) == 27: + break + + # Clean up windows + cv2.destroyAllWindows() + diff --git a/external_dependencies/unitree_sdk2_python/example/b2/camera/capture_image.py b/external_dependencies/unitree_sdk2_python/example/b2/camera/capture_image.py new file mode 100644 index 0000000..dcfaad6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2/camera/capture_image.py @@ -0,0 +1,51 @@ +import time +import os +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient +from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient + +if __name__ == "__main__": + if len(sys.argv) > 1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + # 创建前置相机客户端 + front_client = FrontVideoClient() + front_client.SetTimeout(3.0) + front_client.Init() + + # 创建后置相机客户端 + back_client = BackVideoClient() + back_client.SetTimeout(3.0) + back_client.Init() + + print("##################Get Front Camera Image###################") + # 获取前置相机图像 + front_code, front_data = front_client.GetImageSample() + + if front_code != 0: + print("Get front camera image error. Code:", front_code) + else: + front_image_name = "./front_img.jpg" + print("Front Image Saved as:", front_image_name) + + with open(front_image_name, "+wb") as f: + f.write(bytes(front_data)) + + print("##################Get Back Camera Image###################") + # 获取后置相机图像 + back_code, back_data = back_client.GetImageSample() + + if back_code != 0: + print("Get back camera image error. Code:", back_code) + else: + back_image_name = "./back_img.jpg" + print("Back Image Saved as:", back_image_name) + + with open(back_image_name, "+wb") as f: + f.write(bytes(back_data)) + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/b2/high_level/b2_sport_client.py b/external_dependencies/unitree_sdk2_python/example/b2/high_level/b2_sport_client.py new file mode 100644 index 0000000..695d28e --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2/high_level/b2_sport_client.py @@ -0,0 +1,105 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.b2.sport.sport_client import SportClient +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="stand_up", id=1), + TestOption(name="stand_down", id=2), + TestOption(name="move forward", id=3), + TestOption(name="move lateral", id=4), + TestOption(name="move rotate", id=5), + TestOption(name="stop_move", id=6), + TestOption(name="switch_gait", id=7), + TestOption(name="switch_gait", id=8), + TestOption(name="recovery", id=9), + TestOption(name="balanced stand", id=10) +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"{option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + ChannelFactoryInitialize(0, sys.argv[1]) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = SportClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + print("Input \"list\" to list all test option ...") + + while True: + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.StandUp() + elif test_option.id == 2: + sport_client.StandDown() + elif test_option.id == 3: + sport_client.Move(0.3,0,0) + elif test_option.id == 4: + sport_client.Move(0,0.3,0) + elif test_option.id == 5: + sport_client.Move(0,0,0.5) + elif test_option.id == 6: + sport_client.StopMove() + elif test_option.id == 7: + sport_client.SwitchGait(0) + elif test_option.id == 8: + sport_client.SwitchGait(1) + elif test_option.id == 9: + sport_client.RecoveryStand() + elif test_option.id == 10: + sport_client.BalanceStand() + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/b2/low_level/b2_stand_example.py b/external_dependencies/unitree_sdk2_python/example/b2/low_level/b2_stand_example.py new file mode 100644 index 0000000..a091630 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2/low_level/b2_stand_example.py @@ -0,0 +1,175 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ +from unitree_sdk2py.utils.thread import RecurrentThread +import unitree_legged_const as b2 +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient +from unitree_sdk2py.b2.sport.sport_client import SportClient + +from unitree_sdk2py.utils.crc import CRC + +class Custom: + def __init__(self): + self.Kp = 1000.0 + self.Kd = 10.0 + self.time_consume = 0 + self.rate_count = 0 + self.sin_count = 0 + self.motiontime = 0 + self.dt = 0.002 + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = None + + self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65] + + self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3] + + self.targetPos_3 = [-0.5, 1.36, -2.65, 0.5, 1.36, -2.65, + -0.5, 1.36, -2.65, 0.5, 1.36, -2.65] + + self.startPos = [0.0] * 12 + self.duration_1 = 500 + self.duration_2 = 900 + self.duration_3 = 1000 + self.duration_4 = 900 + self.percent_1 = 0 + self.percent_2 = 0 + self.percent_3 = 0 + self.percent_4 = 0 + + self.firstRun = True + self.done = False + + # thread handling + self.lowCmdWriteThreadPtr = None + + self.crc = CRC() + + def Init(self): + self.InitLowCmd() + + # create publisher # + self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10) + + self.sc = SportClient() + self.sc.SetTimeout(5.0) + self.sc.Init() + + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.sc.StandDown() + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=0.002, target=self.LowCmdWrite, name="writebasiccmd" + ) + self.lowCmdWriteThreadPtr.Start() + + def InitLowCmd(self): + self.low_cmd.head[0] = 0xFE + self.low_cmd.head[1] = 0xEF + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(20): + self.low_cmd.motor_cmd[i].mode = 0x01 + self.low_cmd.motor_cmd[i].q= b2.PosStopF + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = b2.VelStopF + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + + def LowStateMessageHandler(self, msg: LowState_): + self.low_state = msg + + def LowCmdWrite(self): + + if self.firstRun: + for i in range(12): + self.startPos[i] = self.low_state.motor_state[i].q + self.firstRun = False + + self.percent_1 += 1.0 / self.duration_1 + self.percent_1 = min(self.percent_1, 1) + if self.percent_1 < 1: + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 <= 1): + self.percent_2 += 1.0 / self.duration_2 + self.percent_2 = min(self.percent_2, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1): + self.percent_3 += 1.0 / self.duration_3 + self.percent_3 = min(self.percent_3, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1): + self.percent_4 += 1.0 / self.duration_4 + self.percent_4 = min(self.percent_4, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + if custom.percent_4 == 1.0: + time.sleep(1) + print("Done!") + sys.exit(-1) + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/b2/low_level/unitree_legged_const.py b/external_dependencies/unitree_sdk2_python/example/b2/low_level/unitree_legged_const.py new file mode 100644 index 0000000..153a051 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2/low_level/unitree_legged_const.py @@ -0,0 +1,20 @@ +LegID = { + "FR_0": 0, # Front right hip + "FR_1": 1, # Front right thigh + "FR_2": 2, # Front right calf + "FL_0": 3, + "FL_1": 4, + "FL_2": 5, + "RR_0": 6, + "RR_1": 7, + "RR_2": 8, + "RL_0": 9, + "RL_1": 10, + "RL_2": 11, +} + +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/example/b2w/camera/camera_opencv.py b/external_dependencies/unitree_sdk2_python/example/b2w/camera/camera_opencv.py new file mode 100644 index 0000000..3fafcef --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2w/camera/camera_opencv.py @@ -0,0 +1,51 @@ +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient +from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient +import cv2 +import numpy as np +import sys + +def display_image(window_name, data): + # If data is a list, we need to convert it to a bytes object + if isinstance(data, list): + data = bytes(data) + + # Now convert to numpy image + image_data = np.frombuffer(data, dtype=np.uint8) + image = cv2.imdecode(image_data, cv2.IMREAD_COLOR) + if image is not None: + cv2.imshow(window_name, image) + +if __name__ == "__main__": + if len(sys.argv) > 1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + frontCameraClient = FrontVideoClient() # Create a front camera video client + frontCameraClient.SetTimeout(3.0) + frontCameraClient.Init() + + backCameraClient = BackVideoClient() # Create a back camera video client + backCameraClient.SetTimeout(3.0) + backCameraClient.Init() + + # Loop to continuously fetch images + while True: + # Get front camera image + front_code, front_data = frontCameraClient.GetImageSample() + if front_code == 0: + display_image("Front Camera", front_data) + + # Get back camera image + back_code, back_data = backCameraClient.GetImageSample() + if back_code == 0: + display_image("Back Camera", back_data) + + # Press ESC to stop + if cv2.waitKey(20) == 27: + break + + # Clean up windows + cv2.destroyAllWindows() + diff --git a/external_dependencies/unitree_sdk2_python/example/b2w/camera/capture_image.py b/external_dependencies/unitree_sdk2_python/example/b2w/camera/capture_image.py new file mode 100644 index 0000000..dcfaad6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2w/camera/capture_image.py @@ -0,0 +1,51 @@ +import time +import os +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient +from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient + +if __name__ == "__main__": + if len(sys.argv) > 1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + # 创建前置相机客户端 + front_client = FrontVideoClient() + front_client.SetTimeout(3.0) + front_client.Init() + + # 创建后置相机客户端 + back_client = BackVideoClient() + back_client.SetTimeout(3.0) + back_client.Init() + + print("##################Get Front Camera Image###################") + # 获取前置相机图像 + front_code, front_data = front_client.GetImageSample() + + if front_code != 0: + print("Get front camera image error. Code:", front_code) + else: + front_image_name = "./front_img.jpg" + print("Front Image Saved as:", front_image_name) + + with open(front_image_name, "+wb") as f: + f.write(bytes(front_data)) + + print("##################Get Back Camera Image###################") + # 获取后置相机图像 + back_code, back_data = back_client.GetImageSample() + + if back_code != 0: + print("Get back camera image error. Code:", back_code) + else: + back_image_name = "./back_img.jpg" + print("Back Image Saved as:", back_image_name) + + with open(back_image_name, "+wb") as f: + f.write(bytes(back_data)) + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/b2w/high_level/b2w_sport_client.py b/external_dependencies/unitree_sdk2_python/example/b2w/high_level/b2w_sport_client.py new file mode 100644 index 0000000..e324f75 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2w/high_level/b2w_sport_client.py @@ -0,0 +1,101 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.b2.sport.sport_client import SportClient +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="stand_up", id=1), + TestOption(name="stand_down", id=2), + TestOption(name="move forward", id=3), + TestOption(name="move lateral", id=4), + TestOption(name="move rotate", id=5), + TestOption(name="stop_move", id=6), + TestOption(name="switch_gait", id=7), + TestOption(name="switch_gait", id=8), + TestOption(name="recovery", id=9) +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"name: {option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + ChannelFactoryInitialize(0, sys.argv[1]) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = SportClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + print("Input \"list\" to list all test option ...") + + while True: + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.StandUp() + elif test_option.id == 2: + sport_client.StandDown() + elif test_option.id == 3: + sport_client.Move(0.3,0,0) + elif test_option.id == 4: + sport_client.Move(0,0.3,0) + elif test_option.id == 5: + sport_client.Move(0,0,0.5) + elif test_option.id == 6: + sport_client.StopMove() + elif test_option.id == 7: + sport_client.SwitchGait(0) + elif test_option.id == 8: + sport_client.SwitchGait(1) + elif test_option.id == 9: + sport_client.RecoveryStand() + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/b2w/low_level/b2w_stand_example.py b/external_dependencies/unitree_sdk2_python/example/b2w/low_level/b2w_stand_example.py new file mode 100644 index 0000000..b535059 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2w/low_level/b2w_stand_example.py @@ -0,0 +1,196 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +import unitree_legged_const as b2w +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient +from unitree_sdk2py.b2.sport.sport_client import SportClient + +class Custom: + def __init__(self): + self.Kp = 1000.0 + self.Kd = 10.0 + self.time_consume = 0 + self.rate_count = 0 + self.sin_count = 0 + self.motiontime = 0 + self.dt = 0.002 + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = None + + self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65] + + self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3] + + self.targetPos_3 = [-0.65, 1.36, -2.65, 0.65, 1.36, -2.65, + -0.65, 1.36, -2.65, 0.65, 1.36, -2.65] + + self.startPos = [0.0] * 12 + self.duration_1 = 800 + self.duration_2 = 800 + self.duration_3 = 2000 + self.duration_4 = 1500 + self.percent_1 = 0 + self.percent_2 = 0 + self.percent_3 = 0 + self.percent_4 = 0 + + self.firstRun = True + self.done = False + + # thread handling + self.lowCmdWriteThreadPtr = None + + self.crc = CRC() + + def Init(self): + self.InitLowCmd() + + # create publisher # + self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10) + + self.sc = SportClient() + self.sc.SetTimeout(5.0) + self.sc.Init() + + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.sc.StandDown() + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + name="writebasiccmd", interval=self.dt, target=self.LowCmdWrite, + ) + self.lowCmdWriteThreadPtr.Start() + + def InitLowCmd(self): + self.low_cmd.head[0] = 0xFE + self.low_cmd.head[1] = 0xEF + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(20): + self.low_cmd.motor_cmd[i].mode = 0x01 # (PMSM) mode + self.low_cmd.motor_cmd[i].q = b2w.PosStopF + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = b2w.VelStopF + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + + def LowStateMessageHandler(self, msg: LowState_): + self.low_state = msg + + def LowCmdWrite(self): + if self.firstRun: + for i in range(12): + self.startPos[i] = self.low_state.motor_state[i].q + self.firstRun = False + + self.percent_1 += 1.0 / self.duration_1 + self.percent_1 = min(self.percent_1, 1) + if self.percent_1 < 1: + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 <= 1): + self.percent_2 += 1.0 / self.duration_2 + self.percent_2 = min(self.percent_2, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1): + self.percent_3 += 1.0 / self.duration_3 + self.percent_3 = min(self.percent_3, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if self.percent_3 < 0.4: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = 3 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if 0.4 <= self.percent_3 < 0.8: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = -3 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if self.percent_3 >= 0.8: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1): + self.percent_4 += 1.0 / self.duration_4 + self.percent_4 = min(self.percent_4, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + if custom.percent_4 == 1.0: + time.sleep(1) + print("Done!") + sys.exit(-1) + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/b2w/low_level/unitree_legged_const.py b/external_dependencies/unitree_sdk2_python/example/b2w/low_level/unitree_legged_const.py new file mode 100644 index 0000000..7943e31 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/b2w/low_level/unitree_legged_const.py @@ -0,0 +1,24 @@ +LegID = { + "FR_0": 0, # Front right hip + "FR_1": 1, # Front right thigh + "FR_2": 2, # Front right calf + "FL_0": 3, + "FL_1": 4, + "FL_2": 5, + "RR_0": 6, + "RR_1": 7, + "RR_2": 8, + "RL_0": 9, + "RL_1": 10, + "RL_2": 11, + "FR_w": 12, # Front right wheel + "FL_w": 13, # Front left wheel + "RR_w": 14, # Rear right wheel + "RL_w": 15, # Rear left wheel +} + +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/example/g1/audio/g1_audio_client_example.py b/external_dependencies/unitree_sdk2_python/example/g1/audio/g1_audio_client_example.py new file mode 100644 index 0000000..6e0cc25 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/audio/g1_audio_client_example.py @@ -0,0 +1,44 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient +from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + ChannelFactoryInitialize(0, sys.argv[1]) + + audio_client = AudioClient() + audio_client.SetTimeout(10.0) + audio_client.Init() + + sport_client = LocoClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + ret = audio_client.GetVolume() + print("debug GetVolume: ",ret) + + audio_client.SetVolume(85) + + ret = audio_client.GetVolume() + print("debug GetVolume: ",ret) + + sport_client.WaveHand() + + audio_client.TtsMaker("大家好!我是宇树科技人形机器人。语音开发测试例程运行成功！ 很高兴认识你！",0) + time.sleep(8) + audio_client.TtsMaker("接下来测试灯带开发例程！",0) + time.sleep(1) + audio_client.LedControl(255,0,0) + time.sleep(1) + audio_client.LedControl(0,255,0) + time.sleep(1) + audio_client.LedControl(0,0,255) + + time.sleep(3) + audio_client.TtsMaker("测试完毕，谢谢大家！",0) + diff --git a/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm5_sdk_dds_example.py b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm5_sdk_dds_example.py new file mode 100644 index 0000000..6090807 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm5_sdk_dds_example.py @@ -0,0 +1,192 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient + +import numpy as np + +kPi = 3.141592654 +kPi_2 = 1.57079632 + +class G1JointIndex: + # Left leg + LeftHipPitch = 0 + LeftHipRoll = 1 + LeftHipYaw = 2 + LeftKnee = 3 + LeftAnklePitch = 4 + LeftAnkleB = 4 + LeftAnkleRoll = 5 + LeftAnkleA = 5 + + # Right leg + RightHipPitch = 6 + RightHipRoll = 7 + RightHipYaw = 8 + RightKnee = 9 + RightAnklePitch = 10 + RightAnkleB = 10 + RightAnkleRoll = 11 + RightAnkleA = 11 + + WaistYaw = 12 + WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + + # Left arm + LeftShoulderPitch = 15 + LeftShoulderRoll = 16 + LeftShoulderYaw = 17 + LeftElbow = 18 + LeftWristRoll = 19 + LeftWristPitch = 20 # NOTE: INVALID for g1 23dof + LeftWristYaw = 21 # NOTE: INVALID for g1 23dof + + # Right arm + RightShoulderPitch = 22 + RightShoulderRoll = 23 + RightShoulderYaw = 24 + RightElbow = 25 + RightWristRoll = 26 + RightWristPitch = 27 # NOTE: INVALID for g1 23dof + RightWristYaw = 28 # NOTE: INVALID for g1 23dof + + kNotUsedJoint = 29 # NOTE: Weight + +class Custom: + def __init__(self): + self.time_ = 0.0 + self.control_dt_ = 0.02 + self.duration_ = 3.0 + self.counter_ = 0 + self.weight = 0. + self.weight_rate = 0.2 + self.kp = 60. + self.kd = 1.5 + self.dq = 0. + self.tau_ff = 0. + self.mode_machine_ = 0 + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = None + self.first_update_low_state = False + self.crc = CRC() + self.done = False + + self.target_pos = [ + 0.0, kPi_2, 0.0, kPi_2, 0.0, + 0.0, -kPi_2, 0.0, kPi_2, 0.0, + 0.0, 0.0, 0.0 + ] + + self.arm_joints = [ + G1JointIndex.LeftShoulderPitch, G1JointIndex.LeftShoulderRoll, + G1JointIndex.LeftShoulderYaw, G1JointIndex.LeftElbow, + G1JointIndex.LeftWristRoll, + G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll, + G1JointIndex.RightShoulderYaw, G1JointIndex.RightElbow, + G1JointIndex.RightWristRoll, + G1JointIndex.WaistYaw, + G1JointIndex.WaistRoll, + G1JointIndex.WaistPitch + ] + + def Init(self): + # create publisher # + self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_) + self.arm_sdk_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateHandler, 10) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=self.control_dt_, target=self.LowCmdWrite, name="control" + ) + while self.first_update_low_state == False: + time.sleep(1) + + if self.first_update_low_state == True: + self.lowCmdWriteThreadPtr.Start() + + def LowStateHandler(self, msg: LowState_): + self.low_state = msg + + if self.first_update_low_state == False: + self.first_update_low_state = True + + def LowCmdWrite(self): + self.time_ += self.control_dt_ + + if self.time_ < self.duration_ : + # [Stage 1]: set robot to zero posture + self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = 1 # 1:Enable arm_sdk, 0:Disable arm_sdk + for i,joint in enumerate(self.arm_joints): + ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 3 : + # [Stage 2]: lift arms up + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 6 : + # [Stage 3]: set robot back to zero posture + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 7 : + # [Stage 4]: release arm_sdk + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0) + self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk + + else: + self.done = True + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.arm_sdk_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + time.sleep(1) + if custom.done: + print("Done!") + sys.exit(-1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm7_sdk_dds_example.py b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm7_sdk_dds_example.py new file mode 100644 index 0000000..6ced14f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_arm7_sdk_dds_example.py @@ -0,0 +1,194 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient + +import numpy as np + +kPi = 3.141592654 +kPi_2 = 1.57079632 + +class G1JointIndex: + # Left leg + LeftHipPitch = 0 + LeftHipRoll = 1 + LeftHipYaw = 2 + LeftKnee = 3 + LeftAnklePitch = 4 + LeftAnkleB = 4 + LeftAnkleRoll = 5 + LeftAnkleA = 5 + + # Right leg + RightHipPitch = 6 + RightHipRoll = 7 + RightHipYaw = 8 + RightKnee = 9 + RightAnklePitch = 10 + RightAnkleB = 10 + RightAnkleRoll = 11 + RightAnkleA = 11 + + WaistYaw = 12 + WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + + # Left arm + LeftShoulderPitch = 15 + LeftShoulderRoll = 16 + LeftShoulderYaw = 17 + LeftElbow = 18 + LeftWristRoll = 19 + LeftWristPitch = 20 # NOTE: INVALID for g1 23dof + LeftWristYaw = 21 # NOTE: INVALID for g1 23dof + + # Right arm + RightShoulderPitch = 22 + RightShoulderRoll = 23 + RightShoulderYaw = 24 + RightElbow = 25 + RightWristRoll = 26 + RightWristPitch = 27 # NOTE: INVALID for g1 23dof + RightWristYaw = 28 # NOTE: INVALID for g1 23dof + + kNotUsedJoint = 29 # NOTE: Weight + +class Custom: + def __init__(self): + self.time_ = 0.0 + self.control_dt_ = 0.02 + self.duration_ = 3.0 + self.counter_ = 0 + self.weight = 0. + self.weight_rate = 0.2 + self.kp = 60. + self.kd = 1.5 + self.dq = 0. + self.tau_ff = 0. + self.mode_machine_ = 0 + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = None + self.first_update_low_state = False + self.crc = CRC() + self.done = False + + self.target_pos = [ + 0., kPi_2, 0., kPi_2, 0., 0., 0., + 0., -kPi_2, 0., kPi_2, 0., 0., 0., + 0, 0, 0 + ] + + self.arm_joints = [ + G1JointIndex.LeftShoulderPitch, G1JointIndex.LeftShoulderRoll, + G1JointIndex.LeftShoulderYaw, G1JointIndex.LeftElbow, + G1JointIndex.LeftWristRoll, G1JointIndex.LeftWristPitch, + G1JointIndex.LeftWristYaw, + G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll, + G1JointIndex.RightShoulderYaw, G1JointIndex.RightElbow, + G1JointIndex.RightWristRoll, G1JointIndex.RightWristPitch, + G1JointIndex.RightWristYaw, + G1JointIndex.WaistYaw, + G1JointIndex.WaistRoll, + G1JointIndex.WaistPitch + ] + + def Init(self): + # create publisher # + self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_) + self.arm_sdk_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateHandler, 10) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=self.control_dt_, target=self.LowCmdWrite, name="control" + ) + while self.first_update_low_state == False: + time.sleep(1) + + if self.first_update_low_state == True: + self.lowCmdWriteThreadPtr.Start() + + def LowStateHandler(self, msg: LowState_): + self.low_state = msg + + if self.first_update_low_state == False: + self.first_update_low_state = True + + def LowCmdWrite(self): + self.time_ += self.control_dt_ + + if self.time_ < self.duration_ : + # [Stage 1]: set robot to zero posture + self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = 1 # 1:Enable arm_sdk, 0:Disable arm_sdk + for i,joint in enumerate(self.arm_joints): + ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 3 : + # [Stage 2]: lift arms up + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 6 : + # [Stage 3]: set robot back to zero posture + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0) + self.low_cmd.motor_cmd[joint].tau = 0. + self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q + self.low_cmd.motor_cmd[joint].dq = 0. + self.low_cmd.motor_cmd[joint].kp = self.kp + self.low_cmd.motor_cmd[joint].kd = self.kd + + elif self.time_ < self.duration_ * 7 : + # [Stage 4]: release arm_sdk + for i,joint in enumerate(self.arm_joints): + ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0) + self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk + + else: + self.done = True + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.arm_sdk_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + time.sleep(1) + if custom.done: + print("Done!") + sys.exit(-1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_loco_client_example.py b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_loco_client_example.py new file mode 100644 index 0000000..55ee059 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/high_level/g1_loco_client_example.py @@ -0,0 +1,117 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ +from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="Squat2StandUp", id=1), + TestOption(name="StandUp2Squat", id=2), + TestOption(name="move forward", id=3), + TestOption(name="move lateral", id=4), + TestOption(name="move rotate", id=5), + TestOption(name="low stand", id=6), + TestOption(name="high stand", id=7), + TestOption(name="zero torque", id=8), + TestOption(name="wave hand1", id=9), # wave hand without turning around + TestOption(name="wave hand2", id=10), # wave hand and trun around + TestOption(name="shake hand", id=11), + TestOption(name="Lie2StandUp", id=12), +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"{option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + ChannelFactoryInitialize(0, sys.argv[1]) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = LocoClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + print("Input \"list\" to list all test option ...") + while True: + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.Damp() + time.sleep(0.5) + sport_client.Squat2StandUp() + elif test_option.id == 2: + sport_client.StandUp2Squat() + elif test_option.id == 3: + sport_client.Move(0.3,0,0) + elif test_option.id == 4: + sport_client.Move(0,0.3,0) + elif test_option.id == 5: + sport_client.Move(0,0,0.3) + elif test_option.id == 6: + sport_client.LowStand() + elif test_option.id == 7: + sport_client.HighStand() + elif test_option.id == 8: + sport_client.ZeroTorque() + elif test_option.id == 9: + sport_client.WaveHand() + elif test_option.id == 10: + sport_client.WaveHand(True) + elif test_option.id == 11: + sport_client.ShakeHand() + time.sleep(3) + sport_client.ShakeHand() + elif test_option.id == 12: + sport_client.Damp() + time.sleep(0.5) + sport_client.Lie2StandUp() # When using the Lie2StandUp function, ensure that the robot faces up and the ground is hard, flat and rough. + + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_low_level_example.py b/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_low_level_example.py new file mode 100644 index 0000000..382e863 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_low_level_example.py @@ -0,0 +1,205 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient + +import numpy as np + +G1_NUM_MOTOR = 29 + +Kp = [ + 60, 60, 60, 100, 40, 40, # legs + 60, 60, 60, 100, 40, 40, # legs + 60, 40, 40, # waist + 40, 40, 40, 40, 40, 40, 40, # arms + 40, 40, 40, 40, 40, 40, 40 # arms +] + +Kd = [ + 1, 1, 1, 2, 1, 1, # legs + 1, 1, 1, 2, 1, 1, # legs + 1, 1, 1, # waist + 1, 1, 1, 1, 1, 1, 1, # arms + 1, 1, 1, 1, 1, 1, 1 # arms +] + +class G1JointIndex: + LeftHipPitch = 0 + LeftHipRoll = 1 + LeftHipYaw = 2 + LeftKnee = 3 + LeftAnklePitch = 4 + LeftAnkleB = 4 + LeftAnkleRoll = 5 + LeftAnkleA = 5 + RightHipPitch = 6 + RightHipRoll = 7 + RightHipYaw = 8 + RightKnee = 9 + RightAnklePitch = 10 + RightAnkleB = 10 + RightAnkleRoll = 11 + RightAnkleA = 11 + WaistYaw = 12 + WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked + LeftShoulderPitch = 15 + LeftShoulderRoll = 16 + LeftShoulderYaw = 17 + LeftElbow = 18 + LeftWristRoll = 19 + LeftWristPitch = 20 # NOTE: INVALID for g1 23dof + LeftWristYaw = 21 # NOTE: INVALID for g1 23dof + RightShoulderPitch = 22 + RightShoulderRoll = 23 + RightShoulderYaw = 24 + RightElbow = 25 + RightWristRoll = 26 + RightWristPitch = 27 # NOTE: INVALID for g1 23dof + RightWristYaw = 28 # NOTE: INVALID for g1 23dof + + +class Mode: + PR = 0 # Series Control for Pitch/Roll Joints + AB = 1 # Parallel Control for A/B Joints + +class Custom: + def __init__(self): + self.time_ = 0.0 + self.control_dt_ = 0.002 # [2ms] + self.duration_ = 3.0 # [3 s] + self.counter_ = 0 + self.mode_pr_ = Mode.PR + self.mode_machine_ = 0 + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = None + self.update_mode_machine_ = False + self.crc = CRC() + + def Init(self): + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + # create publisher # + self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher_.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateHandler, 10) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=self.control_dt_, target=self.LowCmdWrite, name="control" + ) + while self.update_mode_machine_ == False: + time.sleep(1) + + if self.update_mode_machine_ == True: + self.lowCmdWriteThreadPtr.Start() + + def LowStateHandler(self, msg: LowState_): + self.low_state = msg + + if self.update_mode_machine_ == False: + self.mode_machine_ = self.low_state.mode_machine + self.update_mode_machine_ = True + + self.counter_ +=1 + if (self.counter_ % 500 == 0) : + self.counter_ = 0 + print(self.low_state.imu_state.rpy) + + def LowCmdWrite(self): + self.time_ += self.control_dt_ + + if self.time_ < self.duration_ : + # [Stage 1]: set robot to zero posture + for i in range(G1_NUM_MOTOR): + ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) + self.low_cmd.mode_pr = Mode.PR + self.low_cmd.mode_machine = self.mode_machine_ + self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable + self.low_cmd.motor_cmd[i].tau = 0. + self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q + self.low_cmd.motor_cmd[i].dq = 0. + self.low_cmd.motor_cmd[i].kp = Kp[i] + self.low_cmd.motor_cmd[i].kd = Kd[i] + + elif self.time_ < self.duration_ * 2 : + # [Stage 2]: swing ankle using PR mode + max_P = np.pi * 30.0 / 180.0 + max_R = np.pi * 10.0 / 180.0 + t = self.time_ - self.duration_ + L_P_des = max_P * np.sin(2.0 * np.pi * t) + L_R_des = max_R * np.sin(2.0 * np.pi * t) + R_P_des = max_P * np.sin(2.0 * np.pi * t) + R_R_des = -max_R * np.sin(2.0 * np.pi * t) + + self.low_cmd.mode_pr = Mode.PR + self.low_cmd.mode_machine = self.mode_machine_ + self.low_cmd.motor_cmd[G1JointIndex.LeftAnklePitch].q = L_P_des + self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleRoll].q = L_R_des + self.low_cmd.motor_cmd[G1JointIndex.RightAnklePitch].q = R_P_des + self.low_cmd.motor_cmd[G1JointIndex.RightAnkleRoll].q = R_R_des + + else : + # [Stage 3]: swing ankle using AB mode + max_A = np.pi * 30.0 / 180.0 + max_B = np.pi * 10.0 / 180.0 + t = self.time_ - self.duration_ * 2 + L_A_des = max_A * np.sin(2.0 * np.pi * t) + L_B_des = max_B * np.sin(2.0 * np.pi * t + np.pi) + R_A_des = -max_A * np.sin(2.0 * np.pi * t) + R_B_des = -max_B * np.sin(2.0 * np.pi * t + np.pi) + + self.low_cmd.mode_pr = Mode.AB + self.low_cmd.mode_machine = self.mode_machine_ + self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleA].q = L_A_des + self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleB].q = L_B_des + self.low_cmd.motor_cmd[G1JointIndex.RightAnkleA].q = R_A_des + self.low_cmd.motor_cmd[G1JointIndex.RightAnkleB].q = R_B_des + + max_WristYaw = np.pi * 30.0 / 180.0 + L_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t) + R_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t) + self.low_cmd.motor_cmd[G1JointIndex.LeftWristRoll].q = L_WristYaw_des + self.low_cmd.motor_cmd[G1JointIndex.RightWristRoll].q = R_WristYaw_des + + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher_.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_move_hands_example.py b/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_move_hands_example.py new file mode 100644 index 0000000..4b43fd2 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/low_level/g1_move_hands_example.py @@ -0,0 +1,225 @@ +import time +import sys + +import numpy as np + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread + +MOTOR_NUM_HAND = 7 + +Kp = [0.5] * MOTOR_NUM_HAND +Kd = [0.1] * MOTOR_NUM_HAND + +Kp[0] = 2.0 + +maxTorqueLimits_left = [1.05, 1.05, 1.75, 0.0, 0.0, 0.0, 0.0] +minTorqueLimits_left = [-1.05, -0.72, 0.0, -1.57, -1.75, -1.57, -1.75] +maxTorqueLimits_right = [1.05, 0.74, 0.0, 1.57, 1.75, 1.57, 1.75] +minTorqueLimits_right = [-1.05, -1.05, -1.75, 0.0, 0.0, 0.0, 0.0] + + +def make_hand_mode(motor_index): + status = 0x01 + timeout = 0x01 + mode = (motor_index & 0x0F) + mode |= (status << 4) # bits [4..6] + mode |= (timeout << 7) # bit 7 + return mode + + +class HandControl: + def __init__(self, network_interface="enp36s0f1"): + ChannelFactoryInitialize(0, network_interface) + + self.left_cmd_pub = ChannelPublisher("rt/dex3/left/cmd", HandCmd_) + self.right_cmd_pub = ChannelPublisher("rt/dex3/right/cmd", HandCmd_) + + self.left_state_sub = ChannelSubscriber("rt/dex3/left/state", HandState_) + self.right_state_sub = ChannelSubscriber("rt/dex3/right/state", HandState_) + + self.left_cmd_pub.Init() + self.right_cmd_pub.Init() + + self.left_state_sub.Init(self.left_state_handler, 10) + self.right_state_sub.Init(self.right_state_handler, 10) + + self.left_state = None + self.right_state = None + + self.crc = CRC() + + # Control loop timing + self.control_dt = 0.01 # 10 ms + self.time_ = 0.0 + self.stage_time = 3.0 # 3s per stage + + # To let the code run once we start: + self.run_flag = False + + self.counter = 0 + + def left_state_handler(self, msg: HandState_): + self.left_state = msg + + # self.counter +=1 + # if (self.counter % 1000 == 0) : + # self.counter = 0 + # print('Left hand state:') + # for i in range(MOTOR_NUM_HAND): + # print(180/np.pi*self.left_state.motor_state[i].q) + + def right_state_handler(self, msg: HandState_): + self.right_state = msg + + self.counter += 1 + if (self.counter % 1000 == 0): + self.counter = 0 + print('Right hand state:') + for i in range(MOTOR_NUM_HAND): + print(180 / np.pi * self.right_state.motor_state[i].q) + + def start(self): + """ + Kick off the main control loop thread. + """ + self.run_flag = True + self.control_thread = RecurrentThread(interval=self.control_dt, + target=self.hand_control_loop, + name="HandControlLoop") + self.control_thread.Start() + + def hand_control_loop(self): + """ + This gets called at a fixed rate (every self.control_dt seconds). + We'll demonstrate 3 stages of motion: + 1) Move from current position to 'zero' (or some nominal) in 3s + 2) Sinusoidal motion in stage 2 (3s) + 3) Another motion in stage 3 (final) + """ + if not self.run_flag: + return + + self.time_ += self.control_dt + t = self.time_ + cmd_left = unitree_hg_msg_dds__HandCmd_() + cmd_right = unitree_hg_msg_dds__HandCmd_() + + # cmd_left.motor_cmd.resize(MOTOR_NUM_HAND) + # cmd_right.motor_cmd.resize(MOTOR_NUM_HAND) + + # Prepare stage times + stage1_end = self.stage_time + stage2_end = self.stage_time * 2.0 + + # We'll fetch current positions for left and right if available + # so we can blend from actual state to zero. + # If we haven't gotten a state yet, default to 0. + left_q_now = [0.0] * MOTOR_NUM_HAND + right_q_now = [0.0] * MOTOR_NUM_HAND + + if self.left_state is not None: + for i in range(MOTOR_NUM_HAND): + left_q_now[i] = self.left_state.motor_state[i].q + + if self.right_state is not None: + for i in range(MOTOR_NUM_HAND): + right_q_now[i] = self.right_state.motor_state[i].q + + left_q_desired = np.deg2rad([50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + + right_q_desired = np.deg2rad([50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + + # Decide the desired position for each stage: + if t < stage1_end: + # Stage 1: Move from the current joint positions to zero in [0..3s] + ratio = np.clip(t / stage1_end, 0.0, 1.0) + # Simple linear blend: final = (1-ratio)*initial + ratio*0 + left_q_des = np.zeros(MOTOR_NUM_HAND) + right_q_des = np.zeros(MOTOR_NUM_HAND) + for i in range(MOTOR_NUM_HAND): + left_q_des[i] = (1.0 - ratio) * (left_q_now[i] - left_q_desired[i]) + left_q_desired[i] + right_q_des[i] = (1.0 - ratio) * (right_q_now[i] - right_q_desired[i]) + right_q_desired[i] + + else: + # Stage 2: Some sinusoidal wave + # We'll wave only the first 2 or 3 motors, just as a demo + dt2 = t - stage1_end + freq = 1.0 # 1 Hz + amp = 0.3 # ~ 0.3 rad amplitude + + left_q_des = left_q_desired + right_q_des = right_q_desired + + # E.g. wave motor 0 and 1: + left_q_des[0] += amp * np.sin(2 * np.pi * freq * dt2) + left_q_des[1] += amp * (1 - np.cos(2 * np.pi * freq * dt2)) + left_q_des[2] += amp * (1 - np.cos(2 * np.pi * freq * dt2)) + right_q_des[0] += amp * np.sin(2 * np.pi * freq * dt2) + right_q_des[1] += amp * (np.cos(2 * np.pi * freq * dt2) - 1) + right_q_des[2] += amp * (np.cos(2 * np.pi * freq * dt2) - 1) + + freqA = 2.0 + freqB = 2.0 + ampA = 0.2 + ampB = 0.4 + + left_q_des[3] += ampA * (np.cos(2 * np.pi * freqA * dt2) - 1) + left_q_des[4] += ampB * (np.cos(2 * np.pi * freqB * dt2) - 1) + left_q_des[5] += ampA * (np.cos(2 * np.pi * freqA * dt2) - 1) + left_q_des[6] += ampB * (np.cos(2 * np.pi * freqB * dt2) - 1) + right_q_des[3] += ampA * (1 - np.cos(2 * np.pi * freqA * dt2)) + right_q_des[4] += ampB * (1 - np.cos(2 * np.pi * freqB * dt2)) + right_q_des[5] += ampA * (1 - np.cos(2 * np.pi * freqA * dt2)) + right_q_des[6] += ampB * (1 - np.cos(2 * np.pi * freqB * dt2)) + + # Fill in the commands + for i in range(MOTOR_NUM_HAND): + # Build the bitfield mode (see your C++ example) + mode_val = make_hand_mode(i) + + # Left + cmd_left.motor_cmd[i].mode = mode_val + cmd_left.motor_cmd[i].q = left_q_des[i] + cmd_left.motor_cmd[i].dq = 0.0 + cmd_left.motor_cmd[i].tau = 0.0 + cmd_left.motor_cmd[i].kp = Kp[i] + cmd_left.motor_cmd[i].kd = Kd[i] + + # Right + cmd_right.motor_cmd[i].mode = mode_val + cmd_right.motor_cmd[i].q = right_q_des[i] + cmd_right.motor_cmd[i].dq = 0.0 + cmd_right.motor_cmd[i].tau = 0.0 + cmd_right.motor_cmd[i].kp = Kp[i] + cmd_right.motor_cmd[i].kd = Kd[i] + + # Compute CRC if your firmware requires it + # cmd_left.crc = self.crc.Crc(cmd_left) + # cmd_right.crc = self.crc.Crc(cmd_right) + + # Publish + self.left_cmd_pub.Write(cmd_left) + self.right_cmd_pub.Write(cmd_right) + + +if __name__ == "__main__": + print("WARNING: Make sure your robot’s hands can move freely before running.") + input("Press Enter to continue...") + + # Optionally pass a specific interface name, e.g. "enp37s0f0" or "eth0" + if len(sys.argv) > 1: + net_if = sys.argv[1] + else: + net_if = "enp36s0f1" + + # Create and start + hand_control = HandControl(network_interface=net_if) + hand_control.start() + + # Just wait + while True: + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/g1/readme.md b/external_dependencies/unitree_sdk2_python/example/g1/readme.md new file mode 100644 index 0000000..9fb7900 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/g1/readme.md @@ -0,0 +1,5 @@ +This example is a test of Unitree G1/H1-2 robot. + +**Note:** +idl/unitree_go is used for Unitree Go2/B2/H1/B2w/Go2w robots +idl/unitree_hg is used for Unitree G1/H1-2 robots diff --git a/external_dependencies/unitree_sdk2_python/example/go2/front_camera/camera_opencv.py b/external_dependencies/unitree_sdk2_python/example/go2/front_camera/camera_opencv.py new file mode 100644 index 0000000..9d98839 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/front_camera/camera_opencv.py @@ -0,0 +1,41 @@ +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.video.video_client import VideoClient +import cv2 +import numpy as np +import sys + + +if __name__ == "__main__": + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + client = VideoClient() # Create a video client + client.SetTimeout(3.0) + client.Init() + + code, data = client.GetImageSample() + + # Request normal when code==0 + while code == 0: + # Get Image data from Go2 robot + code, data = client.GetImageSample() + + # Convert to numpy image + image_data = np.frombuffer(bytes(data), dtype=np.uint8) + image = cv2.imdecode(image_data, cv2.IMREAD_COLOR) + + # Display image + cv2.imshow("front_camera", image) + # Press ESC to stop + if cv2.waitKey(20) == 27: + break + + if code != 0: + print("Get image sample error. code:", code) + else: + # Capture an image + cv2.imwrite("front_image.jpg", image) + + cv2.destroyWindow("front_camera") diff --git a/external_dependencies/unitree_sdk2_python/example/go2/front_camera/capture_image.py b/external_dependencies/unitree_sdk2_python/example/go2/front_camera/capture_image.py new file mode 100644 index 0000000..1e85643 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/front_camera/capture_image.py @@ -0,0 +1,30 @@ +import time +import os +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.video.video_client import VideoClient + +if __name__ == "__main__": + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + client = VideoClient() + client.SetTimeout(3.0) + client.Init() + + print("##################GetImageSample###################") + code, data = client.GetImageSample() + + if code != 0: + print("get image sample error. code:", code) + else: + imageName = "./img.jpg" + print("ImageName:", imageName) + + with open(imageName, "+wb") as f: + f.write(bytes(data)) + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_sport_client.py b/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_sport_client.py new file mode 100644 index 0000000..b14d79d --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_sport_client.py @@ -0,0 +1,170 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ +from unitree_sdk2py.go2.sport.sport_client import ( + SportClient, + PathPoint, + SPORT_PATH_POINT_SIZE, +) +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="stand_up", id=1), + TestOption(name="stand_down", id=2), + TestOption(name="move forward", id=3), + TestOption(name="move lateral", id=4), + TestOption(name="move rotate", id=5), + TestOption(name="stop_move", id=6), + TestOption(name="switch_gait", id=7), + TestOption(name="switch_gait", id=8), + TestOption(name="balanced stand", id=9), + TestOption(name="recovery", id=10), + TestOption(name="recovery", id=10), + TestOption(name="left flip", id=11), + TestOption(name="back flip", id=12), + TestOption(name="free walk", id=13), + TestOption(name="free bound", id=14), + TestOption(name="free avoid", id=15), + TestOption(name="walk stair", id=16), + TestOption(name="walk upright", id=17), + TestOption(name="cross step", id=18), + TestOption(name="free jump", id=19) +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"{option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + ChannelFactoryInitialize(0, sys.argv[1]) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = SportClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + while True: + + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.StandUp() + elif test_option.id == 2: + sport_client.StandDown() + elif test_option.id == 3: + sport_client.Move(0.3,0,0) + elif test_option.id == 4: + sport_client.Move(0,0.3,0) + elif test_option.id == 5: + sport_client.Move(0,0,0.5) + elif test_option.id == 6: + sport_client.StopMove() + elif test_option.id == 7: + sport_client.SwitchGait(0) + elif test_option.id == 8: + sport_client.SwitchGait(1) + elif test_option.id == 9: + sport_client.BalanceStand() + elif test_option.id == 10: + sport_client.RecoveryStand() + elif test_option.id == 11: + ret = sport_client.LeftFlip() + print("ret: ",ret) + elif test_option.id == 12: + ret = sport_client.BackFlip() + print("ret: ",ret) + elif test_option.id == 13: + ret = sport_client.FreeWalk(True) + print("ret: ",ret) + elif test_option.id == 14: + ret = sport_client.FreeBound(True) + print("ret: ",ret) + time.sleep(2) + ret = sport_client.FreeBound(False) + print("ret: ",ret) + elif test_option.id == 14: + ret = sport_client.FreeBound(True) + print("ret: ",ret) + time.sleep(2) + ret = sport_client.FreeBound(False) + print("ret: ",ret) + elif test_option.id == 15: + ret = sport_client.FreeAvoid(True) + print("ret: ",ret) + time.sleep(2) + ret = sport_client.FreeAvoid(False) + print("ret: ",ret) + elif test_option.id == 16: + ret = sport_client.WalkStair(True) + print("ret: ",ret) + time.sleep(10) + ret = sport_client.WalkStair(False) + print("ret: ",ret) + elif test_option.id == 17: + ret = sport_client.WalkUpright(True) + print("ret: ",ret) + time.sleep(4) + ret = sport_client.WalkUpright(False) + print("ret: ",ret) + elif test_option.id == 18: + ret = sport_client.CrossStep(True) + print("ret: ",ret) + time.sleep(4) + ret = sport_client.CrossStep(False) + print("ret: ",ret) + elif test_option.id == 19: + ret = sport_client.FreeJump(True) + print("ret: ",ret) + time.sleep(4) + ret = sport_client.FreeJump(False) + print("ret: ",ret) + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_utlidar_switch.py b/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_utlidar_switch.py new file mode 100644 index 0000000..09e0ce9 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/high_level/go2_utlidar_switch.py @@ -0,0 +1,39 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ +from unitree_sdk2py.idl.default import std_msgs_msg_dds__String_ + +class Custom: + def __init__(self): + # create publisher # + self.publisher = ChannelPublisher("rt/utlidar/switch", String_) + self.publisher.Init() + self.low_cmd = std_msgs_msg_dds__String_() + + def go2_utlidar_switch(self,status): + if status == "OFF": + self.low_cmd.data = "OFF" + elif status == "ON": + self.low_cmd.data = "ON" + + self.publisher.Write(self.low_cmd) + + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.go2_utlidar_switch("OFF") + + + diff --git a/external_dependencies/unitree_sdk2_python/example/go2/low_level/go2_stand_example.py b/external_dependencies/unitree_sdk2_python/example/go2/low_level/go2_stand_example.py new file mode 100644 index 0000000..f2241ce --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/low_level/go2_stand_example.py @@ -0,0 +1,176 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +import unitree_legged_const as go2 +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient +from unitree_sdk2py.go2.sport.sport_client import SportClient + +class Custom: + def __init__(self): + self.Kp = 60.0 + self.Kd = 5.0 + self.time_consume = 0 + self.rate_count = 0 + self.sin_count = 0 + self.motiontime = 0 + self.dt = 0.002 # 0.001~0.01 + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = None + + self._targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65] + self._targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3] + self._targetPos_3 = [-0.35, 1.36, -2.65, 0.35, 1.36, -2.65, + -0.5, 1.36, -2.65, 0.5, 1.36, -2.65] + + self.startPos = [0.0] * 12 + self.duration_1 = 500 + self.duration_2 = 500 + self.duration_3 = 1000 + self.duration_4 = 900 + self.percent_1 = 0 + self.percent_2 = 0 + self.percent_3 = 0 + self.percent_4 = 0 + + self.firstRun = True + self.done = False + + # thread handling + self.lowCmdWriteThreadPtr = None + + self.crc = CRC() + + # Public methods + def Init(self): + self.InitLowCmd() + + # create publisher # + self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10) + + self.sc = SportClient() + self.sc.SetTimeout(5.0) + self.sc.Init() + + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.sc.StandDown() + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=0.002, target=self.LowCmdWrite, name="writebasiccmd" + ) + self.lowCmdWriteThreadPtr.Start() + + # Private methods + def InitLowCmd(self): + self.low_cmd.head[0]=0xFE + self.low_cmd.head[1]=0xEF + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(20): + self.low_cmd.motor_cmd[i].mode = 0x01 # (PMSM) mode + self.low_cmd.motor_cmd[i].q= go2.PosStopF + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = go2.VelStopF + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + + def LowStateMessageHandler(self, msg: LowState_): + self.low_state = msg + # print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]]) + # print("IMU state: ", msg.imu_state) + # print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a) + + def LowCmdWrite(self): + + if self.firstRun: + for i in range(12): + self.startPos[i] = self.low_state.motor_state[i].q + self.firstRun = False + + self.percent_1 += 1.0 / self.duration_1 + self.percent_1 = min(self.percent_1, 1) + if self.percent_1 < 1: + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self._targetPos_1[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 <= 1): + self.percent_2 += 1.0 / self.duration_2 + self.percent_2 = min(self.percent_2, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self._targetPos_1[i] + self.percent_2 * self._targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1): + self.percent_3 += 1.0 / self.duration_3 + self.percent_3 = min(self.percent_3, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = self._targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1): + self.percent_4 += 1.0 / self.duration_4 + self.percent_4 = min(self.percent_4, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self._targetPos_2[i] + self.percent_4 * self._targetPos_3[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + if custom.percent_4 == 1.0: + time.sleep(1) + print("Done!") + sys.exit(-1) + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/go2/low_level/unitree_legged_const.py b/external_dependencies/unitree_sdk2_python/example/go2/low_level/unitree_legged_const.py new file mode 100644 index 0000000..153a051 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2/low_level/unitree_legged_const.py @@ -0,0 +1,20 @@ +LegID = { + "FR_0": 0, # Front right hip + "FR_1": 1, # Front right thigh + "FR_2": 2, # Front right calf + "FL_0": 3, + "FL_1": 4, + "FL_2": 5, + "RR_0": 6, + "RR_1": 7, + "RR_2": 8, + "RL_0": 9, + "RL_1": 10, + "RL_2": 11, +} + +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/example/go2w/high_level/go2w_sport_client.py b/external_dependencies/unitree_sdk2_python/example/go2w/high_level/go2w_sport_client.py new file mode 100644 index 0000000..824a882 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2w/high_level/go2w_sport_client.py @@ -0,0 +1,99 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ +from unitree_sdk2py.go2.sport.sport_client import SportClient +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="stand_up", id=1), + TestOption(name="stand_down", id=2), + TestOption(name="move", id=3), + TestOption(name="stop_move", id=4), + TestOption(name="speed_level", id=5), + TestOption(name="switch_gait", id=6), + TestOption(name="get_state", id=7), + TestOption(name="recovery", id=8), + TestOption(name="balance", id=9) +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"{option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(f"Usage: python3 {sys.argv[0]} networkInterface") + sys.exit(-1) + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + ChannelFactoryInitialize(0, sys.argv[1]) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = SportClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + while True: + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.StandUp() + elif test_option.id == 2: + sport_client.StandDown() + elif test_option.id == 3: + sport_client.Move(0.5,0,0) + elif test_option.id == 4: + sport_client.StopMove() + elif test_option.id == 5: + sport_client.SpeedLevel(1) + elif test_option.id == 6: + sport_client.SwitchGait(1) + elif test_option.id == 8: + sport_client.RecoveryStand() + elif test_option.id == 9: + sport_client.BalanceStand() + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/example/go2w/low_level/go2w_stand_example.py b/external_dependencies/unitree_sdk2_python/example/go2w/low_level/go2w_stand_example.py new file mode 100644 index 0000000..e3a54de --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2w/low_level/go2w_stand_example.py @@ -0,0 +1,196 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +import unitree_legged_const as go2w +from unitree_sdk2py.go2.robot_state.robot_state_client import RobotStateClient +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient +from unitree_sdk2py.go2.sport.sport_client import SportClient + +class Custom: + def __init__(self): + self.Kp = 70.0 + self.Kd = 5.0 + self.time_consume = 0 + self.rate_count = 0 + self.sin_count = 0 + self.motiontime = 0 + self.dt = 0.002 + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = None + + self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65] + + self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3] + + self.targetPos_3 = [-0.35, 1.36, -2.65, 0.35, 1.36, -2.65, + -0.5, 1.36, -2.65, 0.5, 1.36, -2.65] + + self.startPos = [0.0] * 12 + self.duration_1 = 500 + self.duration_2 = 500 + self.duration_3 = 2000 + self.duration_4 = 900 + self.percent_1 = 0 + self.percent_2 = 0 + self.percent_3 = 0 + self.percent_4 = 0 + + self.firstRun = True + self.done = False + + # thread handling + self.lowCmdWriteThreadPtr = None + + self.crc = CRC() + + # Public methods + def Init(self): + self.InitLowCmd() + + # create publisher # + self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10) + + self.sc = SportClient() + self.sc.SetTimeout(5.0) + self.sc.Init() + + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.sc.StandUp() + self.sc.StandDown() + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + name="writebasiccmd", interval=0.002, target=self.LowCmdWrite, + ) + self.lowCmdWriteThreadPtr.Start() + + def InitLowCmd(self): + self.low_cmd.head[0] = 0xFE + self.low_cmd.head[1] = 0xEF + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(20): + self.low_cmd.motor_cmd[i].mode = 0x01 + self.low_cmd.motor_cmd[i].q= go2w.PosStopF + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = go2w.VelStopF + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + + def LowStateMessageHandler(self, msg: LowState_): + self.low_state = msg + + def LowCmdWrite(self): + if self.firstRun: + for i in range(12): + self.startPos[i] = self.low_state.motor_state[i].q + self.firstRun = False + + self.percent_1 += 1.0 / self.duration_1 + self.percent_1 = min(self.percent_1, 1) + if self.percent_1 < 1: + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 <= 1): + self.percent_2 += 1.0 / self.duration_2 + self.percent_2 = min(self.percent_2, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1): + self.percent_3 += 1.0 / self.duration_3 + self.percent_3 = min(self.percent_3, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if self.percent_3 < 0.4: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0.0 + self.low_cmd.motor_cmd[i].dq = 3 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if 0.4 <= self.percent_3 < 0.8: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = -3 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if self.percent_3 >= 0.8: + for i in range(12, 16): + self.low_cmd.motor_cmd[i].q = 0 + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1): + self.percent_4 += 1.0 / self.duration_4 + self.percent_4 = min(self.percent_4, 1) + for i in range(12): + self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i] + self.low_cmd.motor_cmd[i].dq = 0 + self.low_cmd.motor_cmd[i].kp = self.Kp + self.low_cmd.motor_cmd[i].kd = self.Kd + self.low_cmd.motor_cmd[i].tau = 0 + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher.Write(self.low_cmd) + +if __name__ == '__main__': + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + if custom.percent_4 == 1.0: + time.sleep(1) + print("Done!") + sys.exit(-1) + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/go2w/low_level/unitree_legged_const.py b/external_dependencies/unitree_sdk2_python/example/go2w/low_level/unitree_legged_const.py new file mode 100644 index 0000000..7943e31 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/go2w/low_level/unitree_legged_const.py @@ -0,0 +1,24 @@ +LegID = { + "FR_0": 0, # Front right hip + "FR_1": 1, # Front right thigh + "FR_2": 2, # Front right calf + "FL_0": 3, + "FL_1": 4, + "FL_2": 5, + "RR_0": 6, + "RR_1": 7, + "RR_2": 8, + "RL_0": 9, + "RL_1": 10, + "RL_2": 11, + "FR_w": 12, # Front right wheel + "FL_w": 13, # Front left wheel + "RR_w": 14, # Rear right wheel + "RL_w": 15, # Rear left wheel +} + +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/example/h1/high_level/h1_loco_client_example.py b/external_dependencies/unitree_sdk2_python/example/h1/high_level/h1_loco_client_example.py new file mode 100644 index 0000000..4432d8a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/h1/high_level/h1_loco_client_example.py @@ -0,0 +1,96 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ +from unitree_sdk2py.h1.loco.h1_loco_client import LocoClient +import math +from dataclasses import dataclass + +@dataclass +class TestOption: + name: str + id: int + +option_list = [ + TestOption(name="damp", id=0), + TestOption(name="stand_up", id=1), + TestOption(name="move forward", id=3), + TestOption(name="move lateral", id=4), + TestOption(name="move rotate", id=5), + TestOption(name="low stand", id=6), + TestOption(name="high stand", id=7), + TestOption(name="zero torque", id=8) +] + +class UserInterface: + def __init__(self): + self.test_option_ = None + + def convert_to_int(self, input_str): + try: + return int(input_str) + except ValueError: + return None + + def terminal_handle(self): + input_str = input("Enter id or name: \n") + + if input_str == "list": + self.test_option_.name = None + self.test_option_.id = None + for option in option_list: + print(f"{option.name}, id: {option.id}") + return + + for option in option_list: + if input_str == option.name or self.convert_to_int(input_str) == option.id: + self.test_option_.name = option.name + self.test_option_.id = option.id + print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}") + return + + print("No matching test option found.") + +if __name__ == "__main__": + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + test_option = TestOption(name=None, id=None) + user_interface = UserInterface() + user_interface.test_option_ = test_option + + sport_client = LocoClient() + sport_client.SetTimeout(10.0) + sport_client.Init() + + while True: + + user_interface.terminal_handle() + + print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n") + + if test_option.id == 0: + sport_client.Damp() + elif test_option.id == 1: + sport_client.StandUp() + elif test_option.id == 3: + sport_client.Move(0.3,0,0) + elif test_option.id == 4: + sport_client.Move(0,0.3,0) + elif test_option.id == 5: + sport_client.Move(0,0,0.3) + elif test_option.id == 6: + sport_client.LowStand() + elif test_option.id == 7: + sport_client.HighStand() + elif test_option.id == 8: + sport_client.ZeroTorque() + + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/h1/low_level/h1_low_level_example.py b/external_dependencies/unitree_sdk2_python/example/h1/low_level/h1_low_level_example.py new file mode 100644 index 0000000..51a9111 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/h1/low_level/h1_low_level_example.py @@ -0,0 +1,167 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient +import unitree_legged_const as h1 +import numpy as np + +H1_NUM_MOTOR = 20 + +class H1JointIndex: + # Right leg + kRightHipYaw = 8 + kRightHipRoll = 0 + kRightHipPitch = 1 + kRightKnee = 2 + kRightAnkle = 11 + # Left leg + kLeftHipYaw = 7 + kLeftHipRoll = 3 + kLeftHipPitch = 4 + kLeftKnee = 5 + kLeftAnkle = 10 + + kWaistYaw = 6 + + kNotUsedJoint = 9 + + # Right arm + kRightShoulderPitch = 12 + kRightShoulderRoll = 13 + kRightShoulderYaw = 14 + kRightElbow = 15 + # Left arm + kLeftShoulderPitch = 16 + kLeftShoulderRoll = 17 + kLeftShoulderYaw = 18 + kLeftElbow = 19 + +class Custom: + def __init__(self): + self.time_ = 0.0 + self.control_dt_ = 0.01 + self.duration_ = 10.0 + self.counter_ = 0 + self.kp_low_ = 60.0 + self.kp_high_ = 200.0 + self.kd_low_ = 1.5 + self.kd_high_ = 5.0 + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.InitLowCmd() + self.low_state = None + self.crc = CRC() + + def Init(self): + # # create publisher # + self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher_.Init() + + # # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateHandler, 10) + + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + self.report_rpy_ptr_ = RecurrentThread( + interval=0.1, target=self.ReportRPY, name="report_rpy" + ) + + self.report_rpy_ptr_.Start() + + def is_weak_motor(self,motor_index): + return motor_index in { + H1JointIndex.kLeftAnkle, + H1JointIndex.kRightAnkle, + H1JointIndex.kRightShoulderPitch, + H1JointIndex.kRightShoulderRoll, + H1JointIndex.kRightShoulderYaw, + H1JointIndex.kRightElbow, + H1JointIndex.kLeftShoulderPitch, + H1JointIndex.kLeftShoulderRoll, + H1JointIndex.kLeftShoulderYaw, + H1JointIndex.kLeftElbow, + } + + def InitLowCmd(self): + self.low_cmd.head[0] = 0xFE + self.low_cmd.head[1] = 0xEF + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(H1_NUM_MOTOR): + if self.is_weak_motor(i): + self.low_cmd.motor_cmd[i].mode = 0x01 + else: + self.low_cmd.motor_cmd[i].mode = 0x0A + self.low_cmd.motor_cmd[i].q= h1.PosStopF + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = h1.VelStopF + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=self.control_dt_, target=self.LowCmdWrite, name="control" + ) + self.lowCmdWriteThreadPtr.Start() + + def LowStateHandler(self, msg: LowState_): + self.low_state = msg + + def ReportRPY(self): + print("rpy: [",self.low_state.imu_state.rpy[0],", " + ,self.low_state.imu_state.rpy[1],", " + ,self.low_state.imu_state.rpy[2],"]" + ) + + def LowCmdWrite(self): + self.time_ += self.control_dt_ + self.time_ = np.clip(self.time_ , 0.0, self.duration_) + + # set robot to zero posture + for i in range(H1_NUM_MOTOR): + ratio = self.time_ / self.duration_ + self.low_cmd.motor_cmd[i].tau = 0. + self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q + self.low_cmd.motor_cmd[i].dq = 0. + self.low_cmd.motor_cmd[i].kp = self.kp_low_ if self.is_weak_motor(i) else self.kp_high_ + self.low_cmd.motor_cmd[i].kd = self.kd_low_ if self.is_weak_motor(i) else self.kd_high_ + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher_.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + if custom.time_ == custom.duration_: + time.sleep(1) + print("Done!") + sys.exit(-1) + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/h1/low_level/unitree_legged_const.py b/external_dependencies/unitree_sdk2_python/example/h1/low_level/unitree_legged_const.py new file mode 100644 index 0000000..c9112ea --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/h1/low_level/unitree_legged_const.py @@ -0,0 +1,5 @@ +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/example/h1_2/low_level/h1_2_low_level_example.py b/external_dependencies/unitree_sdk2_python/example/h1_2/low_level/h1_2_low_level_example.py new file mode 100644 index 0000000..0959b32 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/h1_2/low_level/h1_2_low_level_example.py @@ -0,0 +1,201 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient + +import numpy as np + +H1_2_NUM_MOTOR = 27 + +class H1_2_JointIndex: + # legs + LeftHipYaw = 0 + LeftHipPitch = 1 + LeftHipRoll = 2 + LeftKnee = 3 + LeftAnklePitch = 4 + LeftAnkleB = 4 + LeftAnkleRoll = 5 + LeftAnkleA = 5 + RightHipYaw = 6 + RightHipPitch = 7 + RightHipRoll = 8 + RightKnee = 9 + RightAnklePitch = 10 + RightAnkleB = 10 + RightAnkleRoll = 11 + RightAnkleA = 11 + # torso + WaistYaw = 12 + # arms + LeftShoulderPitch = 13 + LeftShoulderRoll = 14 + LeftShoulderYaw = 15 + LeftElbow = 16 + LeftWristRoll = 17 + LeftWristPitch = 18 + LeftWristYaw = 19 + RightShoulderPitch = 20 + RightShoulderRoll = 21 + RightShoulderYaw = 22 + RightElbow = 23 + RightWristRoll = 24 + RightWristPitch = 25 + RightWristYaw = 26 + + +class Mode: + PR = 0 # Series Control for Pitch/Roll Joints + AB = 1 # Parallel Control for A/B Joints + +class Custom: + def __init__(self): + self.time_ = 0.0 + self.control_dt_ = 0.002 # [2ms] + self.duration_ = 3.0 # [3 s] + self.counter_ = 0 + self.mode_pr_ = Mode.PR + self.mode_machine_ = 0 + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = None + self.update_mode_machine_ = False + self.crc = CRC() + + def Init(self): + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + status, result = self.msc.CheckMode() + while result['name']: + self.msc.ReleaseMode() + status, result = self.msc.CheckMode() + time.sleep(1) + + # create publisher # + self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher_.Init() + + # create subscriber # + self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self.lowstate_subscriber.Init(self.LowStateHandler, 10) + + def Start(self): + self.lowCmdWriteThreadPtr = RecurrentThread( + interval=self.control_dt_, target=self.LowCmdWrite, name="control" + ) + while self.update_mode_machine_ == False: + time.sleep(1) + + if self.update_mode_machine_ == True: + self.lowCmdWriteThreadPtr.Start() + + def LowStateHandler(self, msg: LowState_): + self.low_state = msg + + if self.update_mode_machine_ == False: + self.mode_machine_ = self.low_state.mode_machine + self.update_mode_machine_ = True + + self.counter_ +=1 + if (self.counter_ % 500 == 0) : + self.counter_ = 0 + print(self.low_state.imu_state.rpy) + + def LowCmdWrite(self): + self.time_ += self.control_dt_ + self.low_cmd.mode_pr = Mode.PR + self.low_cmd.mode_machine = self.mode_machine_ + for i in range(H1_2_NUM_MOTOR): + ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) + self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable + self.low_cmd.motor_cmd[i].tau = 0.0 + self.low_cmd.motor_cmd[i].q = 0.0 + self.low_cmd.motor_cmd[i].dq = 0.0 + self.low_cmd.motor_cmd[i].kp = 100.0 if i < 13 else 50.0 + self.low_cmd.motor_cmd[i].kd = 1.0 + + if self.time_ < self.duration_ : + # [Stage 1]: set robot to zero posture + for i in range(H1_2_NUM_MOTOR): + ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) + self.low_cmd.mode_pr = Mode.PR + self.low_cmd.mode_machine = self.mode_machine_ + self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable + self.low_cmd.motor_cmd[i].tau = 0. + self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q + self.low_cmd.motor_cmd[i].dq = 0. + self.low_cmd.motor_cmd[i].kp = 100.0 if i < 13 else 50.0 + self.low_cmd.motor_cmd[i].kd = 1.0 + else : + # [Stage 2]: swing ankle using PR mode + max_P = 0.25 + max_R = 0.25 + t = self.time_ - self.duration_ + L_P_des = max_P * np.cos(2.0 * np.pi * t) + L_R_des = max_R * np.sin(2.0 * np.pi * t) + R_P_des = max_P * np.cos(2.0 * np.pi * t) + R_R_des = -max_R * np.sin(2.0 * np.pi * t) + + Kp_Pitch = 80 + Kd_Pitch = 1 + Kp_Roll = 80 + Kd_Roll = 1 + + self.low_cmd.mode_pr = Mode.PR + self.low_cmd.mode_machine = self.mode_machine_ + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].q = L_P_des + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].kp = Kp_Pitch + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].kd = Kd_Pitch + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].q = L_R_des + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].kp = Kp_Roll + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].kd = Kd_Roll + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].q = R_P_des + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].kp = Kp_Pitch + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].kd = Kd_Pitch + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].q = R_R_des + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].kp = Kp_Roll + self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].kd = Kd_Roll + + max_wrist_roll_angle = 0.5; # [rad] + WristRoll_des = max_wrist_roll_angle * np.sin(2.0 * np.pi * t) + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].q = WristRoll_des + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].kp = 50 + self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].kd = 1 + self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].q = WristRoll_des + self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].dq = 0 + self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].kp = 50 + self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].kd = 1 + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher_.Write(self.low_cmd) + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + custom.Start() + + while True: + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/helloworld/publisher.py b/external_dependencies/unitree_sdk2_python/example/helloworld/publisher.py new file mode 100644 index 0000000..cfe4f1f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/helloworld/publisher.py @@ -0,0 +1,28 @@ +import time + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from user_data import * + + +if __name__ == "__main__": + ChannelFactoryInitialize() + + # Create a publisher to publish the data defined in UserData class + pub = ChannelPublisher("topic", UserData) + pub.Init() + + for i in range(30): + # Create a Userdata message + msg = UserData(" ", 0) + msg.string_data = "Hello world" + msg.float_data = time.time() + + # Publish message + if pub.Write(msg, 0.5): + print("Publish success. msg:", msg) + else: + print("Waitting for subscriber.") + + time.sleep(1) + + pub.Close() diff --git a/external_dependencies/unitree_sdk2_python/example/helloworld/subscriber.py b/external_dependencies/unitree_sdk2_python/example/helloworld/subscriber.py new file mode 100644 index 0000000..a7d0b63 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/helloworld/subscriber.py @@ -0,0 +1,20 @@ +import time + +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from user_data import * + + +if __name__ == "__main__": + ChannelFactoryInitialize() + # Create a subscriber to subscribe the data defined in UserData class + sub = ChannelSubscriber("topic", UserData) + sub.Init() + + while True: + msg = sub.Read() + if msg is not None: + print("Subscribe success. msg:", msg) + else: + print("No data subscribed.") + break + sub.Close() diff --git a/external_dependencies/unitree_sdk2_python/example/helloworld/user_data.py b/external_dependencies/unitree_sdk2_python/example/helloworld/user_data.py new file mode 100644 index 0000000..9696ce3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/helloworld/user_data.py @@ -0,0 +1,9 @@ +from dataclasses import dataclass +from cyclonedds.idl import IdlStruct + + +# This class defines user data consisting of a float data and a string data +@dataclass +class UserData(IdlStruct, typename="UserData"): + string_data: str + float_data: float diff --git a/external_dependencies/unitree_sdk2_python/example/motionSwitcher/motion_switcher_example.py b/external_dependencies/unitree_sdk2_python/example/motionSwitcher/motion_switcher_example.py new file mode 100644 index 0000000..4f39f72 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/motionSwitcher/motion_switcher_example.py @@ -0,0 +1,36 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient + + +class Custom: + def __init__(self): + self.msc = MotionSwitcherClient() + self.msc.SetTimeout(5.0) + self.msc.Init() + + def selectMode(self,name): + ret = self.msc.SelectMode(name) + return ret + + +if __name__ == '__main__': + + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") + + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + selectMode = "ai" + # selectMode = "normal" + # selectMode = "advanced" + # selectMode = "ai-w" # for wheeled robot + ret = custom.selectMode(selectMode) + print("ret: ",ret) + diff --git a/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_move.py b/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_move.py new file mode 100644 index 0000000..284f8ea --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_move.py @@ -0,0 +1,35 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ObstaclesAvoidClient + +if __name__ == "__main__": + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + try: + client = ObstaclesAvoidClient() + client.SetTimeout(3.0) + client.Init() + + while not client.SwitchGet()[1]: + client.SwitchSet(True) + time.sleep(0.1) + + print("obstacles avoid switch on") + + client.UseRemoteCommandFromApi(True) + time.sleep(0.5) + client.Move(0.5, 0.0, 0.0) + time.sleep(1.0) # move 1s + client.Move(0.0, 0.0, 0.0) + client.UseRemoteCommandFromApi(False) + + except KeyboardInterrupt: + client.Move(0.0, 0.0, 0.0) + client.UseRemoteCommandFromApi(False) + print("exit!!") + \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_switch.py b/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_switch.py new file mode 100644 index 0000000..eb345c0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/obstacles_avoid/obstacles_avoid_switch.py @@ -0,0 +1,94 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ObstaclesAvoidClient + +if __name__ == "__main__": + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + client = ObstaclesAvoidClient() + client.SetTimeout(3.0) + client.Init() + + while True: + print("##################GetServerApiVersion###################") + code, serverAPiVersion = client.GetServerApiVersion() + if code != 0: + print("get server api error. code:", code) + else: + print("get server api version:", serverAPiVersion) + + if serverAPiVersion != client.GetApiVersion(): + print("api version not equal.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable) + + time.sleep(3) + + print("##################SwitchSet (on)###################") + code = client.SwitchSet(True) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable1 = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable1) + + time.sleep(3) + + print("##################SwitchSet (off)###################") + code = client.SwitchSet(False) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable1 = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable1) + + time.sleep(3) + + + print("##################SwitchSet (enable)###################") + + code = client.SwitchSet(enable) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success. enable:", enable) + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable) + + time.sleep(3) + \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/example/vui_client/vui_client_example.py b/external_dependencies/unitree_sdk2_python/example/vui_client/vui_client_example.py new file mode 100644 index 0000000..a5e6932 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/vui_client/vui_client_example.py @@ -0,0 +1,77 @@ +import time +import sys + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.vui.vui_client import VuiClient + +if __name__ == "__main__": + if len(sys.argv)>1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + client = VuiClient() + client.SetTimeout(3.0) + client.Init() + + for i in range(1, 11): + print("#################GetBrightness####################") + code, level = client.GetBrightness() + + if code != 0: + print("get brightness error. code:", code) + else: + print("get brightness success. level:", level) + + time.sleep(1) + + print("#################SetBrightness####################") + + code = client.SetBrightness(i) + + if code != 0: + print("set brightness error. code:", code) + else: + print("set brightness success. level:", i) + + time.sleep(1) + + print("#################SetBrightness 0####################") + + code = client.SetBrightness(0) + + if code != 0: + print("set brightness error. code:", code) + else: + print("set brightness 0 success.") + + for i in range(1, 11): + print("#################GetVolume####################") + code, level = client.GetVolume() + + if code != 0: + print("get volume error. code:", code) + else: + print("get volume success. level:", level) + + time.sleep(1) + + print("#################SetVolume####################") + + code = client.SetVolume(i) + + if code != 0: + print("set volume error. code:", code) + else: + print("set volume success. level:", i) + + time.sleep(1) + + print("#################SetVolume 0####################") + + code = client.SetVolume(0) + + if code != 0: + print("set volume error. code:", code) + else: + print("set volume 0 success.") diff --git a/external_dependencies/unitree_sdk2_python/example/wireless_controller/wireless_controller.py b/external_dependencies/unitree_sdk2_python/example/wireless_controller/wireless_controller.py new file mode 100644 index 0000000..995487f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/example/wireless_controller/wireless_controller.py @@ -0,0 +1,131 @@ +import time +import sys +import struct + +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize + +# Uncomment the following two lines when using Go2、Go2-W、B2、B2-W、H1 robot +# from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +# from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ + +# Uncomment the following two lines when using G1、H1-2 robot +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ + +class unitreeRemoteController: + def __init__(self): + # key + self.Lx = 0 + self.Rx = 0 + self.Ry = 0 + self.Ly = 0 + + # button + self.L1 = 0 + self.L2 = 0 + self.R1 = 0 + self.R2 = 0 + self.A = 0 + self.B = 0 + self.X = 0 + self.Y = 0 + self.Up = 0 + self.Down = 0 + self.Left = 0 + self.Right = 0 + self.Select = 0 + self.F1 = 0 + self.F3 = 0 + self.Start = 0 + + def parse_botton(self,data1,data2): + self.R1 = (data1 >> 0) & 1 + self.L1 = (data1 >> 1) & 1 + self.Start = (data1 >> 2) & 1 + self.Select = (data1 >> 3) & 1 + self.R2 = (data1 >> 4) & 1 + self.L2 = (data1 >> 5) & 1 + self.F1 = (data1 >> 6) & 1 + self.F3 = (data1 >> 7) & 1 + self.A = (data2 >> 0) & 1 + self.B = (data2 >> 1) & 1 + self.X = (data2 >> 2) & 1 + self.Y = (data2 >> 3) & 1 + self.Up = (data2 >> 4) & 1 + self.Right = (data2 >> 5) & 1 + self.Down = (data2 >> 6) & 1 + self.Left = (data2 >> 7) & 1 + + def parse_key(self,data): + lx_offset = 4 + self.Lx = struct.unpack('1: + ChannelFactoryInitialize(0, sys.argv[1]) + else: + ChannelFactoryInitialize(0) + + custom = Custom() + custom.Init() + + while True: + time.sleep(1) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/pyproject.toml b/external_dependencies/unitree_sdk2_python/pyproject.toml new file mode 100644 index 0000000..07de284 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools", "wheel"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/setup.py b/external_dependencies/unitree_sdk2_python/setup.py new file mode 100644 index 0000000..54d8cac --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/setup.py @@ -0,0 +1,21 @@ +from setuptools import setup, find_packages + +setup(name='unitree_sdk2py', + version='1.0.1', + author='UnitreeRobotics', + author_email='unitree@unitree.com', + long_description=open('README.md').read(), + long_description_content_type="text/markdown", + license="BSD-3-Clause", + packages=find_packages(include=['unitree_sdk2py','unitree_sdk2py.*']), + description='Unitree robot sdk version 2 for python', + project_urls={ + "Source Code": "https://github.com/unitreerobotics/unitree_sdk2_python", + }, + python_requires='>=3.8', + install_requires=[ + "cyclonedds==0.10.2", + "numpy", + "opencv-python", + ], + ) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/__init__.py new file mode 100644 index 0000000..f071aeb --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/__init__.py @@ -0,0 +1,10 @@ +from . import idl, utils, core, rpc, go2, b2 + +__all__ = [ + "idl" + "utils" + "core", + "rpc", + "go2", + "b2", +] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_api.py new file mode 100644 index 0000000..16f566b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_api.py @@ -0,0 +1,16 @@ +""" +" service name +""" +ROBOT_BACK_VIDEO_SERVICE_NAME = "back_videohub" + + +""" +" service api version +""" +ROBOT_BACK_VIDEO_API_VERSION = "1.0.0.0" + + +""" +" api id +""" +ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE = 1001 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_client.py new file mode 100644 index 0000000..e19c97f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/back_video/back_video_client.py @@ -0,0 +1,23 @@ +import json + +from ...rpc.client import Client +from .back_video_api import * + + +""" +" class FrontVideoClient +""" +class BackVideoClient(Client): + def __init__(self): + super().__init__(ROBOT_BACK_VIDEO_SERVICE_NAME, False) + + + def Init(self): + # set api version + self._SetApiVerson(ROBOT_BACK_VIDEO_API_VERSION) + # regist api + self._RegistApi(ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE, 0) + + # 1001 + def GetImageSample(self): + return self._CallBinary(ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE, []) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_api.py new file mode 100644 index 0000000..6f4717a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_api.py @@ -0,0 +1,16 @@ +""" +" service name +""" +ROBOT_FRONT_VIDEO_SERVICE_NAME = "front_videohub" + + +""" +" service api version +""" +ROBOT_FRONT_VIDEO_API_VERSION = "1.0.0.0" + + +""" +" api id +""" +ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE = 1001 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_client.py new file mode 100644 index 0000000..5ba065a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/front_video/front_video_client.py @@ -0,0 +1,23 @@ +import json + +from ...rpc.client import Client +from .front_video_api import * + + +""" +" class FrontVideoClient +""" +class FrontVideoClient(Client): + def __init__(self): + super().__init__(ROBOT_FRONT_VIDEO_SERVICE_NAME, False) + + + def Init(self): + # set api version + self._SetApiVerson(ROBOT_FRONT_VIDEO_API_VERSION) + # regist api + self._RegistApi(ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE, 0) + + # 1001 + def GetImageSample(self): + return self._CallBinary(ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE, []) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_api.py new file mode 100644 index 0000000..fde54df --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_api.py @@ -0,0 +1,25 @@ +""" +" service name +""" +ROBOT_STATE_SERVICE_NAME = "robot_state" + + +""" +" service api version +""" +ROBOT_STATE_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +ROBOT_STATE_API_ID_SERVICE_SWITCH = 1001 +ROBOT_STATE_API_ID_REPORT_FREQ = 1002 +ROBOT_STATE_API_ID_SERVICE_LIST = 1003 + + +""" +" error code +""" +ROBOT_STATE_ERR_SERVICE_SWITCH = 5201 +ROBOT_STATE_ERR_SERVICE_PROTECTED = 5202 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_client.py new file mode 100644 index 0000000..5b2e74d --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/robot_state/robot_state_client.py @@ -0,0 +1,84 @@ +import json + +from ...rpc.client import Client +from ...rpc.client_internal import * +from .robot_state_api import * + + +""" +" class ServiceState +""" +class ServiceState: + def __init__(self, name: str = None, status: int = None, protect: bool = None): + self.name = name + self.status = status + self.protect = protect + +""" +" class RobotStateClient +""" +class RobotStateClient(Client): + def __init__(self): + super().__init__(ROBOT_STATE_SERVICE_NAME, False) + + def Init(self): + # set api version + self._SetApiVerson(ROBOT_STATE_API_VERSION) + # regist api + self._RegistApi(ROBOT_STATE_API_ID_SERVICE_SWITCH, 0) + self._RegistApi(ROBOT_STATE_API_ID_REPORT_FREQ, 0) + self._RegistApi(ROBOT_STATE_API_ID_SERVICE_LIST, 0) + + def ServiceList(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_LIST, parameter) + + if code != 0: + return code, None + + lst = [] + + d = json.loads(data) + for t in d: + s = ServiceState() + s.name = t["name"] + s.status = t["status"] + s.protect = t["protect"] + lst.append(s) + + return code, lst + + + def ServiceSwitch(self, name: str, switch: bool): + p = {} + p["name"] = name + p["switch"] = int(switch) + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_SWITCH, parameter) + + if code != 0: + return code + + d = json.loads(data) + + status = d["status"] + + if status == 5: + return ROBOT_STATE_ERR_SERVICE_PROTECTED + + if status != 0 and status != 1: + return ROBOT_STATE_ERR_SERVICE_SWITCH + + return code + + def SetReportFreq(self, interval: int, duration: int): + p = {} + p["interval"] = interval + p["duration"] = duration + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_REPORT_FREQ, p) + return code diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_api.py new file mode 100644 index 0000000..72520b6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_api.py @@ -0,0 +1,58 @@ +""" +" service name +""" +SPORT_SERVICE_NAME = "sport" + + +""" +" service api version +""" +SPORT_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +SPORT_API_ID_DAMP = 1001; +SPORT_API_ID_BALANCESTAND = 1002; +SPORT_API_ID_STOPMOVE = 1003; +SPORT_API_ID_STANDUP = 1004; +SPORT_API_ID_STANDDOWN = 1005; +SPORT_API_ID_RECOVERYSTAND = 1006; +SPORT_API_ID_EULER = 1007; +SPORT_API_ID_MOVE = 1008; +SPORT_API_ID_SIT = 1009; +SPORT_API_ID_RISESIT = 1010; +SPORT_API_ID_SWITCHGAIT = 1011; +SPORT_API_ID_TRIGGER = 1012; +SPORT_API_ID_BODYHEIGHT = 1013; +SPORT_API_ID_FOOTRAISEHEIGHT = 1014; +SPORT_API_ID_SPEEDLEVEL = 1015; +SPORT_API_ID_HELLO = 1016; +SPORT_API_ID_STRETCH = 1017; +SPORT_API_ID_TRAJECTORYFOLLOW = 1018; +SPORT_API_ID_CONTINUOUSGAIT = 1019; +SPORT_API_ID_CONTENT = 1020; +SPORT_API_ID_WALLOW = 1021; +SPORT_API_ID_DANCE1 = 1022; +SPORT_API_ID_DANCE2 = 1023; +SPORT_API_ID_GETBODYHEIGHT = 1024; +SPORT_API_ID_GETFOOTRAISEHEIGHT = 1025; +SPORT_API_ID_GETSPEEDLEVEL = 1026; +SPORT_API_ID_SWITCHJOYSTICK = 1027; +SPORT_API_ID_POSE = 1028; +SPORT_API_ID_FRONTJUMP = 1031; +SPORT_API_ID_ECONOMICGAIT = 1035; +SPORT_API_ID_MOVETOPOS = 1036; +SPORT_API_ID_SWITCHEULERMODE = 1037; +SPORT_API_ID_SWITCHMOVEMODE = 1038; + + +""" +" error code +""" +# client side +SPORT_ERR_CLIENT_POINT_PATH = 4101 +# server side +SPORT_ERR_SERVER_OVERTIME = 4201 +SPORT_ERR_SERVER_NOT_INIT = 4202 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_client.py new file mode 100644 index 0000000..9ca34a3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/sport/sport_client.py @@ -0,0 +1,241 @@ +import json + +from ...rpc.client import Client +from .sport_api import * + +""" +" SPORT_PATH_POINT_SIZE +""" +SPORT_PATH_POINT_SIZE = 30 + + +""" +" class PathPoint +""" +class PathPoint: + def __init__(self, timeFromStart: float, x: float, y: float, yaw: float, vx: float, vy: float, vyaw: float): + self.timeFromStart = timeFromStart + self.x = x + self.y = y + self.yaw = yaw + self.vx = vx + self.vy = vy + self.vyaw = vyaw + + +""" +" class SportClient +""" +class SportClient(Client): + def __init__(self, enableLease: bool = False): + super().__init__(SPORT_SERVICE_NAME, enableLease) + + + def Init(self): + # set api version + self._SetApiVerson(SPORT_API_VERSION) + + # regist api + self._RegistApi(SPORT_API_ID_DAMP, 0) + self._RegistApi(SPORT_API_ID_BALANCESTAND, 0) + self._RegistApi(SPORT_API_ID_STOPMOVE, 0) + self._RegistApi(SPORT_API_ID_STANDUP, 0) + self._RegistApi(SPORT_API_ID_STANDDOWN, 0) + self._RegistApi(SPORT_API_ID_RECOVERYSTAND, 0) + self._RegistApi(SPORT_API_ID_EULER, 0) + self._RegistApi(SPORT_API_ID_MOVE, 0) + self._RegistApi(SPORT_API_ID_SIT, 0) + self._RegistApi(SPORT_API_ID_SWITCHGAIT, 0) + self._RegistApi(SPORT_API_ID_BODYHEIGHT, 0) + self._RegistApi(SPORT_API_ID_FOOTRAISEHEIGHT, 0) + self._RegistApi(SPORT_API_ID_SPEEDLEVEL, 0) + self._RegistApi(SPORT_API_ID_TRAJECTORYFOLLOW, 0) + self._RegistApi(SPORT_API_ID_CONTINUOUSGAIT, 0) + self._RegistApi(SPORT_API_ID_MOVETOPOS, 0) + self._RegistApi(SPORT_API_ID_FRONTJUMP, 0) + self._RegistApi(SPORT_API_ID_ECONOMICGAIT, 0) + self._RegistApi(SPORT_API_ID_POSE, 0) + self._RegistApi(SPORT_API_ID_SWITCHEULERMODE, 0) + self._RegistApi(SPORT_API_ID_SWITCHMOVEMODE, 0) + + # 1001 + def Damp(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_DAMP, parameter) + return code + + # 1002 + def BalanceStand(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_BALANCESTAND, parameter) + return code + + # 1003 + def StopMove(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STOPMOVE, parameter) + return code + + # 1004 + def StandUp(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STANDUP, parameter) + return code + + # 1005 + def StandDown(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STANDDOWN, parameter) + return code + + # 1006 + def RecoveryStand(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_RECOVERYSTAND, parameter) + return code + + # 1007 + def Euler(self, roll: float, pitch: float, yaw: float): + p = {} + p["x"] = roll + p["y"] = pitch + p["z"] = yaw + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_EULER, parameter) + return code + + # 1008 + def Move(self, vx: float, vy: float, vyaw: float): + p = {} + p["x"] = vx + p["y"] = vy + p["z"] = vyaw + parameter = json.dumps(p) + code = self._CallNoReply(SPORT_API_ID_MOVE, parameter) + return code + + # 1009 + def Sit(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SIT, parameter) + return code + + # 1011 + def SwitchGait(self, t: int): + p = {} + p["data"] = t + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SWITCHGAIT, parameter) + return code + + # 1013 + def BodyHeight(self, height: float): + p = {} + p["data"] = height + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_BODYHEIGHT, parameter) + return code + + # 1014 + def FootRaiseHeight(self, height: float): + p = {} + p["data"] = height + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FOOTRAISEHEIGHT, parameter) + return code + + # 1015 + def SpeedLevel(self, level: int): + p = {} + p["data"] = level + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SPEEDLEVEL, parameter) + return code + + # 1018 + def TrajectoryFollow(self, path: list): + l = len(path) + if l != SPORT_PATH_POINT_SIZE: + return SPORT_ERR_CLIENT_POINT_PATH + + path_p = [] + for i in range(l): + point = path[i] + p = {} + p["t_from_start"] = point.timeFromStart + p["x"] = point.x + p["y"] = point.y + p["yaw"] = point.yaw + p["vx"] = point.vx + p["vy"] = point.vy + p["vyaw"] = point.vyaw + path_p.append(p) + + parameter = json.dumps(path_p) + code = self._CallNoReply(SPORT_API_ID_TRAJECTORYFOLLOW, parameter) + return code + + # 1019 + def ContinuousGait(self, flag: int): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_CONTINUOUSGAIT, parameter) + return code + + # 1036 + def MoveToPos(self, x: float, y: float, yaw: float): + p = {} + p["x"] = x + p["y"] = y + p["yaw"] = yaw + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_MOVETOPOS, parameter) + return code + + # 1031 + def FrontJump(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FRONTJUMP, parameter) + return code + + + # 1035 + def EconomicGait(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_ECONOMICGAIT, parameter) + return code + + # 1028 + def Pose(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_POSE, parameter) + return code + + # 1037 + def SwitchEulerMode(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SWITCHEULERMODE, parameter) + return code + + # 1038 + def SwitchMoveMode(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SWITCHMOVEMODE, parameter) + return code diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_api.py new file mode 100644 index 0000000..d5dcd34 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_api.py @@ -0,0 +1,21 @@ +""" +" service name +""" +VUI_SERVICE_NAME = "vui" + + +""" +" service api version +""" +VUI_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +VUI_API_ID_SETSWITCH = 1001 +VUI_API_ID_GETSWITCH = 1002 +VUI_API_ID_SETVOLUME = 1003 +VUI_API_ID_GETVOLUME = 1004 +VUI_API_ID_SETBRIGHTNESS = 1005 +VUI_API_ID_GETBRIGHTNESS = 1006 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_client.py new file mode 100644 index 0000000..234f285 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/b2/vui/vui_client.py @@ -0,0 +1,86 @@ +import json + +from ...rpc.client import Client +from .vui_api import * + + +""" +" class VideoClient +""" +class VuiClient(Client): + def __init__(self): + super().__init__(VUI_SERVICE_NAME, False) + + def Init(self): + # set api version + self._SetApiVerson(VUI_API_VERSION) + # regist api + self._RegistApi(VUI_API_ID_SETSWITCH, 0) + self._RegistApi(VUI_API_ID_GETSWITCH, 0) + self._RegistApi(VUI_API_ID_SETVOLUME, 0) + self._RegistApi(VUI_API_ID_GETVOLUME, 0) + self._RegistApi(VUI_API_ID_SETBRIGHTNESS, 0) + self._RegistApi(VUI_API_ID_GETBRIGHTNESS, 0) + + # 1001 + def SetSwitch(self, enable: int): + p = {} + p["enable"] = enable + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETSWITCH, parameter) + return code + + # 1002 + def GetSwitch(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETSWITCH, parameter) + if code == 0: + d = json.loads(data) + return code, d["enable"] + else: + return code, None + + # 1003 + def SetVolume(self, level: int): + p = {} + p["volume"] = level + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETVOLUME, parameter) + return code + + # 1006 + def GetVolume(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETVOLUME, parameter) + if code == 0: + d = json.loads(data) + return code, d["volume"] + else: + return code, None + + # 1005 + def SetBrightness(self, level: int): + p = {} + p["brightness"] = level + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETBRIGHTNESS, parameter) + return code + + # 1006 + def GetBrightness(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETBRIGHTNESS, parameter) + if code == 0: + d = json.loads(data) + return code, d["brightness"] + else: + return code, None \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py new file mode 100644 index 0000000..ddb9a42 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py @@ -0,0 +1,29 @@ +""" +" service name +""" +MOTION_SWITCHER_SERVICE_NAME = "motion_switcher" + + +""" +" service api version +""" +MOTION_SWITCHER_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +MOTION_SWITCHER_API_ID_CHECK_MODE = 1001 +MOTION_SWITCHER_API_ID_SELECT_MODE = 1002 +MOTION_SWITCHER_API_ID_RELEASE_MODE = 1003 +MOTION_SWITCHER_API_ID_SET_SILENT = 1004 +MOTION_SWITCHER_API_ID_GET_SILENT = 1005 + +# """ +# " error code +# """ +# # client side +# SPORT_ERR_CLIENT_POINT_PATH = 4101 +# # server side +# SPORT_ERR_SERVER_OVERTIME = 4201 +# SPORT_ERR_SERVER_NOT_INIT = 4202 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py new file mode 100644 index 0000000..1ad411a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py @@ -0,0 +1,51 @@ +import json + +from ...rpc.client import Client +from .motion_switcher_api import * + +""" +" class MotionSwitcherClient +""" +class MotionSwitcherClient(Client): + def __init__(self): + super().__init__(MOTION_SWITCHER_SERVICE_NAME, False) + + + def Init(self): + # set api version + self._SetApiVerson(MOTION_SWITCHER_API_VERSION) + + # regist api + self._RegistApi(MOTION_SWITCHER_API_ID_CHECK_MODE, 0) + self._RegistApi(MOTION_SWITCHER_API_ID_SELECT_MODE, 0) + self._RegistApi(MOTION_SWITCHER_API_ID_RELEASE_MODE, 0) + self._RegistApi(MOTION_SWITCHER_API_ID_SET_SILENT, 0) + self._RegistApi(MOTION_SWITCHER_API_ID_GET_SILENT, 0) + + # 1001 + def CheckMode(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(MOTION_SWITCHER_API_ID_CHECK_MODE, parameter) + if code == 0: + return code, json.loads(data) + else: + return code, None + + # 1002 + def SelectMode(self, nameOrAlias): + p = {} + p["name"] = nameOrAlias + parameter = json.dumps(p) + code, data = self._Call(MOTION_SWITCHER_API_ID_SELECT_MODE, parameter) + + return code, None + + # 1003 + def ReleaseMode(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(MOTION_SWITCHER_API_ID_RELEASE_MODE, parameter) + + return code, None + \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel.py new file mode 100644 index 0000000..daacc63 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel.py @@ -0,0 +1,290 @@ +import time +from typing import Any, Callable +from threading import Thread, Event + +from cyclonedds.domain import Domain, DomainParticipant +from cyclonedds.internal import dds_c_t +from cyclonedds.pub import DataWriter +from cyclonedds.sub import DataReader +from cyclonedds.topic import Topic +from cyclonedds.qos import Qos +from cyclonedds.core import DDSException, Listener +from cyclonedds.util import duration +from cyclonedds.internal import dds_c_t, InvalidSample + +# for channel config +from .channel_config import ChannelConfigAutoDetermine, ChannelConfigHasInterface + +# for singleton +from ..utils.singleton import Singleton +from ..utils.bqueue import BQueue + + +""" +" class ChannelReader +""" + +""" +" class Channel +""" +class Channel: + + """ + " internal class __Reader + """ + class __Reader: + def __init__(self): + self.__reader = None + self.__handler = None + self.__queue = None + self.__queueEnable = False + self.__threadEvent = None + self.__threadReader = None + + def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, handler: Callable = None, queueLen: int = 0): + if handler is None: + self.__reader = DataReader(participant, topic, qos) + else: + self.__handler = handler + if queueLen > 0: + self.__queueEnable = True + self.__queue = BQueue(queueLen) + self.__threadEvent = Event() + self.__threadReader = Thread(target=self.__ChannelReaderThreadFunc, name="ch_reader", daemon=True) + self.__threadReader.start() + self.__reader = DataReader(participant, topic, qos, Listener(on_data_available=self.__OnDataAvailable)) + + def Read(self, timeout: float = None): + sample = None + try: + if timeout is None: + sample = self.__reader.take_one() + else: + sample = self.__reader.take_one(timeout=duration(seconds=timeout)) + except DDSException as e: + print("[Reader] catch DDSException msg:", e.msg) + except TimeoutError as e: + print("[Reader] take sample timeout") + except: + print("[Reader] take sample error") + + return sample + + def Close(self): + if self.__reader is not None: + del self.__reader + + if self.__queueEnable: + self.__threadEvent.set() + self.__queue.Interrupt() + self.__queue.Clear() + self.__threadReader.join() + + def __OnDataAvailable(self, reader: DataReader): + samples = [] + try: + samples = reader.take(1) + except DDSException as e: + print("[Reader] catch DDSException error. msg:", e.msg) + return + except TimeoutError as e: + print("[Reader] take sample timeout") + return + except: + print("[Reader] take sample error") + return + + if samples is None: + return + + # check invalid sample + sample = samples[0] + if isinstance(sample, InvalidSample): + return + + # do sample + if self.__queueEnable: + self.__queue.Put(sample) + else: + self.__handler(sample) + + def __ChannelReaderThreadFunc(self): + while not self.__threadEvent.is_set(): + sample = self.__queue.Get() + if sample is not None: + self.__handler(sample) + + """ + " internal class __Writer + """ + class __Writer: + def __init__(self): + self.__writer = None + self.__publication_matched_count = 0 + + def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None): + self.__writer = DataWriter(participant, topic, qos, Listener(on_publication_matched=self.__OnPublicationMatched)) + time.sleep(0.2) + + def Write(self, sample: Any, timeout: float = None): + waitsec = 0.0 if timeout is None else timeout + + # check publication_matched_count + while waitsec > 0.0 and self.__publication_matched_count == 0: + time.sleep(0.1) + waitsec = waitsec - 0.1 + # print(time.time()) + + # check waitsec + if timeout is not None and waitsec <= 0.0: + return False + + try: + self.__writer.write(sample) + except DDSException as e: + print("[Writer] catch DDSException error. msg:", e.msg) + return False + except Exception as e: + print("[Writer] write sample error. msg:", e.args()) + return False + + return True + + def Close(self): + if self.__writer is not None: + del self.__writer + + def __OnPublicationMatched(self, writer: DataWriter, status: dds_c_t.publication_matched_status): + self.__publication_matched_count = status.current_count + + + # channel __init__ + def __init__(self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None): + self.__reader = self.__Reader() + self.__writer = self.__Writer() + self.__participant = participant + self.__topic = Topic(self.__participant, name, type, qos) + + def SetWriter(self, qos: Qos = None): + self.__writer.Init(self.__participant, self.__topic, qos) + + def SetReader(self, qos: Qos = None, handler: Callable = None, queueLen: int = 0): + self.__reader.Init(self.__participant, self.__topic, qos, handler, queueLen) + + def Write(self, sample: Any, timeout: float = None): + return self.__writer.Write(sample, timeout) + + def Read(self, timeout: float = None): + return self.__reader.Read(timeout) + + def CloseReader(self): + self.__reader.Close() + + def CloseWriter(self): + self.__writer.Close() + + +""" +" class ChannelFactory +""" +class ChannelFactory(Singleton): + __domain = None + __participant = None + __qos = None + + def __init__(self): + super().__init__() + + def Init(self, id: int, networkInterface: str = None, qos: Qos = None): + config = None + # choose config + if networkInterface is None: + config = ChannelConfigAutoDetermine + else: + config = ChannelConfigHasInterface.replace('$__IF_NAME__$', networkInterface) + + try: + self.__domain = Domain(id, config) + except DDSException as e: + print("[ChannelFactory] create domain error. msg:", e.msg) + return False + except: + print("[ChannelFactory] create domain error.") + return False + + try: + self.__participant = DomainParticipant(id) + except DDSException as e: + print("[ChannelFactory] create domain participant error. msg:", e.msg) + return False + except: + print("[ChannelFactory] create domain participant error") + return False + + self.__qos = qos + + return True + + def CreateChannel(self, name: str, type: Any): + return Channel(self.__participant, name, type, self.__qos) + + def CreateSendChannel(self, name: str, type: Any): + channel = self.CreateChannel(name, type) + channel.SetWriter(None) + return channel + + def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queueLen: int = 0): + channel = self.CreateChannel(name, type) + channel.SetReader(None, handler, queueLen) + return channel + + +""" +" class ChannelPublisher +""" +class ChannelPublisher: + def __init__(self, name: str, type: Any): + factory = ChannelFactory() + self.__channel = factory.CreateChannel(name, type) + self.__inited = False + + def Init(self): + if not self.__inited: + self.__channel.SetWriter(None) + self.__inited = True + + def Close(self): + self.__channel.CloseWriter() + self.__inited = False + + def Write(self, sample: Any, timeout: float = None): + return self.__channel.Write(sample, timeout) + +""" +" class ChannelSubscriber +""" +class ChannelSubscriber: + def __init__(self, name: str, type: Any): + factory = ChannelFactory() + self.__channel = factory.CreateChannel(name, type) + self.__inited = False + + def Init(self, handler: Callable = None, queueLen: int = 0): + if not self.__inited: + self.__channel.SetReader(None, handler, queueLen) + self.__inited = True + + def Close(self): + self.__channel.CloseReader() + self.__inited = False + + def Read(self, timeout: float = None): + return self.__channel.Read(timeout) + +""" +" function ChannelFactoryInitialize. used to intialize channel everenment. +""" +def ChannelFactoryInitialize(id: int = 0, networkInterface: str = None): + factory = ChannelFactory() + if not factory.Init(id, networkInterface): + raise Exception("channel factory init error.") diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_config.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_config.py new file mode 100644 index 0000000..19a67a4 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_config.py @@ -0,0 +1,25 @@ +ChannelConfigHasInterface = ''' + + + + + + + + + config + /tmp/cdds.LOG + + + ''' + +ChannelConfigAutoDetermine = ''' + + + + + + + + + ''' diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_name.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_name.py new file mode 100644 index 0000000..722e408 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/core/channel_name.py @@ -0,0 +1,34 @@ +from enum import Enum + +""" +" Enum ChannelType +""" +class ChannelType(Enum): + SEND = 0 + RECV = 1 + +""" +" function GetClientChannelName +""" +def GetClientChannelName(serviceName: str, channelType: ChannelType): + name = "rt/api/" + serviceName + + if channelType == ChannelType.SEND: + name += "/request" + else: + name += "/response" + + return name + +""" +" function GetClientChannelName +""" +def GetServerChannelName(serviceName: str, channelType: ChannelType): + name = "rt/api/" + serviceName + + if channelType == ChannelType.SEND: + name += "/response" + else: + name += "/request" + + return name \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_api.py new file mode 100644 index 0000000..9aba780 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_api.py @@ -0,0 +1,24 @@ +""" +" service name +""" +AUDIO_SERVICE_NAME = "voice" + +""" +" service api version +""" +AUDIO_API_VERSION = "1.0.0.0" + +""" +" api id +""" +ROBOT_API_ID_AUDIO_TTS = 1001 +ROBOT_API_ID_AUDIO_ASR = 1002 +ROBOT_API_ID_AUDIO_START_PLAY = 1003 +ROBOT_API_ID_AUDIO_STOP_PLAY = 1004 +ROBOT_API_ID_AUDIO_GET_VOLUME = 1005 +ROBOT_API_ID_AUDIO_SET_VOLUME = 1006 +ROBOT_API_ID_AUDIO_SET_RGB_LED = 1010 + +""" +" error code +""" \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_client.py new file mode 100644 index 0000000..d8d0bc3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/audio/g1_audio_client.py @@ -0,0 +1,62 @@ +import json + +from ...rpc.client import Client +from .g1_audio_api import * + +""" +" class SportClient +""" +class AudioClient(Client): + def __init__(self): + super().__init__(AUDIO_SERVICE_NAME, False) + self.tts_index = 0 + + def Init(self): + # set api version + self._SetApiVerson(AUDIO_API_VERSION) + + # regist api + self._RegistApi(ROBOT_API_ID_AUDIO_TTS, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_ASR, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_START_PLAY, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_STOP_PLAY, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_GET_VOLUME, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_SET_VOLUME, 0) + self._RegistApi(ROBOT_API_ID_AUDIO_SET_RGB_LED, 0) + + ## API Call ## + def TtsMaker(self, text: str, speaker_id: int): + self.tts_index += self.tts_index + p = {} + p["index"] = self.tts_index + p["text"] = text + p["speaker_id"] = speaker_id + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_AUDIO_TTS, parameter) + return code + + def GetVolume(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_AUDIO_GET_VOLUME, parameter) + if code == 0: + return code, json.loads(data) + else: + return code, None + + def SetVolume(self, volume: int): + p = {} + p["volume"] = volume + # p["name"] = 'volume' + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_AUDIO_SET_VOLUME, parameter) + return code + + def LedControl(self, R: int, G: int, B: int): + p = {} + p["R"] = R + p["G"] = G + p["B"] = B + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_AUDIO_SET_RGB_LED, parameter) + return code diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_api.py new file mode 100644 index 0000000..937de19 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_api.py @@ -0,0 +1,32 @@ +""" +" service name +""" +LOCO_SERVICE_NAME = "loco" + + +""" +" service api version +""" +LOCO_API_VERSION = "1.0.0.0" + + +""" +" api id +""" +ROBOT_API_ID_LOCO_GET_FSM_ID = 7001 +ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002 +ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003 +ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004 +ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005 +ROBOT_API_ID_LOCO_GET_PHASE = 7006 # deprecated + +ROBOT_API_ID_LOCO_SET_FSM_ID = 7101 +ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102 +ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103 +ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104 +ROBOT_API_ID_LOCO_SET_VELOCITY = 7105 +ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106 + +""" +" error code +""" \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_client.py new file mode 100644 index 0000000..9d3a632 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/g1/loco/g1_loco_client.py @@ -0,0 +1,127 @@ +import json + +from ...rpc.client import Client +from .g1_loco_api import * + +""" +" class SportClient +""" +class LocoClient(Client): + def __init__(self): + super().__init__(LOCO_SERVICE_NAME, False) + self.first_shake_hand_stage_ = -1 + + def Init(self): + # set api version + self._SetApiVerson(LOCO_API_VERSION) + + # regist api + self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated + + self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_VELOCITY, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_ARM_TASK, 0) + + # 7101 + def SetFsmId(self, fsm_id: int): + p = {} + p["data"] = fsm_id + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter) + return code + + # 7102 + def SetBalanceMode(self, balance_mode: int): + p = {} + p["data"] = balance_mode + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter) + return code + + # 7104 + def SetStandHeight(self, stand_height: float): + p = {} + p["data"] = stand_height + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter) + return code + + # 7105 + def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0): + p = {} + velocity = [vx,vy,omega] + p["velocity"] = velocity + p["duration"] = duration + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter) + return code + + # 7106 + def SetTaskId(self, task_id: float): + p = {} + p["data"] = task_id + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter) + return code + + def Damp(self): + self.SetFsmId(1) + + def Start(self): + self.SetFsmId(200) + + def Squat2StandUp(self): + self.SetFsmId(706) + + def Lie2StandUp(self): + self.SetFsmId(702) + + def Sit(self): + self.SetFsmId(3) + + def StandUp2Squat(self): + self.SetFsmId(706) + + def ZeroTorque(self): + self.SetFsmId(0) + + def StopMove(self): + self.SetVelocity(0., 0., 0.) + + def HighStand(self): + UINT32_MAX = (1 << 32) - 1 + self.SetStandHeight(UINT32_MAX) + + def LowStand(self): + UINT32_MIN = 0 + self.SetStandHeight(UINT32_MIN) + + def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False): + duration = 864000.0 if continous_move else 1 + self.SetVelocity(vx, vy, vyaw, duration) + + def BalanceStand(self, balance_mode: int): + self.SetBalanceMode(balance_mode) + + def WaveHand(self, turn_flag: bool = False): + self.SetTaskId(1 if turn_flag else 0) + + def ShakeHand(self, stage: int = -1): + if stage == 0: + self.first_shake_hand_stage_ = False + self.SetTaskId(2) + elif stage == 1: + self.first_shake_hand_stage_ = True + self.SetTaskId(3) + else: + self.first_shake_hand_stage_ = not self.first_shake_hand_stage_ + return self.SetTaskId(3 if self.first_shake_hand_stage_ else 2) + \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_api.py new file mode 100644 index 0000000..ad9dc06 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_api.py @@ -0,0 +1,19 @@ +""" +" service name +""" +OBSTACLES_AVOID_SERVICE_NAME = "obstacles_avoid" + + +""" +" service api version +""" +OBSTACLES_AVOID_API_VERSION = "1.0.0.2" + + +""" +" api id +""" +OBSTACLES_AVOID_API_ID_SWITCH_SET = 1001 +OBSTACLES_AVOID_API_ID_SWITCH_GET = 1002 +OBSTACLES_AVOID_API_ID_MOVE = 1003 +OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API = 1004 \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_client.py new file mode 100644 index 0000000..4d7b62b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_client.py @@ -0,0 +1,60 @@ +import json + +from ...rpc.client import Client +from .obstacles_avoid_api import * + + +""" +" class ObstaclesAvoidClient +""" +class ObstaclesAvoidClient(Client): + def __init__(self): + super().__init__(OBSTACLES_AVOID_SERVICE_NAME, False) + + def Init(self): + # set api version + self._SetApiVerson(OBSTACLES_AVOID_API_VERSION) + # regist api + self._RegistApi(OBSTACLES_AVOID_API_ID_SWITCH_SET, 0) + self._RegistApi(OBSTACLES_AVOID_API_ID_SWITCH_GET, 0) + self._RegistApi(OBSTACLES_AVOID_API_ID_MOVE, 0) + self._RegistApi(OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API, 0) + + # 1001 + def SwitchSet(self, on: bool): + p = {} + p["enable"] = on + parameter = json.dumps(p) + + code, data = self._Call(OBSTACLES_AVOID_API_ID_SWITCH_SET, parameter) + return code + + # 1002 + def SwitchGet(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(OBSTACLES_AVOID_API_ID_SWITCH_GET, parameter) + if code == 0: + d = json.loads(data) + return code, d["enable"] + else: + return code, None + + # 1003 + def Move(self, vx: float, vy: float, vyaw: float): + p = {} + p["x"] = vx + p["y"] = vy + p["yaw"] = vyaw + p["mode"] = 0 + parameter = json.dumps(p) + code = self._CallNoReply(OBSTACLES_AVOID_API_ID_MOVE, parameter) + return code + + def UseRemoteCommandFromApi(self, isRemoteCommandsFromApi: bool): + p = {} + p["is_remote_commands_from_api"] = isRemoteCommandsFromApi + parameter = json.dumps(p) + code, data = self._Call(OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API, parameter) + return code \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_api.py new file mode 100644 index 0000000..fde54df --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_api.py @@ -0,0 +1,25 @@ +""" +" service name +""" +ROBOT_STATE_SERVICE_NAME = "robot_state" + + +""" +" service api version +""" +ROBOT_STATE_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +ROBOT_STATE_API_ID_SERVICE_SWITCH = 1001 +ROBOT_STATE_API_ID_REPORT_FREQ = 1002 +ROBOT_STATE_API_ID_SERVICE_LIST = 1003 + + +""" +" error code +""" +ROBOT_STATE_ERR_SERVICE_SWITCH = 5201 +ROBOT_STATE_ERR_SERVICE_PROTECTED = 5202 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_client.py new file mode 100644 index 0000000..097a891 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/robot_state/robot_state_client.py @@ -0,0 +1,84 @@ +import json + +from ...rpc.client import Client +from ...rpc.internal import * +from .robot_state_api import * + + +""" +" class ServiceState +""" +class ServiceState: + def __init__(self, name: str = None, status: int = None, protect: bool = None): + self.name = name + self.status = status + self.protect = protect + +""" +" class RobotStateClient +""" +class RobotStateClient(Client): + def __init__(self): + super().__init__(ROBOT_STATE_SERVICE_NAME, False) + + def Init(self): + # set api version + self._SetApiVerson(ROBOT_STATE_API_VERSION) + # regist api + self._RegistApi(ROBOT_STATE_API_ID_SERVICE_SWITCH, 0) + self._RegistApi(ROBOT_STATE_API_ID_REPORT_FREQ, 0) + self._RegistApi(ROBOT_STATE_API_ID_SERVICE_LIST, 0) + + def ServiceList(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_LIST, parameter) + + if code != 0: + return code, None + + lst = [] + + d = json.loads(data) + for t in d: + s = ServiceState() + s.name = t["name"] + s.status = t["status"] + s.protect = t["protect"] + lst.append(s) + + return code, lst + + + def ServiceSwitch(self, name: str, switch: bool): + p = {} + p["name"] = name + p["switch"] = int(switch) + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_SWITCH, parameter) + + if code != 0: + return code + + d = json.loads(data) + + status = d["status"] + + if status == 5: + return ROBOT_STATE_ERR_SERVICE_PROTECTED + + if status != 0 and status != 1: + return ROBOT_STATE_ERR_SERVICE_SWITCH + + return code + + def SetReportFreq(self, interval: int, duration: int): + p = {} + p["interval"] = interval + p["duration"] = duration + parameter = json.dumps(p) + + code, data = self._Call(ROBOT_STATE_API_ID_REPORT_FREQ, p) + return code diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_api.py new file mode 100644 index 0000000..bfca62a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_api.py @@ -0,0 +1,74 @@ +""" +" service name +""" +SPORT_SERVICE_NAME = "sport" + + +""" +" service api version +""" +SPORT_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +SPORT_API_ID_DAMP = 1001 +SPORT_API_ID_BALANCESTAND = 1002 +SPORT_API_ID_STOPMOVE = 1003 +SPORT_API_ID_STANDUP = 1004 +SPORT_API_ID_STANDDOWN = 1005 +SPORT_API_ID_RECOVERYSTAND = 1006 +SPORT_API_ID_EULER = 1007 +SPORT_API_ID_MOVE = 1008 +SPORT_API_ID_SIT = 1009 +SPORT_API_ID_RISESIT = 1010 +SPORT_API_ID_SWITCHGAIT = 1011 +SPORT_API_ID_TRIGGER = 1012 +SPORT_API_ID_BODYHEIGHT = 1013 +SPORT_API_ID_FOOTRAISEHEIGHT = 1014 +SPORT_API_ID_SPEEDLEVEL = 1015 +SPORT_API_ID_HELLO = 1016 +SPORT_API_ID_STRETCH = 1017 +SPORT_API_ID_TRAJECTORYFOLLOW = 1018 +SPORT_API_ID_CONTINUOUSGAIT = 1019 +SPORT_API_ID_CONTENT = 1020 +SPORT_API_ID_WALLOW = 1021 +SPORT_API_ID_DANCE1 = 1022 +SPORT_API_ID_DANCE2 = 1023 +SPORT_API_ID_GETBODYHEIGHT = 1024 +SPORT_API_ID_GETFOOTRAISEHEIGHT = 1025 +SPORT_API_ID_GETSPEEDLEVEL = 1026 +SPORT_API_ID_SWITCHJOYSTICK = 1027 +SPORT_API_ID_POSE = 1028 +SPORT_API_ID_SCRAPE = 1029 +SPORT_API_ID_FRONTFLIP = 1030 +SPORT_API_ID_FRONTJUMP = 1031 +SPORT_API_ID_FRONTPOUNCE = 1032 +SPORT_API_ID_WIGGLEHIPS = 1033 +SPORT_API_ID_GETSTATE = 1034 +SPORT_API_ID_ECONOMICGAIT = 1035 +SPORT_API_ID_HEART = 1036 +ROBOT_SPORT_API_ID_DANCE3 = 1037 +ROBOT_SPORT_API_ID_DANCE4 = 1038 +ROBOT_SPORT_API_ID_HOPSPINLEFT = 1039 +ROBOT_SPORT_API_ID_HOPSPINRIGHT = 1040 + +ROBOT_SPORT_API_ID_LEFTFLIP = 1042 +ROBOT_SPORT_API_ID_BACKFLIP = 1044 +ROBOT_SPORT_API_ID_FREEWALK = 1045 +ROBOT_SPORT_API_ID_FREEBOUND = 1046 +ROBOT_SPORT_API_ID_FREEJUMP = 1047 +ROBOT_SPORT_API_ID_FREEAVOID = 1048 +ROBOT_SPORT_API_ID_WALKSTAIR = 1049 +ROBOT_SPORT_API_ID_WALKUPRIGHT = 1050 +ROBOT_SPORT_API_ID_CROSSSTEP = 1051 + +""" +" error code +""" +# client side +SPORT_ERR_CLIENT_POINT_PATH = 4101 +# server side +SPORT_ERR_SERVER_OVERTIME = 4201 +SPORT_ERR_SERVER_NOT_INIT = 4202 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_client.py new file mode 100644 index 0000000..d058e51 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/sport/sport_client.py @@ -0,0 +1,446 @@ +import json + +from ...rpc.client import Client +from .sport_api import * + +""" +" SPORT_PATH_POINT_SIZE +""" +SPORT_PATH_POINT_SIZE = 30 + + +""" +" class PathPoint +""" +class PathPoint: + def __init__(self, timeFromStart: float, x: float, y: float, yaw: float, vx: float, vy: float, vyaw: float): + self.timeFromStart = timeFromStart + self.x = x + self.y = y + self.yaw = yaw + self.vx = vx + self.vy = vy + self.vyaw = vyaw + + +""" +" class SportClient +""" +class SportClient(Client): + def __init__(self, enableLease: bool = False): + super().__init__(SPORT_SERVICE_NAME, enableLease) + + + def Init(self): + # set api version + self._SetApiVerson(SPORT_API_VERSION) + + # regist api + self._RegistApi(SPORT_API_ID_DAMP, 0) + self._RegistApi(SPORT_API_ID_BALANCESTAND, 0) + self._RegistApi(SPORT_API_ID_STOPMOVE, 0) + self._RegistApi(SPORT_API_ID_STANDUP, 0) + self._RegistApi(SPORT_API_ID_STANDDOWN, 0) + self._RegistApi(SPORT_API_ID_RECOVERYSTAND, 0) + self._RegistApi(SPORT_API_ID_EULER, 0) + self._RegistApi(SPORT_API_ID_MOVE, 0) + self._RegistApi(SPORT_API_ID_SIT, 0) + self._RegistApi(SPORT_API_ID_RISESIT, 0) + self._RegistApi(SPORT_API_ID_SWITCHGAIT, 0) + self._RegistApi(SPORT_API_ID_TRIGGER, 0) + self._RegistApi(SPORT_API_ID_BODYHEIGHT, 0) + self._RegistApi(SPORT_API_ID_FOOTRAISEHEIGHT, 0) + self._RegistApi(SPORT_API_ID_SPEEDLEVEL, 0) + self._RegistApi(SPORT_API_ID_HELLO, 0) + self._RegistApi(SPORT_API_ID_STRETCH, 0) + self._RegistApi(SPORT_API_ID_TRAJECTORYFOLLOW, 0) + self._RegistApi(SPORT_API_ID_CONTINUOUSGAIT, 0) + # self._RegistApi(SPORT_API_ID_CONTENT, 0) + self._RegistApi(SPORT_API_ID_WALLOW, 0) + self._RegistApi(SPORT_API_ID_DANCE1, 0) + self._RegistApi(SPORT_API_ID_DANCE2, 0) + # self._RegistApi(SPORT_API_ID_GETBODYHEIGHT, 0) + # self._RegistApi(SPORT_API_ID_GETFOOTRAISEHEIGHT, 0) + # self._RegistApi(SPORT_API_ID_GETSPEEDLEVEL, 0) + self._RegistApi(SPORT_API_ID_SWITCHJOYSTICK, 0) + self._RegistApi(SPORT_API_ID_POSE, 0) + self._RegistApi(SPORT_API_ID_SCRAPE, 0) + self._RegistApi(SPORT_API_ID_FRONTFLIP, 0) + self._RegistApi(SPORT_API_ID_FRONTJUMP, 0) + self._RegistApi(SPORT_API_ID_FRONTPOUNCE, 0) + self._RegistApi(SPORT_API_ID_WIGGLEHIPS, 0) + self._RegistApi(SPORT_API_ID_GETSTATE, 0) + self._RegistApi(SPORT_API_ID_ECONOMICGAIT, 0) + self._RegistApi(SPORT_API_ID_HEART, 0) + + self._RegistApi(ROBOT_SPORT_API_ID_LEFTFLIP, 0) + self._RegistApi(ROBOT_SPORT_API_ID_BACKFLIP, 0) + self._RegistApi(ROBOT_SPORT_API_ID_FREEWALK, 0) + self._RegistApi(ROBOT_SPORT_API_ID_FREEBOUND, 0) + self._RegistApi(ROBOT_SPORT_API_ID_FREEJUMP, 0) + self._RegistApi(ROBOT_SPORT_API_ID_FREEAVOID, 0) + self._RegistApi(ROBOT_SPORT_API_ID_WALKSTAIR, 0) + self._RegistApi(ROBOT_SPORT_API_ID_WALKUPRIGHT, 0) + self._RegistApi(ROBOT_SPORT_API_ID_CROSSSTEP, 0) + + # 1001 + def Damp(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_DAMP, parameter) + return code + + # 1002 + def BalanceStand(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_BALANCESTAND, parameter) + return code + + # 1003 + def StopMove(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STOPMOVE, parameter) + return code + + # 1004 + def StandUp(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STANDUP, parameter) + return code + + # 1005 + def StandDown(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STANDDOWN, parameter) + return code + + # 1006 + def RecoveryStand(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_RECOVERYSTAND, parameter) + return code + + # 1007 + def Euler(self, roll: float, pitch: float, yaw: float): + p = {} + p["x"] = roll + p["y"] = pitch + p["z"] = yaw + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_EULER, parameter) + return code + + # 1008 + def Move(self, vx: float, vy: float, vyaw: float): + p = {} + p["x"] = vx + p["y"] = vy + p["z"] = vyaw + parameter = json.dumps(p) + code = self._CallNoReply(SPORT_API_ID_MOVE, parameter) + return code + + # 1009 + def Sit(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SIT, parameter) + return code + + #1010 + def RiseSit(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_RISESIT, parameter) + return code + + # 1011 + def SwitchGait(self, t: int): + p = {} + p["data"] = t + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SWITCHGAIT, parameter) + return code + + # 1012 + def Trigger(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_TRIGGER, parameter) + return code + + # 1013 + def BodyHeight(self, height: float): + p = {} + p["data"] = height + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_BODYHEIGHT, parameter) + return code + + # 1014 + def FootRaiseHeight(self, height: float): + p = {} + p["data"] = height + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FOOTRAISEHEIGHT, parameter) + return code + + # 1015 + def SpeedLevel(self, level: int): + p = {} + p["data"] = level + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SPEEDLEVEL, parameter) + return code + + # 1016 + def Hello(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_HELLO, parameter) + return code + + # 1017 + def Stretch(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_STRETCH, parameter) + return code + + # 1018 + def TrajectoryFollow(self, path: list): + l = len(path) + if l != SPORT_PATH_POINT_SIZE: + return SPORT_ERR_CLIENT_POINT_PATH + + path_p = [] + for i in range(l): + point = path[i] + p = {} + p["t_from_start"] = point.timeFromStart + p["x"] = point.x + p["y"] = point.y + p["yaw"] = point.yaw + p["vx"] = point.vx + p["vy"] = point.vy + p["vyaw"] = point.vyaw + path_p.append(p) + + parameter = json.dumps(path_p) + code = self._CallNoReply(SPORT_API_ID_TRAJECTORYFOLLOW, parameter) + return code + + # 1019 + def ContinuousGait(self, flag: int): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_CONTINUOUSGAIT, parameter) + return code + + # # 1020 + # def Content(self): + # p = {} + # parameter = json.dumps(p) + # code, data = self._Call(SPORT_API_ID_CONTENT, parameter) + # return code + + # 1021 + def Wallow(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_WALLOW, parameter) + return code + + # 1022 + def Dance1(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_DANCE1, parameter) + return code + + # 1023 + def Dance2(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_DANCE2, parameter) + return code + + # 1025 + def GetFootRaiseHeight(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(SPORT_API_ID_GETFOOTRAISEHEIGHT, parameter) + + if code == 0: + d = json.loads(data) + return code, d["data"] + else: + return code, None + + + # 1026 + def GetSpeedLevel(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(SPORT_API_ID_GETSPEEDLEVEL, parameter) + + if code == 0: + d = json.loads(data) + return code, d["data"] + else: + return code, None + + # 1027 + def SwitchJoystick(self, on: bool): + p = {} + p["data"] = on + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SWITCHJOYSTICK, parameter) + return code + + # 1028 + def Pose(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_POSE, parameter) + return code + + # 1029 + def Scrape(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_SCRAPE, parameter) + return code + + # 1030 + def FrontFlip(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FRONTFLIP, parameter) + return code + + # 1031 + def FrontJump(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FRONTJUMP, parameter) + return code + + # 1032 + def FrontPounce(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_FRONTPOUNCE, parameter) + return code + + # 1033 + def WiggleHips(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_WIGGLEHIPS, parameter) + return code + + # 1034 + def GetState(self, keys: list): + parameter = json.dumps(keys) + code, data = self._Call(SPORT_API_ID_GETSTATE, parameter) + if code == 0: + return code, json.loads(data) + else: + return code, None + + # 1035 + def EconomicGait(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_ECONOMICGAIT, parameter) + return code + + # 1036 + def Heart(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(SPORT_API_ID_HEART, parameter) + return code + + # 1042 + def LeftFlip(self): + p = {} + p["data"] = True + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_LEFTFLIP, parameter) + return code + + # 1044 + def BackFlip(self): + p = {} + p["data"] = True + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_BACKFLIP, parameter) + return code + + # 1045 + def FreeWalk(self, flag: bool): + p = {} + p["data"] = True + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_FREEWALK, parameter) + return code + + # 1046 + def FreeBound(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_FREEBOUND, parameter) + return code + + # 1047 + def FreeJump(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_FREEJUMP, parameter) + return code + + # 1048 + def FreeAvoid(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_FREEAVOID, parameter) + return code + + # 1049 + def WalkStair(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_WALKSTAIR, parameter) + return code + + # 1050 + def WalkUpright(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_WALKUPRIGHT, parameter) + return code + + # 1051 + def CrossStep(self, flag: bool): + p = {} + p["data"] = flag + parameter = json.dumps(p) + code, data = self._Call(ROBOT_SPORT_API_ID_CROSSSTEP, parameter) + return code \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_api.py new file mode 100644 index 0000000..a4cb1b6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_api.py @@ -0,0 +1,16 @@ +""" +" service name +""" +VIDEO_SERVICE_NAME = "videohub" + + +""" +" service api version +""" +VIDEO_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +VIDEO_API_ID_GETIMAGESAMPLE = 1001 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_client.py new file mode 100644 index 0000000..79e1fb5 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/video/video_client.py @@ -0,0 +1,23 @@ +import json + +from ...rpc.client import Client +from .video_api import * + + +""" +" class VideoClient +""" +class VideoClient(Client): + def __init__(self): + super().__init__(VIDEO_SERVICE_NAME, False) + + + def Init(self): + # set api version + self._SetApiVerson(VIDEO_API_VERSION) + # regist api + self._RegistApi(VIDEO_API_ID_GETIMAGESAMPLE, 0) + + # 1001 + def GetImageSample(self): + return self._CallBinary(VIDEO_API_ID_GETIMAGESAMPLE, []) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_api.py new file mode 100644 index 0000000..d5dcd34 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_api.py @@ -0,0 +1,21 @@ +""" +" service name +""" +VUI_SERVICE_NAME = "vui" + + +""" +" service api version +""" +VUI_API_VERSION = "1.0.0.1" + + +""" +" api id +""" +VUI_API_ID_SETSWITCH = 1001 +VUI_API_ID_GETSWITCH = 1002 +VUI_API_ID_SETVOLUME = 1003 +VUI_API_ID_GETVOLUME = 1004 +VUI_API_ID_SETBRIGHTNESS = 1005 +VUI_API_ID_GETBRIGHTNESS = 1006 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_client.py new file mode 100644 index 0000000..234f285 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/go2/vui/vui_client.py @@ -0,0 +1,86 @@ +import json + +from ...rpc.client import Client +from .vui_api import * + + +""" +" class VideoClient +""" +class VuiClient(Client): + def __init__(self): + super().__init__(VUI_SERVICE_NAME, False) + + def Init(self): + # set api version + self._SetApiVerson(VUI_API_VERSION) + # regist api + self._RegistApi(VUI_API_ID_SETSWITCH, 0) + self._RegistApi(VUI_API_ID_GETSWITCH, 0) + self._RegistApi(VUI_API_ID_SETVOLUME, 0) + self._RegistApi(VUI_API_ID_GETVOLUME, 0) + self._RegistApi(VUI_API_ID_SETBRIGHTNESS, 0) + self._RegistApi(VUI_API_ID_GETBRIGHTNESS, 0) + + # 1001 + def SetSwitch(self, enable: int): + p = {} + p["enable"] = enable + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETSWITCH, parameter) + return code + + # 1002 + def GetSwitch(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETSWITCH, parameter) + if code == 0: + d = json.loads(data) + return code, d["enable"] + else: + return code, None + + # 1003 + def SetVolume(self, level: int): + p = {} + p["volume"] = level + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETVOLUME, parameter) + return code + + # 1006 + def GetVolume(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETVOLUME, parameter) + if code == 0: + d = json.loads(data) + return code, d["volume"] + else: + return code, None + + # 1005 + def SetBrightness(self, level: int): + p = {} + p["brightness"] = level + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_SETBRIGHTNESS, parameter) + return code + + # 1006 + def GetBrightness(self): + p = {} + parameter = json.dumps(p) + + code, data = self._Call(VUI_API_ID_GETBRIGHTNESS, parameter) + if code == 0: + d = json.loads(data) + return code, d["brightness"] + else: + return code, None \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_api.py new file mode 100644 index 0000000..bd8ddbe --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_api.py @@ -0,0 +1,31 @@ +""" +" service name +""" +LOCO_SERVICE_NAME = "loco" + + +""" +" service api version +""" +LOCO_API_VERSION = "2.0.0.0" + + +""" +" api id +""" +ROBOT_API_ID_LOCO_GET_FSM_ID = 8001 +ROBOT_API_ID_LOCO_GET_FSM_MODE = 8002 +ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 8003 +ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 8004 +ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 8005 +ROBOT_API_ID_LOCO_GET_PHASE = 8006 # deprecated + +ROBOT_API_ID_LOCO_SET_FSM_ID = 8101 +ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 8102 +ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 8103 +ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 8104 +ROBOT_API_ID_LOCO_SET_VELOCITY = 8105 + +""" +" error code +""" \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_client.py new file mode 100644 index 0000000..3bc01e3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/h1/loco/h1_loco_client.py @@ -0,0 +1,83 @@ +import json + +from ...rpc.client import Client +from .h1_loco_api import * + +""" +" class SportClient +""" +class LocoClient(Client): + def __init__(self): + super().__init__(LOCO_SERVICE_NAME, False) + + + def Init(self): + # set api version + self._SetApiVerson(LOCO_API_VERSION) + + # regist api + self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated + + self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SET_VELOCITY, 0) + + # 8101 + def SetFsmId(self, fsm_id: int): + p = {} + p["data"] = fsm_id + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter) + return code + + # 8104 + def SetStandHeight(self, stand_height: float): + p = {} + p["data"] = stand_height + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter) + return code + + # 8105 + def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0): + p = {} + velocity = [vx,vy,omega] + p["velocity"] = velocity + p["duration"] = duration + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter) + return code + + def Damp(self): + self.SetFsmId(1) + + def Start(self): + self.SetFsmId(204) + + def StandUp(self): + self.SetFsmId(2) + + def ZeroTorque(self): + self.SetFsmId(0) + + def StopMove(self): + self.SetVelocity(0., 0., 0.) + + def HighStand(self): + UINT32_MAX = (1 << 32) - 1 + self.SetStandHeight(UINT32_MAX) + + def LowStand(self): + UINT32_MIN = 0 + self.SetStandHeight(UINT32_MIN) + + def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False): + duration = 864000.0 if continous_move else 1 + self.SetVelocity(vx, vy, vyaw, duration) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/__init__.py new file mode 100644 index 0000000..38d706e --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/__init__.py @@ -0,0 +1,12 @@ +from .default import * +from . import builtin_interfaces, geometry_msgs, sensor_msgs, std_msgs, unitree_go, unitree_api + +__all__ = [ + "builtin_interfaces", + "geometry_msgs", + "sensor_msgs", + "std_msgs", + "unitree_go", + "unitree_hg", + "unitree_api", +] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/__init__.py new file mode 100644 index 0000000..c0397fd --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: builtin_interfaces + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/__init__.py new file mode 100644 index 0000000..b791398 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: builtin_interfaces.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/_Time_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/_Time_.py new file mode 100644 index 0000000..970c671 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/_Time_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: builtin_interfaces.msg.dds_ + IDL file: Time_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import builtin_interfaces + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Time_(idl.IdlStruct, typename="builtin_interfaces.msg.dds_.Time_"): + sec: types.int32 + nanosec: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/__init__.py new file mode 100644 index 0000000..c3f9730 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/builtin_interfaces/msg/dds_/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: builtin_interfaces.msg.dds_ + +""" + +from ._Time_ import Time_ +__all__ = ["Time_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/default.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/default.py new file mode 100644 index 0000000..778006a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/default.py @@ -0,0 +1,272 @@ +from .builtin_interfaces.msg.dds_ import * +from .std_msgs.msg.dds_ import * +from .geometry_msgs.msg.dds_ import * +from .nav_msgs.msg.dds_ import * +from .sensor_msgs.msg.dds_ import * +from .unitree_go.msg.dds_ import * +from .unitree_api.msg.dds_ import * + +# IDL for unitree_hg +from .unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_ +from .unitree_hg.msg.dds_ import LowState_ as HGLowState_ +from .unitree_hg.msg.dds_ import MotorCmd_ as HGMotorCmd_ +from .unitree_hg.msg.dds_ import MotorState_ as HGMotorState_ +from .unitree_hg.msg.dds_ import BmsState_ as HGBmsState_ +from .unitree_hg.msg.dds_ import IMUState_ as HGIMUState_ +from .unitree_hg.msg.dds_ import MainBoardState_ as HGMainBoardState_ +from .unitree_hg.msg.dds_ import PressSensorState_ as HGPressSensorState_ +from .unitree_hg.msg.dds_ import HandCmd_ as HGHandCmd_ +from .unitree_hg.msg.dds_ import HandState_ as HGHandState_ +from .unitree_hg.msg.dds_ import OdoState_ as HGOdoState_ + +""" +" builtin_interfaces_msgs.msg.dds_ dafault +""" +def builtin_interfaces_msgs_msg_dds__Time_(): + return Time_(0, 0) + + +""" +" std_msgs.msg.dds_ dafault +""" +def std_msgs_msg_dds__Header_(): + return Header_(builtin_interfaces_msgs_msg_dds__Time_(), "") + +def std_msgs_msg_dds__String_(): + return String_("") + + +""" +" geometry_msgs.msg.dds_ dafault +""" +def geometry_msgs_msg_dds__Point_(): + return Point_(0.0, 0.0, 0.0) + +def geometry_msgs_msg_dds__Point32_(): + return Point32_(0.0, 0.0, 0.0) + +def geometry_msgs_msg_dds__PointStamped_(): + return PointStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Point_()) + +def geometry_msgs_msg_dds__Quaternion_(): + return Quaternion_(0.0, 0.0, 0.0, 0.0) + +def geometry_msgs_msg_dds__Vector3_(): + return Vector3_(0.0, 0.0, 0.0) + +def geometry_msgs_msg_dds__Pose_(): + return Pose_(geometry_msgs_msg_dds__Point_(), geometry_msgs_msg_dds__Quaternion_()) + +def geometry_msgs_msg_dds__Pose2D_(): + return Pose2D_(0.0, 0.0, 0.0) + +def geometry_msgs_msg_dds__PoseStamped_(): + return PoseStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Pose_()) + +def geometry_msgs_msg_dds__PoseWithCovariance_(): + return PoseWithCovariance_(geometry_msgs_msg_dds__Pose_(), [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ]) + +def geometry_msgs_msg_dds__PoseWithCovarianceStamped_(): + return PoseWithCovarianceStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__PoseWithCovariance_()) + +def geometry_msgs_msg_dds__QuaternionStamped_(): + return QuaternionStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Quaternion_()) + +def geometry_msgs_msg_dds__Twist_(): + return Twist_(geometry_msgs_msg_dds__Vector3_(), geometry_msgs_msg_dds__Vector3_()) + +def geometry_msgs_msg_dds__TwistStamped_(): + return TwistStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Twist_()) + +def geometry_msgs_msg_dds__TwistWithCovariance_(): + return TwistWithCovariance_(geometry_msgs_msg_dds__Twist_(), [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ]) + +def geometry_msgs_msg_dds__TwistWithCovarianceStamped_(): + return TwistWithCovarianceStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__TwistWithCovariance_()) + + +""" +" nav_msgs.msg.dds_ dafault +""" +def nav_msgs_msg_dds__MapMetaData_(): + return MapMetaData_(builtin_interfaces_msgs_msg_dds__Time_(), 0, 0, geometry_msgs_msg_dds__Pose_()) + +def nav_msgs_msg_dds__OccupancyGrid_(): + return OccupancyGrid_(std_msgs_msg_dds__Header_(), nav_msgs_msg_dds__MapMetaData_(), []) + +def nav_msgs_msg_dds__Odometry_(): + return Odometry_(std_msgs_msg_dds__Header_(), "", geometry_msgs_msg_dds__PoseWithCovariance_(), + geometry_msgs_msg_dds__TwistWithCovariance_()) + + +""" +" sensor_msgs.msg.dds_ dafault +""" +def sensor_msgs_msg_dds__PointField_Constants_PointField_(): + return PointField_("", 0, 0, 0) + +def sensor_msgs_msg_dds__PointField_Constants_PointCloud2_(): + return PointCloud2_(std_msgs_msg_dds__Header_(), 0, 0, [], False, 0, 0, [], False) + + +""" +" unitree_go.msg.dds_ dafault +""" +def unitree_go_msg_dds__AudioData_(): + return AudioData_(0, []) + +def unitree_go_msg_dds__BmsCmd_(): + return BmsCmd_(0, [0, 0, 0]) + +def unitree_go_msg_dds__BmsState_(): + return BmsState_(0, 0, 0, 0, 0, 0, [0, 0], [0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + +def unitree_go_msg_dds__Error_(): + return Error_(0, 0) + +def unitree_go_msg_dds__Go2FrontVideoData_(): + return Go2FrontVideoData_(0, [], [], []) + +def unitree_go_msg_dds__HeightMap_(): + return HeightMap_(0.0, "", 0.0, 0, 0, [0.0, 0.0], []) + +def unitree_go_msg_dds__IMUState_(): + return IMUState_([0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], 0) + +def unitree_go_msg_dds__InterfaceConfig_(): + return InterfaceConfig_(0, 0, [0, 0]) + +def unitree_go_msg_dds__LidarState_(): + return LidarState_(0.0, "", "", "", 0.0, 0.0, 0, 0.0, 0.0, 0, 0, 0.0, 0.0, [0.0, 0.0, 0.0], 0.0, 0, 0) + +def unitree_go_msg_dds__MotorCmd_(): + return MotorCmd_(0, 0.0, 0.0, 0.0, 0.0, 0.0, [0, 0, 0]) + +def unitree_go_msg_dds__MotorState_(): + return MotorState_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, [0, 0]) + +def unitree_go_msg_dds__LowCmd_(): + return LowCmd_([0, 0], 0, 0, [0, 0], [0, 0], 0, [unitree_go_msg_dds__MotorCmd_() for i in range(20)], + unitree_go_msg_dds__BmsCmd_(), + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0], 0, 0, 0) + +def unitree_go_msg_dds__LowState_(): + return LowState_([0, 0], 0, 0, [0, 0], [0, 0], 0, unitree_go_msg_dds__IMUState_(), + [unitree_go_msg_dds__MotorState_() for i in range(20)], + unitree_go_msg_dds__BmsState_(), [0, 0, 0, 0], [0, 0, 0, 0], 0, + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + 0, 0, 0, 0, 0.0, 0.0, [0, 0, 0, 0], 0, 0) + +def unitree_go_msg_dds__Req_(): + return Req_("", "") + +def unitree_go_msg_dds__Res_(): + return Res_("", [], "") + +def unitree_go_msg_dds__TimeSpec_(): + return TimeSpec_(0, 0) + +def unitree_go_msg_dds__PathPoint_(): + return PathPoint_(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + +def unitree_go_msg_dds__SportModeState_(): + return SportModeState_(unitree_go_msg_dds__TimeSpec_(), 0, unitree_go_msg_dds__IMUState_(), + 0, 0, 0, 0.0, [0.0, 0.0, 0.0], 0.0, + [0.0, 0.0, 0.0], 0.0, [0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],[unitree_go_msg_dds__PathPoint_() for i in range(10)]) + +def unitree_go_msg_dds__UwbState_(): + return UwbState_([0, 0], 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, [0.0, 0.0], 0, 0, 0) + +def unitree_go_msg_dds__UwbSwitch_(): + return UwbSwitch_(0) + +def unitree_go_msg_dds__WirelessController_(): + return WirelessController_(0.0, 0.0, 0.0, 0.0, 0) + + +""" +" unitree_hg.msg.dds_ dafault +""" +def unitree_hg_msg_dds__BmsCmd_(): + return HGBmsCmd_(0, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + +def unitree_hg_msg_dds__BmsState_(): + return HGBmsState_(0, 0, 0, + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0], 0, 0, 0, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 0, 0, [0, 0, 0, 0, 0], [0, 0, 0]) + +def unitree_hg_msg_dds__IMUState_(): + return HGIMUState_([0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], 0) + +def unitree_hg_msg_dds__MotorCmd_(): + return HGMotorCmd_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0) + +def unitree_hg_msg_dds__MotorState_(): + return HGMotorState_(0, 0.0, 0.0, 0.0, 0.0, [0, 0], 0.0, [0, 0], 0, [0, 0, 0, 0]) + +def unitree_hg_msg_dds__MainBoardState_(): + return HGMainBoardState_([0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0, 0, 0]) + +def unitree_hg_msg_dds__LowCmd_(): + return HGLowCmd_(0, 0, [unitree_hg_msg_dds__MotorCmd_() for i in range(35)], [0, 0, 0, 0], 0) + +def unitree_hg_msg_dds__LowState_(): + return HGLowState_([0, 0], 0, 0, 0, unitree_hg_msg_dds__IMUState_(), + [unitree_hg_msg_dds__MotorState_() for i in range(35)], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0], 0) + +def unitree_hg_msg_dds__PressSensorState_(): + return HGPressSensorState_([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 0, 0) + +def unitree_hg_msg_dds__HandCmd_(): + return HGHandCmd_([unitree_hg_msg_dds__MotorCmd_() for i in range(7)], [0, 0, 0, 0]) + +def unitree_hg_msg_dds__HandState_(): + return HGHandState_([unitree_hg_msg_dds__MotorState_() for i in range(7)], + [unitree_hg_msg_dds__PressSensorState_() for i in range(7)], + unitree_hg_msg_dds__IMUState_(), + 0.0, 0.0, 0.0, 0.0, [0, 0], [0, 0]) + +def unitree_hg_msg_dds__OdoState_(): + return HGOdoState_([0, 0], 0, [0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], 0) + + +""" +" unitree_api.msg.dds_ dafault +""" +def unitree_api_msg_dds__RequestIdentity_(): + return RequestIdentity_(0, 0) + +def unitree_api_msg_dds__RequestLease_(): + return RequestLease_(0, unitree_hg_msg_dds__IMUState_(), [], ) + +def unitree_api_msg_dds__RequestPolicy_(): + return RequestPolicy_(0, False) + +def unitree_api_msg_dds__RequestHeader_(): + return RequestHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__RequestLease_(), + unitree_api_msg_dds__RequestPolicy_()) + +def unitree_api_msg_dds__Request_(): + return Request_(unitree_api_msg_dds__RequestHeader_(), "", []) + +def unitree_api_msg_dds__ResponseStatus_(): + return ResponseStatus_(0) + +def unitree_api_msg_dds__ResponseHeader_(): + return ResponseHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__ResponseStatus_()) + +def unitree_api_msg_dds__Response_(): + return Response_(unitree_api_msg_dds__ResponseHeader_(), "", [], 0, 0, [0, 0]) + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/__init__.py new file mode 100644 index 0000000..ab19108 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/__init__.py new file mode 100644 index 0000000..7d10dec --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point32_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point32_.py new file mode 100644 index 0000000..34eefd4 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point32_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Point32_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Point32_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Point32_"): + x: types.float32 + y: types.float32 + z: types.float32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PointStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PointStamped_.py new file mode 100644 index 0000000..19de6ae --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PointStamped_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: PointStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PointStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PointStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + point: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Point_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point_.py new file mode 100644 index 0000000..efbc562 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Point_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Point_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Point_"): + x: types.float64 + y: types.float64 + z: types.float64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose2D_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose2D_.py new file mode 100644 index 0000000..c223370 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose2D_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Pose2D_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Pose2D_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Pose2D_"): + x: types.float64 + y: types.float64 + theta: types.float64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseStamped_.py new file mode 100644 index 0000000..7864bb0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseStamped_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: PoseStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PoseStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PoseStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Pose_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovarianceStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovarianceStamped_.py new file mode 100644 index 0000000..843d4c8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovarianceStamped_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: PoseWithCovarianceStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PoseWithCovarianceStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PoseWithCovarianceStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.PoseWithCovariance_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovariance_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovariance_.py new file mode 100644 index 0000000..bd75301 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseWithCovariance_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: PoseWithCovariance_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PoseWithCovariance_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PoseWithCovariance_"): + pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Pose_' + covariance: types.array[types.float64, 36] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose_.py new file mode 100644 index 0000000..2ea78d1 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Pose_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Pose_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Pose_"): + position: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Point_' + orientation: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Quaternion_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_QuaternionStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_QuaternionStamped_.py new file mode 100644 index 0000000..ae2b360 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_QuaternionStamped_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: QuaternionStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class QuaternionStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.QuaternionStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + quaternion: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Quaternion_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Quaternion_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Quaternion_.py new file mode 100644 index 0000000..0a401fb --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Quaternion_.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Quaternion_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Quaternion_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Quaternion_"): + x: types.float64 + y: types.float64 + z: types.float64 + w: types.float64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistStamped_.py new file mode 100644 index 0000000..97aa7c9 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistStamped_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: TwistStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class TwistStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.TwistStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + twist: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Twist_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovarianceStamped_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovarianceStamped_.py new file mode 100644 index 0000000..d322b7f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovarianceStamped_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: TwistWithCovarianceStamped_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class TwistWithCovarianceStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.TwistWithCovarianceStamped_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + twist: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.TwistWithCovariance_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovariance_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovariance_.py new file mode 100644 index 0000000..7281bfc --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_TwistWithCovariance_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: TwistWithCovariance_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class TwistWithCovariance_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.TwistWithCovariance_"): + twist: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Twist_' + covariance: types.array[types.float64, 36] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Twist_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Twist_.py new file mode 100644 index 0000000..5cd3b23 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Twist_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Twist_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Twist_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Twist_"): + linear: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Vector3_' + angular: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Vector3_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Vector3_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Vector3_.py new file mode 100644 index 0000000..9cdeb0f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Vector3_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + IDL file: Vector3_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import geometry_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Vector3_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Vector3_"): + x: types.float64 + y: types.float64 + z: types.float64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/__init__.py new file mode 100644 index 0000000..68c4fe2 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/geometry_msgs/msg/dds_/__init__.py @@ -0,0 +1,23 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: geometry_msgs.msg.dds_ + +""" + +from ._Point32_ import Point32_ +from ._Point_ import Point_ +from ._PointStamped_ import PointStamped_ +from ._Pose2D_ import Pose2D_ +from ._Pose_ import Pose_ +from ._PoseStamped_ import PoseStamped_ +from ._PoseWithCovariance_ import PoseWithCovariance_ +from ._PoseWithCovarianceStamped_ import PoseWithCovarianceStamped_ +from ._Quaternion_ import Quaternion_ +from ._QuaternionStamped_ import QuaternionStamped_ +from ._Twist_ import Twist_ +from ._TwistStamped_ import TwistStamped_ +from ._TwistWithCovariance_ import TwistWithCovariance_ +from ._TwistWithCovarianceStamped_ import TwistWithCovarianceStamped_ +from ._Vector3_ import Vector3_ +__all__ = ["Point32_", "Point_", "PointStamped_", "Pose2D_", "Pose_", "PoseStamped_", "PoseWithCovariance_", "PoseWithCovarianceStamped_", "Quaternion_", "QuaternionStamped_", "Twist_", "TwistStamped_", "TwistWithCovariance_", "TwistWithCovarianceStamped_", "Vector3_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/__init__.py new file mode 100644 index 0000000..a2724c2 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/__init__.py new file mode 100644 index 0000000..d47683b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_MapMetaData_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_MapMetaData_.py new file mode 100644 index 0000000..9228cd6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_MapMetaData_.py @@ -0,0 +1,35 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs.msg.dds_ + IDL file: MapMetaData_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import nav_msgs + +# if TYPE_CHECKING: +# import builtin_interfaces.msg.dds_ +# import geometry_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MapMetaData_(idl.IdlStruct, typename="nav_msgs.msg.dds_.MapMetaData_"): + map_load_time: 'unitree_sdk2py.idl.builtin_interfaces.msg.dds_.Time_' + resolution: types.float32 + width: types.uint32 + height: types.uint32 + origin: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Pose_' + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_OccupancyGrid_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_OccupancyGrid_.py new file mode 100644 index 0000000..0274c58 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_OccupancyGrid_.py @@ -0,0 +1,33 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs.msg.dds_ + IDL file: OccupancyGrid_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import nav_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class OccupancyGrid_(idl.IdlStruct, typename="nav_msgs.msg.dds_.OccupancyGrid_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + info: 'unitree_sdk2py.idl.nav_msgs.msg.dds_.MapMetaData_' + data: types.sequence[types.uint8] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_Odometry_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_Odometry_.py new file mode 100644 index 0000000..054bb5c --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/_Odometry_.py @@ -0,0 +1,35 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs.msg.dds_ + IDL file: Odometry_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import nav_msgs + +# if TYPE_CHECKING: +# import geometry_msgs.msg.dds_ +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Odometry_(idl.IdlStruct, typename="nav_msgs.msg.dds_.Odometry_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + child_frame_id: str + pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.PoseWithCovariance_' + twist: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.TwistWithCovariance_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/__init__.py new file mode 100644 index 0000000..a14e17a --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/nav_msgs/msg/dds_/__init__.py @@ -0,0 +1,11 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: nav_msgs.msg.dds_ + +""" + +from ._MapMetaData_ import MapMetaData_ +from ._OccupancyGrid_ import OccupancyGrid_ +from ._Odometry_ import Odometry_ +__all__ = ["MapMetaData_", "OccupancyGrid_", "Odometry_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/__init__.py new file mode 100644 index 0000000..984e7e2 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/__init__.py new file mode 100644 index 0000000..62a2292 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/_PointField_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/_PointField_.py new file mode 100644 index 0000000..673aa84 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/_PointField_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg.dds_.PointField_Constants + IDL file: PointField_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import sensor_msgs + +INT8_ = 1 +UINT8_ = 2 +INT16_ = 3 +UINT16_ = 4 +INT32_ = 5 +UINT32_ = 6 +FLOAT32_ = 7 +FLOAT64_ = 8 + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/__init__.py new file mode 100644 index 0000000..310fecd --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/PointField_Constants/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg.dds_.PointField_Constants + +""" + +from ._PointField_ import FLOAT32_, FLOAT64_, INT16_, INT32_, INT8_, UINT16_, UINT32_, UINT8_ +__all__ = ["FLOAT32_", "FLOAT64_", "INT16_", "INT32_", "INT8_", "UINT16_", "UINT32_", "UINT8_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointCloud2_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointCloud2_.py new file mode 100644 index 0000000..ffdf713 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointCloud2_.py @@ -0,0 +1,39 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg.dds_ + IDL file: PointCloud2_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import sensor_msgs + +# if TYPE_CHECKING: +# import std_msgs.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PointCloud2_(idl.IdlStruct, typename="sensor_msgs.msg.dds_.PointCloud2_"): + header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_' + height: types.uint32 + width: types.uint32 + fields: types.sequence['unitree_sdk2py.idl.sensor_msgs.msg.dds_.PointField_'] + is_bigendian: bool + point_step: types.uint32 + row_step: types.uint32 + data: types.sequence[types.uint8] + is_dense: bool + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointField_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointField_.py new file mode 100644 index 0000000..e62dbf9 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/_PointField_.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg.dds_ + IDL file: PointField_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import sensor_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PointField_(idl.IdlStruct, typename="sensor_msgs.msg.dds_.PointField_"): + name: str + offset: types.uint32 + datatype: types.uint8 + count: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/__init__.py new file mode 100644 index 0000000..68e0141 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/sensor_msgs/msg/dds_/__init__.py @@ -0,0 +1,11 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: sensor_msgs.msg.dds_ + +""" + +from . import PointField_Constants +from ._PointCloud2_ import PointCloud2_ +from ._PointField_ import PointField_ +__all__ = ["PointField_Constants", "PointCloud2_", "PointField_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/__init__.py new file mode 100644 index 0000000..c17a705 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: std_msgs + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/__init__.py new file mode 100644 index 0000000..02c8fe3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: std_msgs.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_Header_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_Header_.py new file mode 100644 index 0000000..bd3564f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_Header_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: std_msgs.msg.dds_ + IDL file: Header_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import std_msgs + +# if TYPE_CHECKING: +# import builtin_interfaces.msg.dds_ + + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Header_(idl.IdlStruct, typename="std_msgs.msg.dds_.Header_"): + stamp: 'unitree_sdk2py.idl.builtin_interfaces.msg.dds_.Time_' + frame_id: str + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_String_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_String_.py new file mode 100644 index 0000000..052f122 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/_String_.py @@ -0,0 +1,27 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: std_msgs.msg.dds_ + IDL file: String_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import std_msgs + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class String_(idl.IdlStruct, typename="std_msgs.msg.dds_.String_"): + data: str + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/__init__.py new file mode 100644 index 0000000..f5282e5 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/std_msgs/msg/dds_/__init__.py @@ -0,0 +1,10 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: std_msgs.msg.dds_ + +""" + +from ._Header_ import Header_ +from ._String_ import String_ +__all__ = ["Header_", "String_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/__init__.py new file mode 100644 index 0000000..e6546ff --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/__init__.py new file mode 100644 index 0000000..bbf9fde --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestHeader_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestHeader_.py new file mode 100644 index 0000000..57d67a0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestHeader_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: RequestHeader_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class RequestHeader_(idl.IdlStruct, typename="unitree_api.msg.dds_.RequestHeader_"): + identity: 'unitree_sdk2py.idl.unitree_api.msg.dds_.RequestIdentity_' + lease: 'unitree_sdk2py.idl.unitree_api.msg.dds_.RequestLease_' + policy: 'unitree_sdk2py.idl.unitree_api.msg.dds_.RequestPolicy_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestIdentity_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestIdentity_.py new file mode 100644 index 0000000..a891b54 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestIdentity_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: RequestIdentity_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class RequestIdentity_(idl.IdlStruct, typename="unitree_api.msg.dds_.RequestIdentity_"): + id: types.int64 + api_id: types.int64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestLease_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestLease_.py new file mode 100644 index 0000000..32cef0f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestLease_.py @@ -0,0 +1,27 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: RequestLease_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class RequestLease_(idl.IdlStruct, typename="unitree_api.msg.dds_.RequestLease_"): + id: types.int64 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestPolicy_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestPolicy_.py new file mode 100644 index 0000000..aa7ef2f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_RequestPolicy_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: RequestPolicy_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class RequestPolicy_(idl.IdlStruct, typename="unitree_api.msg.dds_.RequestPolicy_"): + priority: types.int32 + noreply: bool + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Request_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Request_.py new file mode 100644 index 0000000..39c6b43 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Request_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: Request_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Request_(idl.IdlStruct, typename="unitree_api.msg.dds_.Request_"): + header: 'unitree_sdk2py.idl.unitree_api.msg.dds_.RequestHeader_' + parameter: str + binary: types.sequence[types.uint8] + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseHeader_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseHeader_.py new file mode 100644 index 0000000..ef47340 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseHeader_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: ResponseHeader_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class ResponseHeader_(idl.IdlStruct, typename="unitree_api.msg.dds_.ResponseHeader_"): + identity: 'unitree_sdk2py.idl.unitree_api.msg.dds_.RequestIdentity_' + status: 'unitree_sdk2py.idl.unitree_api.msg.dds_.ResponseStatus_' + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseStatus_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseStatus_.py new file mode 100644 index 0000000..92c0b1b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_ResponseStatus_.py @@ -0,0 +1,27 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: ResponseStatus_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class ResponseStatus_(idl.IdlStruct, typename="unitree_api.msg.dds_.ResponseStatus_"): + code: types.int32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Response_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Response_.py new file mode 100644 index 0000000..c743651 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/_Response_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + IDL file: Response_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_api + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Response_(idl.IdlStruct, typename="unitree_api.msg.dds_.Response_"): + header: 'unitree_sdk2py.idl.unitree_api.msg.dds_.ResponseHeader_' + data: str + binary: types.sequence[types.uint8] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/__init__.py new file mode 100644 index 0000000..1e4c9f8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_api/msg/dds_/__init__.py @@ -0,0 +1,16 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.10.2 + Module: unitree_api.msg.dds_ + +""" + +from ._RequestHeader_ import RequestHeader_ +from ._RequestIdentity_ import RequestIdentity_ +from ._RequestLease_ import RequestLease_ +from ._RequestPolicy_ import RequestPolicy_ +from ._Request_ import Request_ +from ._ResponseHeader_ import ResponseHeader_ +from ._ResponseStatus_ import ResponseStatus_ +from ._Response_ import Response_ +__all__ = ["RequestHeader_", "RequestIdentity_", "RequestLease_", "RequestPolicy_", "Request_", "ResponseHeader_", "ResponseStatus_", "Response_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/__init__.py new file mode 100644 index 0000000..b553006 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/__init__.py new file mode 100644 index 0000000..f8cae9b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_AudioData_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_AudioData_.py new file mode 100644 index 0000000..27913c9 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_AudioData_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: AudioData_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class AudioData_(idl.IdlStruct, typename="unitree_go.msg.dds_.AudioData_"): + time_frame: types.uint64 + data: types.sequence[types.uint8] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsCmd_.py new file mode 100644 index 0000000..5da647e --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsCmd_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: BmsCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class BmsCmd_(idl.IdlStruct, typename="unitree_go.msg.dds_.BmsCmd_"): + off: types.uint8 + reserve: types.array[types.uint8, 3] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsState_.py new file mode 100644 index 0000000..af635e8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_BmsState_.py @@ -0,0 +1,35 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: BmsState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class BmsState_(idl.IdlStruct, typename="unitree_go.msg.dds_.BmsState_"): + version_high: types.uint8 + version_low: types.uint8 + status: types.uint8 + soc: types.uint8 + current: types.int32 + cycle: types.uint16 + bq_ntc: types.array[types.uint8, 2] + mcu_ntc: types.array[types.uint8, 2] + cell_vol: types.array[types.uint16, 15] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Error_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Error_.py new file mode 100644 index 0000000..c338c69 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Error_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: Error_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Error_(idl.IdlStruct, typename="unitree_go.msg.dds_.Error_"): + source: types.uint32 + state: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Go2FrontVideoData_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Go2FrontVideoData_.py new file mode 100644 index 0000000..04d245b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Go2FrontVideoData_.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: Go2FrontVideoData_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Go2FrontVideoData_(idl.IdlStruct, typename="unitree_go.msg.dds_.Go2FrontVideoData_"): + time_frame: types.uint64 + video720p: types.sequence[types.uint8] + video360p: types.sequence[types.uint8] + video180p: types.sequence[types.uint8] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_HeightMap_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_HeightMap_.py new file mode 100644 index 0000000..48168da --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_HeightMap_.py @@ -0,0 +1,33 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: HeightMap_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class HeightMap_(idl.IdlStruct, typename="unitree_go.msg.dds_.HeightMap_"): + stamp: types.float64 + frame_id: str + resolution: types.float32 + width: types.uint32 + height: types.uint32 + origin: types.array[types.float32, 2] + data: types.sequence[types.float32] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_IMUState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_IMUState_.py new file mode 100644 index 0000000..e48908f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_IMUState_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: IMUState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class IMUState_(idl.IdlStruct, typename="unitree_go.msg.dds_.IMUState_"): + quaternion: types.array[types.float32, 4] + gyroscope: types.array[types.float32, 3] + accelerometer: types.array[types.float32, 3] + rpy: types.array[types.float32, 3] + temperature: types.uint8 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_InterfaceConfig_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_InterfaceConfig_.py new file mode 100644 index 0000000..5efbd64 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_InterfaceConfig_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: InterfaceConfig_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class InterfaceConfig_(idl.IdlStruct, typename="unitree_go.msg.dds_.InterfaceConfig_"): + mode: types.uint8 + value: types.uint8 + reserve: types.array[types.uint8, 2] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LidarState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LidarState_.py new file mode 100644 index 0000000..6861068 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LidarState_.py @@ -0,0 +1,43 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: LidarState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LidarState_(idl.IdlStruct, typename="unitree_go.msg.dds_.LidarState_"): + stamp: types.float64 + firmware_version: str + software_version: str + sdk_version: str + sys_rotation_speed: types.float32 + com_rotation_speed: types.float32 + error_state: types.uint8 + cloud_frequency: types.float32 + cloud_packet_loss_rate: types.float32 + cloud_size: types.uint32 + cloud_scan_num: types.uint32 + imu_frequency: types.float32 + imu_packet_loss_rate: types.float32 + imu_rpy: types.array[types.float32, 3] + serial_recv_stamp: types.float64 + serial_buffer_size: types.uint32 + serial_buffer_read: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowCmd_.py new file mode 100644 index 0000000..bcd4781 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowCmd_.py @@ -0,0 +1,40 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: LowCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LowCmd_(idl.IdlStruct, typename="unitree_go.msg.dds_.LowCmd_"): + head: types.array[types.uint8, 2] + level_flag: types.uint8 + frame_reserve: types.uint8 + sn: types.array[types.uint32, 2] + version: types.array[types.uint32, 2] + bandwidth: types.uint16 + motor_cmd: types.array['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorCmd_', 20] + bms_cmd: 'unitree_sdk2py.idl.unitree_go.msg.dds_.BmsCmd_' + wireless_remote: types.array[types.uint8, 40] + led: types.array[types.uint8, 12] + fan: types.array[types.uint8, 2] + gpio: types.uint8 + reserve: types.uint32 + crc: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowState_.py new file mode 100644 index 0000000..79ecbe6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_LowState_.py @@ -0,0 +1,48 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: LowState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LowState_(idl.IdlStruct, typename="unitree_go.msg.dds_.LowState_"): + head: types.array[types.uint8, 2] + level_flag: types.uint8 + frame_reserve: types.uint8 + sn: types.array[types.uint32, 2] + version: types.array[types.uint32, 2] + bandwidth: types.uint16 + imu_state: 'unitree_sdk2py.idl.unitree_go.msg.dds_.IMUState_' + motor_state: types.array['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorState_', 20] + bms_state: 'unitree_sdk2py.idl.unitree_go.msg.dds_.BmsState_' + foot_force: types.array[types.int16, 4] + foot_force_est: types.array[types.int16, 4] + tick: types.uint32 + wireless_remote: types.array[types.uint8, 40] + bit_flag: types.uint8 + adc_reel: types.float32 + temperature_ntc1: types.uint8 + temperature_ntc2: types.uint8 + power_v: types.float32 + power_a: types.float32 + fan_frequency: types.array[types.uint16, 4] + reserve: types.uint32 + crc: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmd_.py new file mode 100644 index 0000000..5a2dd85 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmd_.py @@ -0,0 +1,33 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: MotorCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorCmd_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorCmd_"): + mode: types.uint8 + q: types.float32 + dq: types.float32 + tau: types.float32 + kp: types.float32 + kd: types.float32 + reserve: types.array[types.uint32, 3] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py new file mode 100644 index 0000000..11d3029 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py @@ -0,0 +1,23 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: MotorCmds_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass, field + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorCmds_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorCmds_"): + cmds: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: []) + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorState_.py new file mode 100644 index 0000000..0787acb --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorState_.py @@ -0,0 +1,37 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: MotorState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorState_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorState_"): + mode: types.uint8 + q: types.float32 + dq: types.float32 + ddq: types.float32 + tau_est: types.float32 + q_raw: types.float32 + dq_raw: types.float32 + ddq_raw: types.float32 + temperature: types.uint8 + lost: types.uint32 + reserve: types.array[types.uint32, 2] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py new file mode 100644 index 0000000..4b0245b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py @@ -0,0 +1,23 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: MotorStates_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass, field + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorStates_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorStates_"): + states: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: []) + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_PathPoint_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_PathPoint_.py new file mode 100644 index 0000000..20a8c55 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_PathPoint_.py @@ -0,0 +1,33 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: PathPoint_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PathPoint_(idl.IdlStruct, typename="unitree_go.msg.dds_.PathPoint_"): + t_from_start: types.float32 + x: types.float32 + y: types.float32 + yaw: types.float32 + vx: types.float32 + vy: types.float32 + vyaw: types.float32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Req_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Req_.py new file mode 100644 index 0000000..5bb39b3 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Req_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: Req_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Req_(idl.IdlStruct, typename="unitree_go.msg.dds_.Req_"): + uuid: str + body: str + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Res_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Res_.py new file mode 100644 index 0000000..684a5a7 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_Res_.py @@ -0,0 +1,29 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: Res_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Res_(idl.IdlStruct, typename="unitree_go.msg.dds_.Res_"): + uuid: str + data: types.sequence[types.uint8] + body: str + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_SportModeState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_SportModeState_.py new file mode 100644 index 0000000..e0a26a1 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_SportModeState_.py @@ -0,0 +1,42 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: SportModeState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class SportModeState_(idl.IdlStruct, typename="unitree_go.msg.dds_.SportModeState_"): + stamp: 'unitree_sdk2py.idl.unitree_go.msg.dds_.TimeSpec_' + error_code: types.uint32 + imu_state: 'unitree_sdk2py.idl.unitree_go.msg.dds_.IMUState_' + mode: types.uint8 + progress: types.float32 + gait_type: types.uint8 + foot_raise_height: types.float32 + position: types.array[types.float32, 3] + body_height: types.float32 + velocity: types.array[types.float32, 3] + yaw_speed: types.float32 + range_obstacle: types.array[types.float32, 4] + foot_force: types.array[types.int16, 4] + foot_position_body: types.array[types.float32, 12] + foot_speed_body: types.array[types.float32, 12] + path_point: types.array['unitree_sdk2py.idl.unitree_go.msg.dds_.PathPoint_', 10] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_TimeSpec_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_TimeSpec_.py new file mode 100644 index 0000000..7a580b6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_TimeSpec_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: TimeSpec_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class TimeSpec_(idl.IdlStruct, typename="unitree_go.msg.dds_.TimeSpec_"): + sec: types.int32 + nanosec: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbState_.py new file mode 100644 index 0000000..d5f2588 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbState_.py @@ -0,0 +1,43 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: UwbState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class UwbState_(idl.IdlStruct, typename="unitree_go.msg.dds_.UwbState_"): + version: types.array[types.uint8, 2] + channel: types.uint8 + joy_mode: types.uint8 + orientation_est: types.float32 + pitch_est: types.float32 + distance_est: types.float32 + yaw_est: types.float32 + tag_roll: types.float32 + tag_pitch: types.float32 + tag_yaw: types.float32 + base_roll: types.float32 + base_pitch: types.float32 + base_yaw: types.float32 + joystick: types.array[types.float32, 2] + error_state: types.uint8 + buttons: types.uint8 + enabled_from_app: types.uint8 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbSwitch_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbSwitch_.py new file mode 100644 index 0000000..b902ef8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_UwbSwitch_.py @@ -0,0 +1,27 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: UwbSwitch_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class UwbSwitch_(idl.IdlStruct, typename="unitree_go.msg.dds_.UwbSwitch_"): + enabled: types.uint8 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_WirelessController_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_WirelessController_.py new file mode 100644 index 0000000..18919dd --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/_WirelessController_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + IDL file: WirelessController_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class WirelessController_(idl.IdlStruct, typename="unitree_go.msg.dds_.WirelessController_"): + lx: types.float32 + ly: types.float32 + rx: types.float32 + ry: types.float32 + keys: types.uint16 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py new file mode 100644 index 0000000..fc24f6f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg.dds_ + +""" + +from ._AudioData_ import AudioData_ +from ._BmsCmd_ import BmsCmd_ +from ._BmsState_ import BmsState_ +from ._Error_ import Error_ +from ._Go2FrontVideoData_ import Go2FrontVideoData_ +from ._HeightMap_ import HeightMap_ +from ._IMUState_ import IMUState_ +from ._InterfaceConfig_ import InterfaceConfig_ +from ._LidarState_ import LidarState_ +from ._LowCmd_ import LowCmd_ +from ._LowState_ import LowState_ +from ._MotorCmd_ import MotorCmd_ +from ._MotorCmds_ import MotorCmds_ +from ._MotorState_ import MotorState_ +from ._MotorStates_ import MotorStates_ +from ._Req_ import Req_ +from ._Res_ import Res_ +from ._SportModeState_ import SportModeState_ +from ._TimeSpec_ import TimeSpec_ +from ._PathPoint_ import PathPoint_ +from ._UwbState_ import UwbState_ +from ._UwbSwitch_ import UwbSwitch_ +from ._WirelessController_ import WirelessController_ +__all__ = ["AudioData_", "BmsCmd_", "BmsState_", "Error_", "Go2FrontVideoData_", "HeightMap_", "IMUState_", "InterfaceConfig_", "LidarState_", "LowCmd_", "LowState_", "MotorCmd_", "MotorCmds_", "MotorState_", "MotorStates_", "Req_", "Res_", "SportModeState_", "TimeSpec_", "PathPoint_", "UwbState_", "UwbSwitch_", "WirelessController_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest new file mode 100644 index 0000000..fd4ea94 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest @@ -0,0 +1,43 @@ +BmsCmd_ +msg + + +BmsState_ +msg + + +HandCmd_ +msg + + +HandState_ +msg + + +IMUState_ +msg + + +LowCmd_ +msg + + +LowState_ +msg + + +MainBoardState_ +msg + + +MotorCmd_ +msg + + +MotorState_ +msg + + +PressSensorState_ +msg + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/__init__.py new file mode 100644 index 0000000..5f55ee5 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest new file mode 100644 index 0000000..3218e96 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest @@ -0,0 +1,43 @@ +BmsCmd_ +dds_ + + +BmsState_ +dds_ + + +HandCmd_ +dds_ + + +HandState_ +dds_ + + +IMUState_ +dds_ + + +LowCmd_ +dds_ + + +LowState_ +dds_ + + +MainBoardState_ +dds_ + + +MotorCmd_ +dds_ + + +MotorState_ +dds_ + + +PressSensorState_ +dds_ + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/__init__.py new file mode 100644 index 0000000..02b132d --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest new file mode 100644 index 0000000..6174f37 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest @@ -0,0 +1,43 @@ +BmsCmd_ + +BmsCmd_ + +BmsState_ + +BmsState_ + +HandCmd_ + +HandCmd_ + +HandState_ + +HandState_ + +IMUState_ + +IMUState_ + +LowCmd_ + +LowCmd_ + +LowState_ + +LowState_ + +MainBoardState_ + +MainBoardState_ + +MotorCmd_ + +MotorCmd_ + +MotorState_ + +MotorState_ + +PressSensorState_ + +PressSensorState_ diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py new file mode 100644 index 0000000..525d1a4 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: BmsCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class BmsCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.BmsCmd_"): + cmd: types.uint8 + reserve: types.array[types.uint8, 40] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py new file mode 100644 index 0000000..dd0c6bd --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py @@ -0,0 +1,39 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: BmsState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class BmsState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.BmsState_"): + version_high: types.uint8 + version_low: types.uint8 + fn: types.uint8 + cell_vol: types.array[types.uint16, 40] + bmsvoltage: types.array[types.uint32, 3] + current: types.int32 + soc: types.uint8 + soh: types.uint8 + temperature: types.array[types.int16, 12] + cycle: types.uint16 + manufacturer_date: types.uint16 + bmsstate: types.array[types.uint32, 5] + reserve: types.array[types.uint32, 3] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py new file mode 100644 index 0000000..044afcb --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py @@ -0,0 +1,28 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: HandCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class HandCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandCmd_"): + motor_cmd: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorCmd_'] + reserve: types.array[types.uint32, 4] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py new file mode 100644 index 0000000..351dc75 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py @@ -0,0 +1,35 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: HandState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class HandState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandState_"): + motor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorState_'] + press_sensor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.PressSensorState_'] + imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_' + power_v: types.float32 + power_a: types.float32 + system_v: types.float32 + device_v: types.float32 + error: types.array[types.uint32, 2] + reserve: types.array[types.uint32, 2] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py new file mode 100644 index 0000000..75d01a6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: IMUState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class IMUState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.IMUState_"): + quaternion: types.array[types.float32, 4] + gyroscope: types.array[types.float32, 3] + accelerometer: types.array[types.float32, 3] + rpy: types.array[types.float32, 3] + temperature: types.int16 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py new file mode 100644 index 0000000..56b4b8b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: LowCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LowCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.LowCmd_"): + mode_pr: types.uint8 + mode_machine: types.uint8 + motor_cmd: types.array['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorCmd_', 35] + reserve: types.array[types.uint32, 4] + crc: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py new file mode 100644 index 0000000..003c815 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py @@ -0,0 +1,37 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: LowState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LowState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.LowState_"): + version: types.array[types.uint32, 2] + mode_pr: types.uint8 + mode_machine: types.uint8 + tick: types.uint32 + imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_' + # position: types.array[types.float32, 3] + # linear_velocity: types.array[types.float32, 3] + motor_state: types.array['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorState_', 35] + wireless_remote: types.array[types.uint8, 40] + reserve: types.array[types.uint32, 4] + crc: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py new file mode 100644 index 0000000..c280245 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: MainBoardState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MainBoardState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MainBoardState_"): + fan_state: types.array[types.uint16, 6] + temperature: types.array[types.int16, 6] + value: types.array[types.float32, 6] + state: types.array[types.uint32, 6] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py new file mode 100644 index 0000000..3dd8f9d --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py @@ -0,0 +1,33 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: MotorCmd_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MotorCmd_"): + mode: types.uint8 + q: types.float32 + dq: types.float32 + tau: types.float32 + kp: types.float32 + kd: types.float32 + reserve: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py new file mode 100644 index 0000000..839fccf --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py @@ -0,0 +1,36 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: MotorState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MotorState_"): + mode: types.uint8 + q: types.float32 + dq: types.float32 + ddq: types.float32 + tau_est: types.float32 + temperature: types.array[types.int16, 2] + vol: types.float32 + sensor: types.array[types.uint32, 2] + motorstate: types.uint32 + reserve: types.array[types.uint32, 4] + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_OdoState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_OdoState_.py new file mode 100644 index 0000000..f3ff673 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_OdoState_.py @@ -0,0 +1,32 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DX IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: OdoState_.idl + + +""" + +from enum import auto +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class OdoState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.OdoState_"): + version: types.array[types.uint32, 2] + tick: types.uint32 + position: types.array[types.float32, 3] + orientation: types.array[types.float32, 4] # quaternion [x, y, z, w] + linear_velocity: types.array[types.float32, 3] + angular_velocity: types.array[types.float32, 3] + crc: types.uint32 + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py new file mode 100644 index 0000000..7dcf794 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + IDL file: PressSensorState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +# import unitree_hg + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class PressSensorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.PressSensorState_"): + pressure: types.array[types.float32, 12] + temperature: types.array[types.float32, 12] + lost: types.uint32 + reserve: types.uint32 + + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py new file mode 100644 index 0000000..b4bca37 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py @@ -0,0 +1,20 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_hg.msg.dds_ + +""" + +from ._BmsCmd_ import BmsCmd_ +from ._BmsState_ import BmsState_ +from ._HandCmd_ import HandCmd_ +from ._HandState_ import HandState_ +from ._IMUState_ import IMUState_ +from ._OdoState_ import OdoState_ +from ._LowCmd_ import LowCmd_ +from ._LowState_ import LowState_ +from ._MainBoardState_ import MainBoardState_ +from ._MotorCmd_ import MotorCmd_ +from ._MotorState_ import MotorState_ +from ._PressSensorState_ import PressSensorState_ +__all__ = ["BmsCmd_", "BmsState_", "HandCmd_", "HandState_", "IMUState_", "OdoState_", "LowCmd_", "LowState_", "MainBoardState_", "MotorCmd_", "MotorState_", "PressSensorState_", ] diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client.py new file mode 100644 index 0000000..d774e53 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client.py @@ -0,0 +1,89 @@ +from .client_base import ClientBase +from .lease_client import LeaseClient +from .internal import * + +""" +" class Client +""" +class Client(ClientBase): + def __init__(self, serviceName: str, enabaleLease: bool = False): + super().__init__(serviceName) + + self.__apiMapping = {} + self.__apiVersion = None + self.__leaseClient = None + self.__enableLease = enabaleLease + + if (self.__enableLease): + self.__leaseClient = LeaseClient(serviceName) + self.__leaseClient.Init() + + def WaitLeaseApplied(self): + if self.__enableLease: + self.__leaseClient.WaitApplied() + + def GetLeaseId(self): + if self.__enableLease: + return self.__leaseClient.GetId() + else: + return None + + def GetApiVersion(self): + return self.__apiVersion + + def GetServerApiVersion(self): + code, apiVerson = self._CallBase(RPC_API_ID_INTERNAL_API_VERSION, "{}", 0, 0) + if code != 0: + print("[Client] get server api version error:", code) + return code, None + else: + return code, apiVerson + + def _SetApiVerson(self, apiVersion: str): + self.__apiVersion = apiVersion + + def _Call(self, apiId: int, parameter: str): + ret, proirity, leaseId = self.__CheckApi(apiId) + if ret == 0: + return self._CallBase(apiId, parameter, proirity, leaseId) + else: + return RPC_ERR_CLIENT_API_NOT_REG, None + + def _CallNoReply(self, apiId: int, parameter: str): + ret, proirity, leaseId = self.__CheckApi(apiId) + if ret == 0: + return self._CallNoReplyBase(apiId, parameter, proirity, leaseId) + else: + return RPC_ERR_CLIENT_API_NOT_REG + + def _CallBinary(self, apiId: int, parameter: list): + ret, proirity, leaseId = self.__CheckApi(apiId) + if ret == 0: + return self._CallBinaryBase(apiId, parameter, proirity, leaseId) + else: + return RPC_ERR_CLIENT_API_NOT_REG, None + + def _CallBinaryNoReply(self, apiId: int, parameter: list): + ret, proirity, leaseId = self.__CheckApi(apiId) + if ret == 0: + return self._CallBinaryNoReplyBase(apiId, parameter, proirity, leaseId) + else: + return RPC_ERR_CLIENT_API_NOT_REG + + def _RegistApi(self, apiId: int, proirity: int): + self.__apiMapping[apiId] = proirity + + def __CheckApi(self, apiId: int): + proirity = 0 + leaseId = 0 + + if apiId > RPC_INTERNAL_API_ID_MAX: + proirity = self.__apiMapping.get(apiId) + + if proirity is None: + return RPC_ERR_CLIENT_API_NOT_REG, proirity, leaseId + + if self.__enableLease: + leaseId = self.__leaseClient.GetId() + + return 0, proirity, leaseId \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_base.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_base.py new file mode 100644 index 0000000..1bac028 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_base.py @@ -0,0 +1,93 @@ +import time + +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import RequestHeader_ as RequestHeader +from ..idl.unitree_api.msg.dds_ import RequestLease_ as RequestLease +from ..idl.unitree_api.msg.dds_ import RequestIdentity_ as RequestIdentity +from ..idl.unitree_api.msg.dds_ import RequestPolicy_ as RequestPolicy + +from ..utils.future import FutureResult + +from .client_stub import ClientStub +from .internal import * + + +""" +" class ClientBase +""" +class ClientBase: + def __init__(self, serviceName: str): + self.__timeout = 1.0 + self.__stub = ClientStub(serviceName) + self.__stub.Init() + + def SetTimeout(self, timeout: float): + self.__timeout = timeout + + def _CallBase(self, apiId: int, parameter: str, proirity: int = 0, leaseId: int = 0): + # print("[CallBase] call apiId:", apiId, ", proirity:", proirity, ", leaseId:", leaseId) + header = self.__SetHeader(apiId, leaseId, proirity, False) + request = Request(header, parameter, []) + + future = self.__stub.SendRequest(request, self.__timeout) + if future is None: + return RPC_ERR_CLIENT_SEND, None + + result = future.GetResult(self.__timeout) + + if result.code != FutureResult.FUTURE_SUCC: + self.__stub.RemoveFuture(request.header.identity.id) + code = RPC_ERR_CLIENT_API_TIMEOUT if result.code == FutureResult.FUTUTE_ERR_TIMEOUT else RPC_ERR_UNKNOWN + return code, None + + response = result.value + + if response.header.identity.api_id != apiId: + return RPC_ERR_CLIENT_API_NOT_MATCH, None + else: + return response.header.status.code, response.data + + def _CallNoReplyBase(self, apiId: int, parameter: str, proirity: int, leaseId: int): + header = self.__SetHeader(apiId, leaseId, proirity, True) + request = Request(header, parameter, []) + + if self.__stub.Send(request, self.__timeout): + return 0 + else: + return RPC_ERR_CLIENT_SEND + + def _CallBinaryBase(self, apiId: int, parameter: list, proirity: int, leaseId: int): + header = self.__SetHeader(apiId, leaseId, proirity, False) + request = Request(header, "", parameter) + + future = self.__stub.SendRequest(request, self.__timeout) + if future is None: + return RPC_ERR_CLIENT_SEND, None + + result = future.GetResult(self.__timeout) + if result.code != FutureResult.FUTURE_SUCC: + self.__stub.RemoveFuture(request.header.identity.id) + code = RPC_ERR_CLIENT_API_TIMEOUT if result.code == FutureResult.FUTUTE_ERR_TIMEOUT else RPC_ERR_UNKNOWN + return code, None + + response = result.value + + if response.header.identity.api_id != apiId: + return RPC_ERR_CLIENT_API_NOT_MATCH, None + else: + return response.header.status.code, response.binary + + def _CallBinaryNoReplyBase(self, apiId: int, parameter: list, proirity: int, leaseId: int): + header = self.__SetHeader(apiId, leaseId, proirity, True) + request = Request(header, "", parameter) + + if self.__stub.Send(request, self.__timeout): + return 0 + else: + return RPC_ERR_CLIENT_SEND + + def __SetHeader(self, apiId: int, leaseId: int, priority: int, noReply: bool): + identity = RequestIdentity(time.monotonic_ns(), apiId) + lease = RequestLease(leaseId) + policy = RequestPolicy(priority, noReply) + return RequestHeader(identity, lease, policy) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_stub.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_stub.py new file mode 100644 index 0000000..67c2c8f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/client_stub.py @@ -0,0 +1,69 @@ +import time + +from enum import Enum +from threading import Thread, Condition + +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import Response_ as Response + +from ..core.channel import ChannelFactory +from ..core.channel_name import ChannelType, GetClientChannelName +from .request_future import RequestFuture, RequestFutureQueue + + +""" +" class ClientStub +""" +class ClientStub: + def __init__(self, serviceName: str): + self.__serviceName = serviceName + self.__futureQueue = None + + self.__sendChannel = None + self.__recvChannel = None + + def Init(self): + factory = ChannelFactory() + self.__futureQueue = RequestFutureQueue() + + # create channel + self.__sendChannel = factory.CreateSendChannel(GetClientChannelName(self.__serviceName, ChannelType.SEND), Request) + self.__recvChannel = factory.CreateRecvChannel(GetClientChannelName(self.__serviceName, ChannelType.RECV), Response, + self.__ResponseHandler,10) + time.sleep(0.5) + + + def Send(self, request: Request, timeout: float): + if self.__sendChannel.Write(request, timeout): + return True + else: + print("[ClientStub] send error. id:", request.header.identity.id) + return False + + def SendRequest(self, request: Request, timeout: float): + id = request.header.identity.id + + future = RequestFuture() + future.SetRequestId(id) + self.__futureQueue.Set(id, future) + + if self.__sendChannel.Write(request, timeout): + return future + else: + print("[ClientStub] send request error. id:", request.header.identity.id) + self.__futureQueue.Remove(id) + return None + + def RemoveFuture(self, requestId: int): + self.__futureQueue.Remove(requestId) + + def __ResponseHandler(self, response: Response): + id = response.header.identity.id + # apiId = response.header.identity.api_id + # print("[ClientStub] responseHandler recv response id:", id, ", apiId:", apiId) + future = self.__futureQueue.Get(id) + if future is None: + # print("[ClientStub] get future from queue error. id:", id) + pass + elif not future.Ready(response): + print("[ClientStub] set future ready error.") diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/internal.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/internal.py new file mode 100644 index 0000000..875a78b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/internal.py @@ -0,0 +1,31 @@ +# internal api id max +RPC_INTERNAL_API_ID_MAX = 100 + +# internal api id +RPC_API_ID_INTERNAL_API_VERSION = 1 + +# lease api id +RPC_API_ID_LEASE_APPLY = 101 +RPC_API_ID_LEASE_RENEWAL = 102 + +# lease term default +RPC_LEASE_TERM = 1.0 + +# internal error +RPC_OK = 0 +# client error +RPC_ERR_UNKNOWN = 3001 +RPC_ERR_CLIENT_SEND = 3102 +RPC_ERR_CLIENT_API_NOT_REG = 3103 +RPC_ERR_CLIENT_API_TIMEOUT = 3104 +RPC_ERR_CLIENT_API_NOT_MATCH = 3105 +RPC_ERR_CLIENT_API_DATA = 3106 +RPC_ERR_CLIENT_LEASE_INVALID = 3107 +# server error +RPC_ERR_SERVER_SEND = 3201 +RPC_ERR_SERVER_INTERNAL = 3202 +RPC_ERR_SERVER_API_NOT_IMPL = 3203 +RPC_ERR_SERVER_API_PARAMETER = 3204 +RPC_ERR_SERVER_LEASE_DENIED = 3205 +RPC_ERR_SERVER_LEASE_NOT_EXIST = 3206 +RPC_ERR_SERVER_LEASE_EXIST = 3207 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_client.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_client.py new file mode 100644 index 0000000..37f600f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_client.py @@ -0,0 +1,113 @@ +import time +import socket +import os +import json + +from threading import Thread, Lock + +from .client_base import ClientBase +from .internal import * + + +""" +" class LeaseContext +""" +class LeaseContext: + def __init__(self): + self.id = 0 + self.term = RPC_LEASE_TERM + + def Update(self, id, term): + self.id = id + self.term = term + + def Reset(self): + self.id = 0 + self.term = RPC_LEASE_TERM + + def Valid(self): + return self.id != 0 + + +""" +" class LeaseClient +""" +class LeaseClient(ClientBase): + def __init__(self, name: str): + self.__name = name + "_lease" + self.__contextName = socket.gethostname() + "/" + name + "/" + str(os.getpid()) + self.__context = LeaseContext() + self.__thread = None + self.__lock = Lock() + super().__init__(self.__name) + print("[LeaseClient] lease name:", self.__name, ", context name:", self.__contextName) + + def Init(self): + self.SetTimeout(1.0) + self.__thread = Thread(target=self.__ThreadFunc, name=self.__name, daemon=True) + self.__thread.start() + + def WaitApplied(self): + while True: + with self.__lock: + if self.__context.Valid(): + break + time.sleep(0.1) + + def GetId(self): + with self.__lock: + return self.__context.id + + def Applied(self): + with self.__lock: + return self.__context.Valid() + + def __Apply(self): + parameter = {} + parameter["name"] = self.__contextName + p = json.dumps(parameter) + + c, d = self._CallBase(RPC_API_ID_LEASE_APPLY, p) + if c != 0: + print("[LeaseClient] apply lease error. code:", c) + return + + data = json.loads(d) + + id = data["id"] + term = data["term"] + + print("[LeaseClient] lease applied id:", id, ", term:", term) + + with self.__lock: + self.__context.Update(id, float(term/1000000)) + + def __Renewal(self): + parameter = {} + p = json.dumps(parameter) + + c, d = self._CallBase(RPC_API_ID_LEASE_RENEWAL, p, 0, self.__context.id) + if c != 0: + print("[LeaseClient] renewal lease error. code:", c) + if c == RPC_ERR_SERVER_LEASE_NOT_EXIST: + with self.__lock: + self.__context.Reset() + + def __GetWaitSec(self): + waitsec = 0.0 + if self.__context.Valid(): + waitsec = self.__context.term + + if waitsec <= 0: + waitsec = RPC_LEASE_TERM + + return waitsec * 0.3 + + def __ThreadFunc(self): + while True: + if self.__context.Valid(): + self.__Renewal() + else: + self.__Apply() + # sleep waitsec + time.sleep(self.__GetWaitSec()) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_server.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_server.py new file mode 100644 index 0000000..d5b27d1 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/lease_server.py @@ -0,0 +1,151 @@ +import time +import json + +from threading import Lock + +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import ResponseHeader_ as ResponseHeader +from ..idl.unitree_api.msg.dds_ import ResponseStatus_ as ResponseStatus +from ..idl.unitree_api.msg.dds_ import Response_ as Response + +from .internal import * +from .server_base import ServerBase + + +""" +" class LeaseCache +""" +class LeaseCache: + def __init__(self): + self.lastModified = 0 + self.id = 0 + self.name = None + + def Set(self, id: int, name: str, lastModified: int) : + self.id = id + self.name = name + self.lastModified = lastModified + + def Renewal(self, lastModified: int): + self.lastModified = lastModified + + def Clear(self): + self.id = 0 + self.lastModified = 0 + self.name = None + + +""" +" class LeaseServer +""" +class LeaseServer(ServerBase): + def __init__(self, name: str, term: float): + self.__term = int(term * 1000000) + self.__lock = Lock() + self.__cache = LeaseCache() + super().__init__(name + "_lease") + + def Init(self): + pass + + def Start(self, enablePrioQueue: bool = False): + super()._SetServerRequestHandler(self.__ServerRequestHandler) + super()._Start(enablePrioQueue) + + def CheckRequestLeaseDenied(self, leaseId: int): + with self.__lock: + if self.__cache.id == 0: + return self.__cache.id != leaseId + + now = self.__Now() + if now > self.__cache.lastModified + self.__term: + self.__cache.Clear() + return False + else: + return self.__cache.id != leaseId + + def __Apply(self, parameter: str): + name = "" + data = "" + + try: + p = json.loads(parameter) + name = p.get("name") + + except: + print("[LeaseServer] apply json loads error. parameter:", parameter) + return RPC_ERR_SERVER_API_PARAMETER, data + + if not name: + name = "anonymous" + + id = 0 + lastModified = 0 + setted = False + + now = self.__Now() + + with self.__lock: + id = self.__cache.id + lastModified = self.__cache.lastModified + + if id == 0 or now > lastModified + self.__term: + if id != 0: + print("[LeaseServer] id expired:", id, ", name:", self.__cache.name) + + id = self.__GenerateId() + self.__cache.Set(id, name, now) + setted = True + + print("[LeaseServer] id stored:", id, ", name:", name) + + if setted: + d = {} + d["id"] = id + d["term"] = self.__term + data = json.dumps(d) + return 0, data + else: + return RPC_ERR_SERVER_LEASE_EXIST, data + + + def __Renewal(self, id: int): + now = self.__Now() + + with self.__lock: + if self.__cache.id != id: + return RPC_ERR_SERVER_LEASE_NOT_EXIST + + if now > self.__cache.lastModified + self.__term: + self.__cache.Clear() + return RPC_ERR_SERVER_LEASE_NOT_EXIST + else: + self.__cache.Renewal(now) + return 0 + + def __ServerRequestHandler(self, request: Request): + identity = request.header.identity + parameter = request.parameter + apiId = identity.api_id + code = RPC_ERR_SERVER_API_NOT_IMPL + data = "" + + if apiId == RPC_API_ID_LEASE_APPLY: + code, data = self.__Apply(parameter) + elif apiId == RPC_API_ID_LEASE_RENEWAL: + code = self.__Renewal(request.header.lease.id) + else: + print("[LeaseServer] api is not implemented. apiId", apiId) + + if request.header.policy.noreply: + return + + status = ResponseStatus(code) + response = Response(ResponseHeader(identity, status), data, []) + self._SendResponse(response) + + def __GenerateId(self): + return self.__Now() + + def __Now(self): + return int(time.time_ns()/1000) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/request_future.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/request_future.py new file mode 100644 index 0000000..037ab10 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/request_future.py @@ -0,0 +1,46 @@ +from threading import Condition, Lock +from enum import Enum + +from ..idl.unitree_api.msg.dds_ import Response_ as Response +from ..utils.future import Future, FutureResult + + +""" +" class RequestFuture +""" +class RequestFuture(Future): + def __init__(self): + self.__requestId = None + super().__init__() + + def SetRequestId(self, requestId: int): + self.__requestId = requestId + + def GetRequestId(self): + return self.__requestId + + +class RequestFutureQueue: + def __init__(self): + self.__data = {} + self.__lock = Lock() + + def Set(self, requestId: int, future: RequestFuture): + if future is None: + return False + with self.__lock: + self.__data[requestId] = future + return True + + def Get(self, requestId: int): + future = None + with self.__lock: + future = self.__data.get(requestId) + if future is not None: + self.__data.pop(requestId) + return future + + def Remove(self, requestId: int): + with self.__lock: + if id in self.__data: + self.__data.pop(requestId) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server.py new file mode 100644 index 0000000..4e389a8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server.py @@ -0,0 +1,122 @@ +import time + +from typing import Callable, Any + +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import ResponseStatus_ as ResponseStatus +from ..idl.unitree_api.msg.dds_ import ResponseHeader_ as ResponseHeader +from ..idl.unitree_api.msg.dds_ import Response_ as Response + +from .server_base import ServerBase +from .lease_server import LeaseServer +from .internal import * + +""" +" class Server +""" +class Server(ServerBase): + def __init__(self, name: str): + self.__apiVersion = "" + self.__apiHandlerMapping = {} + self.__apiBinaryHandlerMapping = {} + self.__apiBinarySet = {} + self.__enableLease = False + self.__leaseServer = None + super().__init__(name) + + def Init(self): + pass + + def StartLease(self, term: float = 1.0): + self.__enableLease = True + self.__leaseServer = LeaseServer(self.GetName(), term) + self.__leaseServer.Init() + self.__leaseServer.Start(False) + + def Start(self, enablePrioQueue: bool = False): + super()._SetServerRequestHandler(self.__ServerRequestHandler) + super()._Start(enablePrioQueue) + + def GetApiVersion(self): + return self.__apiVersion + + def _SetApiVersion(self, apiVersion: str): + self.__apiVersion = apiVersion + print("[Server] set api version:", self.__apiVersion) + + def _RegistHandler(self, apiId: int, handler: Callable, checkLease: bool): + self.__apiHandlerMapping[apiId] = (handler, checkLease) + + def _RegistBinaryHandler(self, apiId: int, handler: Callable, checkLease: bool): + self.__apiBinaryHandlerMapping[apiId] = (handler, checkLease) + self.__apiBinarySet.add(apiId) + + def __GetHandler(self, apiId: int): + if apiId in self.__apiHandlerMapping: + return self.__apiHandlerMapping.get(apiId) + else: + return None, False + + def __GetBinaryHandler(self, apiId: int): + if apiId in self.__apiBinaryHandlerMapping: + return self.__apiBinaryHandlerMapping.get(apiId) + else: + return None, False + + def __IsBinary(self, apiId): + return apiId in self.__apiBinarySet + + def __CheckLeaseDenied(self, leaseId: int): + if (self.__enableLease): + return self.__leaseServer.CheckRequestLeaseDenied(leaseId) + else: + return False + + def __ServerRequestHandler(self, request: Request): + parameter = request.parameter + parameterBinary = request.binary + + identity = request.header.identity + leaseId = request.header.lease.id + apiId = identity.api_id + + code = 0 + data = "" + dataBinary = [] + + if apiId == RPC_API_ID_INTERNAL_API_VERSION: + data = self.__apiVersion + else: + requestHandler = None + binaryRequestHandler = None + checkLease = False + + if self.__IsBinary(apiId): + binaryRequestHandler, checkLease = self.__GetBinaryHandler(apiId) + else: + requestHandler, checkLease = self.__GetHandler(apiId) + + if requestHandler is None and binaryRequestHandler is None: + code = RPC_ERR_SERVER_API_NOT_IMPL + elif checkLease and self.__CheckLeaseDenied(leaseId): + code = RPC_ERR_SERVER_LEASE_DENIED + else: + try: + if binaryRequestHandler is None: + code, data = requestHandler(parameter) + if code != 0: + data = "" + else: + code, dataBinary = binaryRequestHandler(parameterBinary) + if code != 0: + dataBinary = [] + except: + code = RPC_ERR_SERVER_INTERNAL + + if request.header.policy.noreply: + return + + status = ResponseStatus(code) + response = Response(ResponseHeader(identity, status), data, dataBinary) + + self._SendResponse(response) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_base.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_base.py new file mode 100644 index 0000000..f010561 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_base.py @@ -0,0 +1,32 @@ +import time + +from typing import Callable, Any + +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import Response_ as Response + +from .server_stub import ServerStub + + +""" +" class ServerBase +""" +class ServerBase: + def __init__(self, name: str): + self.__name = name + self.__serverRequestHandler = None + self.__serverStub = ServerStub(self.__name) + + def GetName(self): + return self.__name + + def _Start(self, enablePrioQueue: bool = False): + self.__serverStub.Init(self.__serverRequestHandler, enablePrioQueue) + print("[ServerBase] server started. name:", self.__name, ", enable proirity queue:", enablePrioQueue) + + def _SetServerRequestHandler(self, serverRequestHandler: Callable): + self.__serverRequestHandler = serverRequestHandler + + def _SendResponse(self, response: Response): + if not self.__serverStub.Send(response, 1.0): + print("[ServerBase] send response error.") diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_stub.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_stub.py new file mode 100644 index 0000000..e67be1f --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/rpc/server_stub.py @@ -0,0 +1,78 @@ +import time + +from enum import Enum +from threading import Thread, Condition +from typing import Callable, Any + +from ..utils.bqueue import BQueue +from ..idl.unitree_api.msg.dds_ import Request_ as Request +from ..idl.unitree_api.msg.dds_ import Response_ as Response + +from ..core.channel import ChannelFactory +from ..core.channel_name import ChannelType, GetServerChannelName + + +""" +" class ServerStub +""" +class ServerStub: + def __init__(self, serviceName: str): + self.__serviceName = serviceName + self.__serverRquestHandler = None + self.__sendChannel = None + self.__recvChannel = None + self.__enablePriority = None + self.__queue = None + self.__prioQueue = None + self.__queueThread = None + self.__prioQueueThread = None + + def Init(self, serverRequestHander: Callable, enablePriority: bool = False): + self.__serverRquestHandler = serverRequestHander + self.__enablePriority = enablePriority + + factory = ChannelFactory() + + # create channel + self.__sendChannel = factory.CreateSendChannel(GetServerChannelName(self.__serviceName, ChannelType.SEND), Response) + self.__recvChannel = factory.CreateRecvChannel(GetServerChannelName(self.__serviceName, ChannelType.RECV), Request, self.__Enqueue, 10) + + # start priority request thread + self.__queue = BQueue(10) + self.__queueThread = Thread(target=self.__QueueThreadFunc, name="server_queue", daemon=True) + self.__queueThread.start() + + if enablePriority: + self.__prioQueue = BQueue(5) + self.__prioQueueThread = Thread(target=self.__PrioQueueThreadFunc, name="server_prio_queue", daemon=True) + self.__prioQueueThread.start() + + # wait thread started + time.sleep(0.5) + + def Send(self, response: Response, timeout: float): + if self.__sendChannel.Write(response, timeout): + return True + else: + print("[ServerStub] send error. id:", response.header.identity.id) + return False + + def __Enqueue(self, request: Request): + if self.__enablePriority and request.header.policy.priority > 0: + self.__prioQueue.Put(request, True) + else: + self.__queue.Put(request, True) + + def __QueueThreadFunc(self): + while True: + request = self.__queue.Get() + if request is None: + continue + self.__serverRquestHandler(request) + + def __PrioQueueThreadFunc(self): + while True: + request = self.__prioQueue.Get() + if request is None: + continue + self.__serverRquestHandler(request) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/obstacles_avoid_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/obstacles_avoid_client_example.py new file mode 100644 index 0000000..7936fe7 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/obstacles_avoid_client_example.py @@ -0,0 +1,91 @@ +import time +import os + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ObstaclesAvoidClient + +if __name__ == "__main__": + ChannelFactoryInitialize(0, "enp3s0") + + client = ObstaclesAvoidClient() + client.SetTimeout(3.0) + client.Init() + + while True: + print("##################GetServerApiVersion###################") + code, serverAPiVersion = client.GetServerApiVersion() + if code != 0: + print("get server api error. code:", code) + else: + print("get server api version:", serverAPiVersion) + + if serverAPiVersion != client.GetApiVersion(): + print("api version not equal.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable) + + time.sleep(3) + + print("##################SwitchSet (on)###################") + code = client.SwitchSet(True) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable1 = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable1) + + time.sleep(3) + + print("##################SwitchSet (off)###################") + code = client.SwitchSet(False) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success.") + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable1 = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable1) + + time.sleep(3) + + + print("##################SwitchSet (enable)###################") + + code = client.SwitchSet(enable) + if code != 0: + print("switch set error. code:", code) + else: + print("switch set success. enable:", enable) + + time.sleep(3) + + print("##################SwitchGet###################") + code, enable = client.SwitchGet() + if code != 0: + print("switch get error. code:", code) + else: + print("switch get success. enable:", enable) + + time.sleep(3) + \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/robot_service_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/robot_service_client_example.py new file mode 100644 index 0000000..01467c0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/robot_service_client_example.py @@ -0,0 +1,50 @@ +import time +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.robot_state.robot_state_client import RobotStateClient + +if __name__ == "__main__": + ChannelFactoryInitialize(0, "enx000ec6768747") + rsc = RobotStateClient() + rsc.SetTimeout(3.0) + rsc.Init() + + while True: + print("##################GetServerApiVersion###################") + code, serverAPiVersion = rsc.GetServerApiVersion() + + if code != 0: + print("get server api error. code:", code) + else: + print("get server api version:", serverAPiVersion) + + time.sleep(3) + + print("##################ServiceList###################") + code, lst = rsc.ServiceList() + + if code != 0: + print("list sevrice error. code:", code) + else: + print("list service success. len:", len(lst)) + for s in lst: + print("name:", s.name, ", protect:", s.protect, ", status:", s.status) + + time.sleep(3) + + print("##################ServiceSwitch###################") + code = rsc.ServiceSwitch("sport_mode", False) + if code != 0: + print("service stop sport_mode error. code:", code) + else: + print("service stop sport_mode success. code:", code) + + time.sleep(1) + + code = rsc.ServiceSwitch("sport_mode", True) + if code != 0: + print("service start sport_mode error. code:", code) + else: + print("service start sport_mode success. code:", code) + + time.sleep(3) + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/sport_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/sport_client_example.py new file mode 100644 index 0000000..22a8de0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/sport_client_example.py @@ -0,0 +1,109 @@ +import time +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.sport.sport_client import SportClient, PathPoint, SPORT_PATH_POINT_SIZE + +if __name__ == "__main__": + ChannelFactoryInitialize(0, "enp2s0") + client = SportClient() + client.SetTimeout(10.0) + client.Init() + + print("##################GetServerApiVersion###################") + code, serverAPiVersion = client.GetServerApiVersion() + if code != 0: + print("get server api error. code:", code) + else: + print("get server api version:", serverAPiVersion) + + if serverAPiVersion != client.GetApiVersion(): + print("api version not equal.") + + time.sleep(3) + + print("##################Trigger###################") + code = client.Trigger() + if code != 0: + print("sport trigger error. code:", code) + else: + print("sport trigger success.") + + time.sleep(3) + + while True: + print("##################RecoveryStand###################") + code = client.RecoveryStand() + + if code != 0: + print("sport recovery stand error. code:", code) + else: + print("sport recovery stand success.") + + time.sleep(3) + + print("##################StandDown###################") + code = client.StandDown() + if code != 0: + print("sport stand down error. code:", code) + else: + print("sport stand down success.") + + time.sleep(3) + + print("##################Damp###################") + code = client.Damp() + if code != 0: + print("sport damp error. code:", code) + else: + print("sport damp down success.") + + time.sleep(3) + + print("##################RecoveryStand###################") + code = client.RecoveryStand() + + if code != 0: + print("sport recovery stand error. code:", code) + else: + print("sport recovery stand success.") + + time.sleep(3) + + print("##################Sit###################") + code = client.Sit() + if code != 0: + print("sport stand down error. code:", code) + else: + print("sport stand down success.") + + time.sleep(3) + + print("##################RiseSit###################") + code = client.RiseSit() + + if code != 0: + print("sport rise sit error. code:", code) + else: + print("sport rise sit success.") + + time.sleep(3) + + print("##################SetBodyHight###################") + code = client.BodyHeight(0.18) + + if code != 0: + print("sport body hight error. code:", code) + else: + print("sport body hight success.") + + time.sleep(3) + + print("##################GetState#################") + keys = ["state", "bodyHeight", "footRaiseHeight", "speedLevel", "gait"] + code, data = client.GetState(keys) + + if code != 0: + print("sport get state error. code:", code) + else: + print("sport get state success. data:", data) + + time.sleep(3) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/video_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/video_client_example.py new file mode 100644 index 0000000..7eec949 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/video_client_example.py @@ -0,0 +1,26 @@ +import time +import os + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.video.video_client import VideoClient + +if __name__ == "__main__": + ChannelFactoryInitialize(0, "enp2s0") + + client = VideoClient() + client.SetTimeout(3.0) + client.Init() + + print("##################GetImageSample###################") + code, data = client.GetImageSample() + + if code != 0: + print("get image sample error. code:", code) + else: + imageName = os.path.dirname(__file__) + time.strftime('/%Y%m%d%H%M%S.jpg',time.localtime()) + print("ImageName:", imageName) + + with open(imageName, "+wb") as f: + f.write(bytes(data)) + + time.sleep(1) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/vui_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/vui_client_example.py new file mode 100644 index 0000000..6df7c09 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/client/vui_client_example.py @@ -0,0 +1,74 @@ +import time +import os + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.go2.vui.vui_client import VuiClient + +if __name__ == "__main__": + ChannelFactoryInitialize(0, "enp2s0") + + client = VuiClient() + client.SetTimeout(3.0) + client.Init() + + for i in range(1, 11): + print("#################GetBrightness####################") + code, level = client.GetBrightness() + + if code != 0: + print("get brightness error. code:", code) + else: + print("get brightness success. level:", level) + + time.sleep(1) + + print("#################SetBrightness####################") + + code = client.SetBrightness(i) + + if code != 0: + print("set brightness error. code:", code) + else: + print("set brightness success. level:", i) + + time.sleep(1) + + print("#################SetBrightness 0####################") + + code = client.SetBrightness(0) + + if code != 0: + print("set brightness error. code:", code) + else: + print("set brightness 0 success.") + + for i in range(1, 11): + print("#################GetVolume####################") + code, level = client.GetVolume() + + if code != 0: + print("get volume error. code:", code) + else: + print("get volume success. level:", level) + + time.sleep(1) + + print("#################SetVolume####################") + + code = client.SetVolume(i) + + if code != 0: + print("set volume error. code:", code) + else: + print("set volume success. level:", i) + + time.sleep(1) + + print("#################SetVolume 0####################") + + code = client.SetVolume(0) + + if code != 0: + print("set volume error. code:", code) + else: + print("set volume 0 success.") diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/crc/test_crc.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/crc/test_crc.py new file mode 100644 index 0000000..ceb1b4b --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/crc/test_crc.py @@ -0,0 +1,27 @@ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.utils.crc import CRC + +crc = CRC() + +""" +" LowCmd/LowState CRC +""" +cmd = unitree_go_msg_dds__LowCmd_() +cmd.crc = crc.Crc(cmd) + +state = unitree_go_msg_dds__LowState_() +state.crc = crc.Crc(state) + +print("CRC[LowCmd, LowState]: {}, {}".format(cmd.crc, state.crc)) + +""" +" LowCmd/LowState for HG CRC. () +""" +cmd = unitree_hg_msg_dds__LowCmd_() +cmd.crc = crc.Crc(cmd) + +state = unitree_hg_msg_dds__LowState_() +state.crc = crc.Crc(state) + +print("CRC[HGLowCmd, HGLowState]: {}, {}".format(cmd.crc, state.crc)) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/helloworld.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/helloworld.py new file mode 100644 index 0000000..7e93803 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/helloworld.py @@ -0,0 +1,6 @@ +from dataclasses import dataclass +from cyclonedds.idl import IdlStruct + +@dataclass +class HelloWorld(IdlStruct, typename="HelloWorld"): + data: str \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/publisher.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/publisher.py new file mode 100644 index 0000000..36e8fb8 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/publisher.py @@ -0,0 +1,22 @@ +import time + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from helloworld import HelloWorld + +ChannelFactoryInitialize() + +pub = ChannelPublisher("topic", HelloWorld) +pub.Init() + +for i in range(30): + msg = HelloWorld("Hello world. time:" + str(time.time())) + # msg.data = "Hello world. time:" + str(time.time()) + + if pub.Write(msg, 0.5): + print("publish success. msg:", msg) + else: + print("publish error.") + + time.sleep(1) + +pub.Close() \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/subscriber.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/subscriber.py new file mode 100644 index 0000000..38e4763 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/helloworld/subscriber.py @@ -0,0 +1,19 @@ +import time + +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from helloworld import HelloWorld + +ChannelFactoryInitialize() + +sub = ChannelSubscriber("topic", HelloWorld) +sub.Init() + +while True: + msg = sub.Read() + + if msg is None: + print("subscribe error.") + else: + print("subscribe success. msg:", msg) + +pub.Close() \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/lowlevel_control.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/lowlevel_control.py new file mode 100644 index 0000000..8e94fe1 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/lowlevel_control.py @@ -0,0 +1,51 @@ +import time + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ +from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.utils.thread import Thread +import unitree_go2_const as go2 + +crc = CRC() +lowCmdThreadPtr=Thread() + +if __name__ == '__main__': + + ChannelFactoryInitialize(1, "enp2s0") + # Create a publisher to publish the data defined in UserData class + pub = ChannelPublisher("lowcmd", LowCmd_) + pub.Init() + + while True: + # Create a Userdata message + cmd = unitree_go_msg_dds__LowCmd_() + + # Toque controle, set RL_2 toque + cmd.motor_cmd[go2.LegID["RL_2"]].mode = 0x01 + cmd.motor_cmd[go2.LegID["RL_2"]].q = go2.PosStopF # Set to stop position(rad) + cmd.motor_cmd[go2.LegID["RL_2"]].kp = 0 + cmd.motor_cmd[go2.LegID["RL_2"]].dq = go2.VelStopF # Set to stop angular velocity(rad/s) + cmd.motor_cmd[go2.LegID["RL_2"]].kd = 0 + cmd.motor_cmd[go2.LegID["RL_2"]].tau = 1 # target toque is set to 1N.m + + # Poinstion(rad) control, set RL_0 rad + cmd.motor_cmd[go2.LegID["RL_0"]].mode = 0x01 + cmd.motor_cmd[go2.LegID["RL_0"]].q = 0 # Taregt angular(rad) + cmd.motor_cmd[go2.LegID["RL_0"]].kp = 10 # Poinstion(rad) control kp gain + cmd.motor_cmd[go2.LegID["RL_0"]].dq = 0 # Taregt angular velocity(rad/ss) + cmd.motor_cmd[go2.LegID["RL_0"]].kd = 1 # Poinstion(rad) control kd gain + cmd.motor_cmd[go2.LegID["RL_0"]].tau = 0 # Feedforward toque 1N.m + + cmd.crc = crc.Crc(cmd) + + #Publish message + if pub.Write(cmd): + print("Publish success. msg:", cmd.crc) + else: + print("Waitting for subscriber.") + + time.sleep(0.002) + + pub.Close() diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/read_lowstate.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/read_lowstate.py new file mode 100644 index 0000000..d3f20ca --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/read_lowstate.py @@ -0,0 +1,24 @@ +import time +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ + +import unitree_go2_const as go2 + + +def LowStateHandler(msg: LowState_): + + # print front right hip motor states + print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]]) + print("IMU state: ", msg.imu_state) + print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a) + + +if __name__ == "__main__": + # Modify "enp2s0" to the actual network interface + ChannelFactoryInitialize(0, "enp2s0") + sub = ChannelSubscriber("rt/lowstate", LowState_) + sub.Init(LowStateHandler, 10) + + while True: + time.sleep(10.0) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/sub_lowstate.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/sub_lowstate.py new file mode 100644 index 0000000..9a94619 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/sub_lowstate.py @@ -0,0 +1,15 @@ +import time +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ + +def LowStateHandler(msg: LowState_): + print(msg.motor_state) + + +ChannelFactoryInitialize(0, "enp2s0") +sub = ChannelSubscriber("rt/lowstate", LowState_) +sub.Init(LowStateHandler, 10) + +while True: + time.sleep(10.0) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/unitree_go2_const.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/unitree_go2_const.py new file mode 100644 index 0000000..153a051 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/lowlevel/unitree_go2_const.py @@ -0,0 +1,20 @@ +LegID = { + "FR_0": 0, # Front right hip + "FR_1": 1, # Front right thigh + "FR_2": 2, # Front right calf + "FL_0": 3, + "FL_1": 4, + "FL_2": 5, + "RR_0": 6, + "RR_1": 7, + "RR_2": 8, + "RL_0": 9, + "RL_1": 10, + "RL_2": 11, +} + +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_api.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_api.py new file mode 100644 index 0000000..68778ca --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_api.py @@ -0,0 +1,9 @@ +# service name +TEST_SERVICE_NAME = "test" + +# api version +TEST_API_VERSION = "1.0.0.1" + +# api id +TEST_API_ID_MOVE = 1008 +TEST_API_ID_STOP = 1002 \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_client_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_client_example.py new file mode 100644 index 0000000..35c6ad4 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_client_example.py @@ -0,0 +1,62 @@ +import time +import json + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.rpc.client import Client + +from test_api import * + +""" +" class TestClient +""" +class TestClient(Client): + def __init__(self, enableLease: bool = False): + super().__init__("test", enableLease) + + def Init(self): + self._RegistApi(TEST_API_ID_MOVE, 0) + self._RegistApi(TEST_API_ID_STOP, 1) + self._SetApiVerson(TEST_API_VERSION) + + def Move(self, vx: float, vy: float, vyaw: float): + parameter = {} + parameter["vx"] = vx + parameter["vy"] = vy + parameter["vyaw"] = vyaw + p = json.dumps(parameter) + + c, d = self._Call(TEST_API_ID_MOVE, p) + return c + + def Stop(self): + parameter = {} + p = json.dumps(parameter) + + c, d = self._Call(TEST_API_ID_STOP, p) + return c + +if __name__ == "__main__": + # initialize channel factory. + ChannelFactoryInitialize(0) + + # create client + client = TestClient(True) + client.Init() + client.SetTimeout(5.0) + + # get server version + code, serverApiVersion = client.GetServerApiVersion() + print("server api version:", serverApiVersion) + + # wait lease applied + client.WaitLeaseApplied() + + # test api + while True: + code = client.Move(0.2, 0, 0) + print("client move ret:", code) + time.sleep(1.0) + + code = client.Stop() + print("client stop ret:", code) + time.sleep(1.0) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_server_example.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_server_example.py new file mode 100644 index 0000000..a929e01 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/test/rpc/test_server_example.py @@ -0,0 +1,45 @@ +import time +import json + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.rpc.server import Server + +from test_api import * + + +""" +" class TestServer +""" +class TestServer(Server): + def __init__(self): + super().__init__("test") + + def Init(self): + self._RegistHandler(TEST_API_ID_MOVE, self.Move, 1) + self._RegistHandler(TEST_API_ID_STOP, self.Stop, 0) + self._SetApiVersion(TEST_API_VERSION) + + def Move(self, parameter: str): + p = json.loads(parameter) + x = p["vx"] + y = p["vy"] + yaw = p["vyaw"] + print("Move Called. vx:", x, ", vy:", y, ", vyaw:", yaw) + return 0, "" + + def Stop(self, parameter: str): + print("Stop Called.") + return 0, "" + +if __name__ == "__main__": + # initialize channel factory. + ChannelFactoryInitialize(0) + + # create server + server = TestServer() + server.Init() + server.StartLease(1.0) + server.Start(False) + + while True: + time.sleep(10) \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/__init__.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/bqueue.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/bqueue.py new file mode 100644 index 0000000..a612bfb --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/bqueue.py @@ -0,0 +1,58 @@ +from typing import Any +from collections import deque +from threading import Condition + +class BQueue: + def __init__(self, maxLen: int = 10): + self.__curLen = 0 + self.__maxLen = maxLen + self.__queue = deque() + self.__condition = Condition() + + def Put(self, x: Any, replace: bool = False): + noReplaced = True + with self.__condition: + if self.__curLen >= self.__maxLen: + if not replace: + return False + else: + noReplaced = False + self.__queue.popleft() + self.__curLen -= 1 + + self.__queue.append(x) + self.__curLen += 1 + self.__condition.notify() + + return noReplaced + + def Get(self, timeout: float = None): + with self.__condition: + if not self.__queue: + try: + self.__condition.wait(timeout) + except: + return None + + if not self.__queue: + return None + + self.__curLen -= 1 + return self.__queue.popleft() + + def Clear(self): + with self.__condition: + if self.__queue: + self.__queue.clear() + self.__curLen = 0 + + def Size(self): + with self.__condition: + return self.__curLen + + def Interrupt(self, notifyAll: bool = False): + with self.__condition: + if notifyAll: + self.__condition.notify() + else: + self.__condition.notify_all() diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/clib_lookup.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/clib_lookup.py new file mode 100644 index 0000000..ca7073d --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/clib_lookup.py @@ -0,0 +1,17 @@ +import os +import ctypes + +clib = ctypes.CDLL(None, use_errno=True) + +def CLIBCheckError(ret, func, args): + if ret < 0: + code = ctypes.get_errno() + raise OSError(code, os.strerror(code)) + return ret + +def CLIBLookup(name, resType, argTypes): + func = clib[name] + func.restye = resType + func.argtypes = argTypes + func.errcheck = CLIBCheckError + return func diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/crc.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/crc.py new file mode 100644 index 0000000..c250706 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/crc.py @@ -0,0 +1,228 @@ +import struct +import cyclonedds +import cyclonedds.idl as idl + +from .singleton import Singleton +from ..idl.unitree_go.msg.dds_ import LowCmd_ +from ..idl.unitree_go.msg.dds_ import LowState_ + +from ..idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_ +from ..idl.unitree_hg.msg.dds_ import LowState_ as HGLowState_ +import ctypes +import os +import platform + +class CRC(Singleton): + def __init__(self): + #4 bytes aligned, little-endian format. + #size 812 + self.__packFmtLowCmd = '<4B4IH2x' + 'B3x5f3I' * 20 + '4B' + '55Bx2I' + #size 1180 + self.__packFmtLowState = '<4B4IH2x' + '13fb3x' + 'B3x7fb3x3I' * 20 + '4BiH4b15H' + '8hI41B3xf2b2x2f4h2I' + #size 1004 + self.__packFmtHGLowCmd = '<2B2x' + 'B3x5fI' * 35 + '5I' + #size 2092 + self.__packFmtHGLowState = '<2I2B2xI' + '13fh2x' + 'B3x4f2hf7I' * 35 + '40B5I' + + + script_dir = os.path.dirname(os.path.abspath(__file__)) + self.platform = platform.system() + if self.platform == "Linux": + if platform.machine()=="x86_64": + self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_amd64.so') + elif platform.machine()=="aarch64": + self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_aarch64.so') + + self.crc_lib.crc32_core.argtypes = (ctypes.POINTER(ctypes.c_uint32), ctypes.c_uint32) + self.crc_lib.crc32_core.restype = ctypes.c_uint32 + + def Crc(self, msg: idl.IdlStruct): + if msg.__idl_typename__ == 'unitree_go.msg.dds_.LowCmd_': + return self.__Crc32(self.__PackLowCmd(msg)) + elif msg.__idl_typename__ == 'unitree_go.msg.dds_.LowState_': + return self.__Crc32(self.__PackLowState(msg)) + if msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowCmd_': + return self.__Crc32(self.__PackHGLowCmd(msg)) + elif msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowState_': + return self.__Crc32(self.__PackHGLowState(msg)) + else: + raise TypeError('unknown IDL message type to crc') + + def __PackLowCmd(self, cmd: LowCmd_): + origData = [] + origData.extend(cmd.head) + origData.append(cmd.level_flag) + origData.append(cmd.frame_reserve) + origData.extend(cmd.sn) + origData.extend(cmd.version) + origData.append(cmd.bandwidth) + + for i in range(20): + origData.append(cmd.motor_cmd[i].mode) + origData.append(cmd.motor_cmd[i].q) + origData.append(cmd.motor_cmd[i].dq) + origData.append(cmd.motor_cmd[i].tau) + origData.append(cmd.motor_cmd[i].kp) + origData.append(cmd.motor_cmd[i].kd) + origData.extend(cmd.motor_cmd[i].reserve) + + origData.append(cmd.bms_cmd.off) + origData.extend(cmd.bms_cmd.reserve) + + origData.extend(cmd.wireless_remote) + origData.extend(cmd.led) + origData.extend(cmd.fan) + origData.append(cmd.gpio) + origData.append(cmd.reserve) + origData.append(cmd.crc) + + return self.__Trans(struct.pack(self.__packFmtLowCmd, *origData)) + + def __PackLowState(self, state: LowState_): + origData = [] + origData.extend(state.head) + origData.append(state.level_flag) + origData.append(state.frame_reserve) + origData.extend(state.sn) + origData.extend(state.version) + origData.append(state.bandwidth) + + origData.extend(state.imu_state.quaternion) + origData.extend(state.imu_state.gyroscope) + origData.extend(state.imu_state.accelerometer) + origData.extend(state.imu_state.rpy) + origData.append(state.imu_state.temperature) + + for i in range(20): + origData.append(state.motor_state[i].mode) + origData.append(state.motor_state[i].q) + origData.append(state.motor_state[i].dq) + origData.append(state.motor_state[i].ddq) + origData.append(state.motor_state[i].tau_est) + origData.append(state.motor_state[i].q_raw) + origData.append(state.motor_state[i].dq_raw) + origData.append(state.motor_state[i].ddq_raw) + origData.append(state.motor_state[i].temperature) + origData.append(state.motor_state[i].lost) + origData.extend(state.motor_state[i].reserve) + + origData.append(state.bms_state.version_high) + origData.append(state.bms_state.version_low) + origData.append(state.bms_state.status) + origData.append(state.bms_state.soc) + origData.append(state.bms_state.current) + origData.append(state.bms_state.cycle) + origData.extend(state.bms_state.bq_ntc) + origData.extend(state.bms_state.mcu_ntc) + origData.extend(state.bms_state.cell_vol) + + origData.extend(state.foot_force) + origData.extend(state.foot_force_est) + origData.append(state.tick) + origData.extend(state.wireless_remote) + origData.append(state.bit_flag) + origData.append(state.adc_reel) + origData.append(state.temperature_ntc1) + origData.append(state.temperature_ntc2) + origData.append(state.power_v) + origData.append(state.power_a) + origData.extend(state.fan_frequency) + origData.append(state.reserve) + origData.append(state.crc) + + return self.__Trans(struct.pack(self.__packFmtLowState, *origData)) + + def __PackHGLowCmd(self, cmd: HGLowCmd_): + origData = [] + origData.append(cmd.mode_pr) + origData.append(cmd.mode_machine) + + for i in range(35): + origData.append(cmd.motor_cmd[i].mode) + origData.append(cmd.motor_cmd[i].q) + origData.append(cmd.motor_cmd[i].dq) + origData.append(cmd.motor_cmd[i].tau) + origData.append(cmd.motor_cmd[i].kp) + origData.append(cmd.motor_cmd[i].kd) + origData.append(cmd.motor_cmd[i].reserve) + + origData.extend(cmd.reserve) + origData.append(cmd.crc) + + return self.__Trans(struct.pack(self.__packFmtHGLowCmd, *origData)) + + def __PackHGLowState(self, state: HGLowState_): + origData = [] + origData.extend(state.version) + origData.append(state.mode_pr) + origData.append(state.mode_machine) + origData.append(state.tick) + + origData.extend(state.imu_state.quaternion) + origData.extend(state.imu_state.gyroscope) + origData.extend(state.imu_state.accelerometer) + origData.extend(state.imu_state.rpy) + origData.append(state.imu_state.temperature) + + for i in range(35): + origData.append(state.motor_state[i].mode) + origData.append(state.motor_state[i].q) + origData.append(state.motor_state[i].dq) + origData.append(state.motor_state[i].ddq) + origData.append(state.motor_state[i].tau_est) + origData.extend(state.motor_state[i].temperature) + origData.append(state.motor_state[i].vol) + origData.extend(state.motor_state[i].sensor) + origData.append(state.motor_state[i].motorstate) + origData.extend(state.motor_state[i].reserve) + + origData.extend(state.wireless_remote) + origData.extend(state.reserve) + origData.append(state.crc) + + return self.__Trans(struct.pack(self.__packFmtHGLowState, *origData)) + + def __Trans(self, packData): + calcData = [] + calcLen = ((len(packData)>>2)-1) + + for i in range(calcLen): + d = ((packData[i*4+3] << 24) | (packData[i*4+2] << 16) | (packData[i*4+1] << 8) | (packData[i*4])) + calcData.append(d) + + return calcData + + def _crc_py(self, data): + bit = 0 + crc = 0xFFFFFFFF + polynomial = 0x04c11db7 + + for i in range(len(data)): + bit = 1 << 31 + current = data[i] + + for b in range(32): + if crc & 0x80000000: + crc = (crc << 1) & 0xFFFFFFFF + crc ^= polynomial + else: + crc = (crc << 1) & 0xFFFFFFFF + + if current & bit: + crc ^= polynomial + + bit >>= 1 + + return crc + + def _crc_ctypes(self, data): + uint32_array = (ctypes.c_uint32 * len(data))(*data) + length = len(data) + crc=self.crc_lib.crc32_core(uint32_array, length) + return crc + + def __Crc32(self, data): + if self.platform == "Linux": + return self._crc_ctypes(data) + else: + return self._crc_py(data) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/future.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/future.py new file mode 100644 index 0000000..f539087 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/future.py @@ -0,0 +1,104 @@ +from threading import Condition +from typing import Any +from enum import Enum + +""" +" Enum RequtestFutureState +""" +class FutureState(Enum): + DEFER = 0 + READY = 1 + FAILED = 2 + +""" +" class FutureException +""" +class FutureResult: + FUTURE_SUCC = 0 + FUTUTE_ERR_TIMEOUT = 1 + FUTURE_ERR_FAILED = 2 + FUTURE_ERR_UNKNOWN = 3 + + def __init__(self, code: int, msg: str, value: Any = None): + self.code = code + self.msg = msg + self.value = value + + def __str__(self): + return f"FutureResult(code={str(self.code)}, msg='{self.msg}', value={self.value})" + +class Future: + def __init__(self): + self.__state = FutureState.DEFER + self.__msg = None + self.__condition = Condition() + + def GetResult(self, timeout: float = None): + with self.__condition: + return self.__WaitResult(timeout) + + def Wait(self, timeout: float = None): + with self.__condition: + return self.__Wait(timeout) + + def Ready(self, value): + with self.__condition: + ready = self.__Ready(value) + self.__condition.notify() + return ready + + def Fail(self, reason: str): + with self.__condition: + fail = self.__Fail(reason) + self.__condition.notify() + return fail + + def __Wait(self, timeout: float = None): + if not self.__IsDeferred(): + return True + try: + if timeout is None: + return self.__condition.wait() + else: + return self.__condition.wait(timeout) + except: + print("[Future] future wait error") + return False + + def __WaitResult(self, timeout: float = None): + if not self.__Wait(timeout): + return FutureResult(FutureResult.FUTUTE_ERR_TIMEOUT, "future wait timeout") + + if self.__IsReady(): + return FutureResult(FutureResult.FUTURE_SUCC, "success", self.__value) + elif self.__IsFailed(): + return FutureResult(FutureResult.FUTURE_ERR_FAILED, self.__msg) + else: + return FutureResult(FutureResult.FUTURE_ERR_UNKNOWN, "future state error:" + str(self.__state)) + + def __Ready(self, value): + if not self.__IsDeferred(): + print("[Future] futrue state is not defer") + return False + else: + self.__value = value + self.__state = FutureState.READY + return True + + def __Fail(self, message: str): + if not self.__IsDeferred(): + print("[Future] futrue state is not DEFER") + return False + else: + self.__msg = message + self.__state = FutureState.FAILED + return True + + def __IsDeferred(self): + return self.__state == FutureState.DEFER + + def __IsReady(self): + return self.__state == FutureState.READY + + def __IsFailed(self): + return self.__state == FutureState.FAILED \ No newline at end of file diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/hz_sample.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/hz_sample.py new file mode 100644 index 0000000..962fe3c --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/hz_sample.py @@ -0,0 +1,24 @@ +import time +from threading import Lock +from .thread import RecurrentThread + +class HZSample: + def __init__(self, interval: float = 1.0): + self.__count = 0 + self.__inter = interval if interval > 0.0 else 1.0 + self.__lock = Lock() + self.__thread = RecurrentThread(self.__inter, target=self.TimerFunc) + + def Start(self): + self.__thread.Start() + + def Sample(self): + with self.__lock: + self.__count += 1 + + def TimerFunc(self): + count = 0 + with self.__lock: + count = self.__count + self.__count = 0 + print("HZ: {}".format(count/self.__inter)) diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/joystick.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/joystick.py new file mode 100644 index 0000000..aeda672 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/joystick.py @@ -0,0 +1,251 @@ +import math +import struct +import os +os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "hide" # Disable pygame welcome message +import pygame +import time + +class Button: + def __init__(self) -> None: + self.pressed = False + self.on_pressed = False + self.on_released = False + self.data = 0 + self.click_count = 0 # 记录连续点击次数 + self.last_pressed_time = 0 # 上次按下时间 + + def __call__(self, data) -> None: + current_time = time.perf_counter() + # print('before',self.data) + + self.pressed = (data != 0) + self.on_pressed = self.pressed and self.data == 0 + self.on_released = not self.pressed and self.data != 0 + + # print('after',self.data) + # 处理连续点击 + if self.on_pressed: + # print('on_pressed') + # print('on_pressed current_time',current_time) + # print('on_pressed last_pressed_time',self.last_pressed_time) + # print('on_pressed diff',current_time-self.last_pressed_time) + + if current_time - self.last_pressed_time <= 0.3: # 0.1 秒以内的连续点击 + self.click_count += 1 + # print(self.click_count) + else: + self.click_count = 0 # 超过时间间隔，重置计数器 + self.last_pressed_time = current_time + self.data = data + + def reset_click_count(self): + """手动重置连续点击计数器""" + self.click_count = 0 + +class Axis: + def __init__(self) -> None: + self.data = 0.0 + self.pressed = False + self.on_pressed = False + self.on_released = False + + self.smooth = 0.03 + self.deadzone = 0.01 + self.threshold = 0.5 + + def __call__(self, data) -> None: + data_deadzone = 0.0 if math.fabs(data) < self.deadzone else data + new_data = self.data * (1 - self.smooth) + data_deadzone * self.smooth + self.pressed = math.fabs(new_data) > self.threshold + self.on_pressed = self.pressed and math.fabs(self.data) < self.threshold + self.on_released = not self.pressed and math.fabs(self.data) > self.threshold + self.data = new_data + + +class Joystick: + def __init__(self) -> None: + # Buttons + self.back = Button() + self.start = Button() + # self.LS = Button() + # self.RS = Button() + self.LB = Button() + self.RB = Button() + self.LT = Button() + self.RT = Button() + self.A = Button() + self.B = Button() + self.X = Button() + self.Y = Button() + self.up = Button() + self.down = Button() + self.left = Button() + self.right = Button() + self.F1 = Button() + self.F2 = Button() + + # Axes + # self.LT = Axis() + # self.RT = Axis() + self.lx = Axis() + self.ly = Axis() + self.rx = Axis() + self.ry = Axis() + + self.last_active_time = time.perf_counter() # 最后一次活动时间 + self.inactive_timeout = 0.5 # 超时时间（单位：秒） + def update(self): + """ + Update the current handle key based on the original data + Used to update flag bits such as on_pressed + + Examples: + >>> new_A_data = 1 + >>> self.A( new_A_data ) + """ + pass + + def extract(self, wireless_remote): + """ + Extract data from unitree_joystick + wireless_remote: uint8_t[40] + """ + # Buttons + button1 = [int(data) for data in f'{wireless_remote[2]:08b}'] + button2 = [int(data) for data in f'{wireless_remote[3]:08b}'] + self.LT(button1[2]) + self.RT(button1[3]) + self.back(button1[4]) + self.start(button1[5]) + self.LB(button1[6]) + self.RB(button1[7]) + self.left(button2[0]) + self.down(button2[1]) + self.right(button2[2]) + self.up(button2[3]) + self.Y(button2[4]) + self.X(button2[5]) + self.B(button2[6]) + self.A(button2[7]) + # Axes + self.lx( struct.unpack('f', bytes(wireless_remote[4:8]))[0] ) + self.rx( struct.unpack('f', bytes(wireless_remote[8:12]))[0] ) + self.ry( struct.unpack('f', bytes(wireless_remote[12:16]))[0] ) + self.ly( struct.unpack('f', bytes(wireless_remote[20:24]))[0] ) + + + # 检查是否有按键按下 + if any([ + self.LT.pressed, self.RT.pressed, self.back.pressed, self.start.pressed, + self.LB.pressed, self.RB.pressed, self.left.pressed, self.down.pressed, + self.right.pressed, self.up.pressed, self.Y.pressed, self.X.pressed, + self.B.pressed, self.A.pressed + ]): + self.last_active_time = time.perf_counter() # 更新最后一次活动时间 + elif time.perf_counter() - self.last_active_time > self.inactive_timeout: + # 超过设定的超时时间未按下任何键，重置所有按键的点击计数 + self.reset_all_click_counts() + self.last_active_time = time.perf_counter() # 重置最后活动时间 + + def reset_all_click_counts(self): + """重置所有按键的连续点击计数器""" + for button in [ + self.LT, self.RT, self.back, self.start, self.LB, self.RB, + self.left, self.down, self.right, self.up, self.Y, self.X, self.B, self.A + ]: + button.reset_click_count() + + def combine(self): + """ + Merge data from Joystick to wireless_remote + """ + # prepare an empty list + wireless_remote = [0 for _ in range(40)] + + # Buttons + wireless_remote[2] = int(''.join([f'{key}' for key in [ + 0, 0, round(self.LT.data), round(self.RT.data), + self.back.data, self.start.data, self.LB.data, self.RB.data, + ]]), 2) + wireless_remote[3] = int(''.join([f'{key}' for key in [ + self.left.data, self.down.data, self.right.data, + self.up.data, self.Y.data, self.X.data, self.B.data, self.A.data, + ]]), 2) + + # Axes + sticks = [self.lx.data, self.rx.data, self.ry.data, self.ly.data] + packs = list(map(lambda x: struct.pack('f', x), sticks)) + wireless_remote[4:8] = packs[0] + wireless_remote[8:12] = packs[1] + wireless_remote[12:16] = packs[2] + wireless_remote[20:24] = packs[3] + return wireless_remote + +class PyGameJoystick(Joystick): + def __init__(self) -> None: + super().__init__() + + pygame.init() + pygame.joystick.init() + if pygame.joystick.get_count() <= 0: + raise Exception("No joystick found!") + + self._joystick = pygame.joystick.Joystick(0) + self._joystick.init() + + def print(self): + print("\naxes: ") + for i in range(self._joystick.get_numaxes()): + print(self._joystick.get_axis(i), end=" ") + print("\nbuttons: ") + for i in range(self._joystick.get_numbuttons()): + print(self._joystick.get_button(i), end=" ") + print("\nhats: ") + for i in range(self._joystick.get_numhats()): + print(self._joystick.get_hat(i), end=" ") + print("\nballs: ") + for i in range(self._joystick.get_numballs()): + print(self._joystick.get_ball(i), end=" ") + print("\n") + +class LogicJoystick(PyGameJoystick): + """ Logic F710 """ + def __init__(self) -> None: + super().__init__() + + def update(self): + pygame.event.pump() + + self.back(self._joystick.get_button(6)) + self.start(self._joystick.get_button(7)) + self.LS(self._joystick.get_button(9)) + self.RS(self._joystick.get_button(10)) + self.LB(self._joystick.get_button(4)) + self.RB(self._joystick.get_button(5)) + self.A(self._joystick.get_button(0)) + self.B(self._joystick.get_button(1)) + self.X(self._joystick.get_button(2)) + self.Y(self._joystick.get_button(3)) + + self.LT((self._joystick.get_axis(2) + 1)/2) + self.RT((self._joystick.get_axis(5) + 1)/2) + self.rx(self._joystick.get_axis(3)) + self.ry(-self._joystick.get_axis(4)) + + + # Logitech controller has 2 modes + # mode 1: light down + self.up(1 if self._joystick.get_hat(0)[1] > 0.5 else 0) + self.down(1 if self._joystick.get_hat(0)[1] < -0.5 else 0) + self.left(1 if self._joystick.get_hat(0)[0] < -0.5 else 0) + self.right(1 if self._joystick.get_hat(0)[0] > 0.5 else 0) + self.lx(self._joystick.get_axis(0)) + self.ly(-self._joystick.get_axis(1)) + # mode 2: light up + # self.up(1 if self._joystick.get_axis(1) < -0.5 else 0) + # self.down(1 if self._joystick.get_axis(0) > 0.5 else 0) + # self.left(1 if self._joystick.get_axis(0) < -0.5 else 0) + # self.right(1 if self._joystick.get_axis(0) > 0.5 else 0) + # self.lx(self._joystick.get_hat(0)[1]) + # self.ly(self._joystick.get_hat(0)[1]) + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_aarch64.so b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_aarch64.so new file mode 100644 index 0000000..4dfb614 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_aarch64.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a4db103653db78540d141ff4dc55216b83e5298280e639b2ad54a84ac2fae9a5 +size 7928 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_amd64.so b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_amd64.so new file mode 100644 index 0000000..8db8bf6 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/lib/crc_amd64.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b136a91a7e99c5cf914f465e131f63c897a17107939184a6f85d1a57cf315ade +size 15136 diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/singleton.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/singleton.py new file mode 100644 index 0000000..62ca3b0 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/singleton.py @@ -0,0 +1,11 @@ +class Singleton: + __instance = None + + def __new__(cls, *args, **kwargs): + if cls.__instance is None: + cls.__instance = super(Singleton, cls).__new__(cls) + return cls.__instance + + def __init__(self): + pass + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/thread.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/thread.py new file mode 100644 index 0000000..f17cf4e --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/thread.py @@ -0,0 +1,83 @@ +import sys +import os +import errno +import ctypes +import struct +import threading + +from .future import Future +from .timerfd import * + +class Thread(Future): + def __init__(self, target = None, name = None, args = (), kwargs = None): + super().__init__() + self.__target = target + self.__args = args + self.__kwargs = {} if kwargs is None else kwargs + self.__thread = threading.Thread(target=self.__ThreadFunc, name=name, daemon=True) + + def Start(self): + return self.__thread.start() + + def GetId(self): + return self.__thread.ident + + def GetNativeId(self): + return self.__thread.native_id + + def __ThreadFunc(self): + value = None + try: + value = self.__target(*self.__args, **self.__kwargs) + self.Ready(value) + except: + info = sys.exc_info() + self.Fail(f"[Thread] target func raise exception: name={info[0].__name__}, args={str(info[1].args)}") + +class RecurrentThread(Thread): + def __init__(self, interval: float = 1.0, target = None, name = None, args = (), kwargs = None): + self.__quit = False + self.__inter = interval + self.__loopTarget = target + self.__loopArgs = args + self.__loopKwargs = {} if kwargs is None else kwargs + + if interval is None or interval <= 0.0: + super().__init__(target=self.__LoopFunc_0, name=name) + else: + super().__init__(target=self.__LoopFunc, name=name) + + def Wait(self, timeout: float = None): + self.__quit = True + super().Wait(timeout) + + def __LoopFunc(self): + # clock type CLOCK_MONOTONIC = 1 + tfd = timerfd_create(1, 0) + spec = itimerspec.from_seconds(self.__inter, self.__inter) + timerfd_settime(tfd, 0, ctypes.byref(spec), None) + + while not self.__quit: + try: + self.__loopTarget(*self.__loopArgs, **self.__loopKwargs) + except: + info = sys.exc_info() + print(f"[RecurrentThread] target func raise exception: name={info[0].__name__}, args={str(info[1].args)}") + + try: + buf = os.read(tfd, 8) + # print(struct.unpack("Q", buf)[0]) + except OSError as e: + if e.errno != errno.EAGAIN: + raise e + + os.close(tfd) + + def __LoopFunc_0(self): + while not self.__quit: + try: + self.__loopTarget(*self.__args, **self.__kwargs) + except: + info = sys.exc_info() + print(f"[RecurrentThread] target func raise exception: name={info[0].__name__}, args={str(info[1].args)}") + diff --git a/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/timerfd.py b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/timerfd.py new file mode 100644 index 0000000..002ef39 --- /dev/null +++ b/external_dependencies/unitree_sdk2_python/unitree_sdk2py/utils/timerfd.py @@ -0,0 +1,45 @@ +import math +import ctypes +from .clib_lookup import CLIBLookup + +class timespec(ctypes.Structure): + _fields_ = [("sec", ctypes.c_long), ("nsec", ctypes.c_long)] + __slots__ = [name for name,type in _fields_] + + @classmethod + def from_seconds(cls, secs): + c = cls() + c.seconds = secs + return c + + @property + def seconds(self): + return self.sec + self.nsec / 1000000000 + + @seconds.setter + def seconds(self, secs): + x, y = math.modf(secs) + self.sec = int(y) + self.nsec = int(x * 1000000000) + + +class itimerspec(ctypes.Structure): + _fields_ = [("interval", timespec),("value", timespec)] + __slots__ = [name for name,type in _fields_] + + @classmethod + def from_seconds(cls, interval, value): + spec = cls() + spec.interval.seconds = interval + spec.value.seconds = value + return spec + + +# function timerfd_create +timerfd_create = CLIBLookup("timerfd_create", ctypes.c_int, (ctypes.c_long, ctypes.c_int)) + +# function timerfd_settime +timerfd_settime = CLIBLookup("timerfd_settime", ctypes.c_int, (ctypes.c_int, ctypes.c_int, ctypes.POINTER(itimerspec), ctypes.POINTER(itimerspec))) + +# function timerfd_gettime +timerfd_gettime = CLIBLookup("timerfd_gettime", ctypes.c_int, (ctypes.c_int, ctypes.POINTER(itimerspec))) diff --git a/gr00t_wbc/__init__.py b/gr00t_wbc/__init__.py new file mode 100644 index 0000000..3308a84 --- /dev/null +++ b/gr00t_wbc/__init__.py @@ -0,0 +1,3 @@ +from .version import VERSION, VERSION_SHORT # noqa + +__version__ = VERSION # noqa diff --git a/gr00t_wbc/control/__init__.py b/gr00t_wbc/control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/base/__init__.py b/gr00t_wbc/control/base/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/base/env.py b/gr00t_wbc/control/base/env.py new file mode 100644 index 0000000..dcca21e --- /dev/null +++ b/gr00t_wbc/control/base/env.py @@ -0,0 +1,45 @@ +import gymnasium as gym + + +class Env: + """Base interface for all environments in the Gr00t framework""" + + def observe(self) -> dict[str, any]: + """Read the current state of this environment + + Returns: + dict: A dictionary of observations + """ + pass + + def queue_action(self, action: dict[str, any]): + """Queue an action to be executed + + Args: + action: A dictionary of action parameters + """ + pass + + def reset(self, **kwargs): + """Reset this environment to initial state""" + pass + + def observation_space(self) -> gym.Space: + """Get the observation space of this environment + + Returns: + gym.Space: The observation space + """ + pass + + def action_space(self) -> gym.Space: + """Get the action space of this environment + + Returns: + gym.Space: The action space + """ + pass + + def close(self): + """Close and clean up this environment""" + pass diff --git a/gr00t_wbc/control/base/humanoid_env.py b/gr00t_wbc/control/base/humanoid_env.py new file mode 100644 index 0000000..92e7dfb --- /dev/null +++ b/gr00t_wbc/control/base/humanoid_env.py @@ -0,0 +1,60 @@ +from abc import abstractmethod + +from gr00t_wbc.control.base.env import Env +from gr00t_wbc.control.base.sensor import Sensor +from gr00t_wbc.control.robot_model.robot_model import RobotModel + + +class Hands: + """Container class for left and right hand environments. + + Attributes: + left: Environment for the left hand + right: Environment for the right hand + """ + + left: Env + right: Env + + +class HumanoidEnv(Env): + """Base class for humanoid robot environments. + + This class provides the interface for accessing the robot's body, hands, and sensors. + """ + + def body(self) -> Env: + """Get the robot's body environment. + + Returns: + Env: The body environment + """ + pass + + def hands(self) -> Hands: + """Get the robot's hands. + + Returns: + Hands: Container with left and right hand environments + """ + pass + + def sensors(self) -> dict[str, Sensor]: + """Get the sensors of this environment + + Returns: + dict: A dictionary of sensors + """ + pass + + @abstractmethod + def robot_model(self) -> RobotModel: + """Get the robot model of this environment + This robot model is used to dispatch whole body actions to body + and hand actuators and to reconstruct proprioceptive + observations from body and hands. + + Returns: + RobotModel: The robot model + """ + pass diff --git a/gr00t_wbc/control/base/policy.py b/gr00t_wbc/control/base/policy.py new file mode 100755 index 0000000..a6ebb7e --- /dev/null +++ b/gr00t_wbc/control/base/policy.py @@ -0,0 +1,47 @@ +from abc import ABC, abstractmethod +from typing import Optional + + +class Policy(ABC): + """Base class for implementing control policies in the Gr00t framework. + + A Policy defines how an agent should behave in an environment by mapping observations + to actions. This abstract base class provides the interface that all concrete policy + implementations must follow. + """ + + def set_goal(self, goal: dict[str, any]): + """Set the command from the planner that the policy should follow. + + Args: + goal: Dictionary containing high-level commands or goals from the planner + """ + pass + + def set_observation(self, observation: dict[str, any]): + """Update the policy's current observation of the environment. + + Args: + observation: Dictionary containing the current state/observation of the environment + """ + self.observation = observation + + @abstractmethod + def get_action(self, time: Optional[float] = None) -> dict[str, any]: + """Compute and return the next action at the specified time, based on current observation + and planner command. + + Args: + time: Optional "monotonic time" for time-dependent policies + + Returns: + Dictionary containing the action to be executed + """ + + def close(self): + """Clean up any resources used by the policy.""" + pass + + def reset(self): + """Reset the policy to its initial state.""" + pass diff --git a/gr00t_wbc/control/base/sensor.py b/gr00t_wbc/control/base/sensor.py new file mode 100644 index 0000000..c9e5ba8 --- /dev/null +++ b/gr00t_wbc/control/base/sensor.py @@ -0,0 +1,35 @@ +import gymnasium as gym + + +class Sensor: + """Base class for implementing sensors in the Gr00t framework. + + A Sensor provides information about a specific sensor on the robot (e.g. camera, IMU, + force sensor). This abstract base class defines the interface that all concrete sensor + implementations must follow. + """ + + def read(self, **kwargs) -> any: + """Read the current sensor value. + + Args: + **kwargs: Additional parameters specific to the sensor implementation + (e.g. camera resolution, sampling rate) + + Returns: + The sensor reading value (e.g. image data, acceleration measurements) + """ + pass + + def observation_space(self) -> gym.Space: + """Get the observation space of this sensor. + + Returns: + gym.Space: The observation space defining the shape and bounds of sensor readings + (e.g. image dimensions for camera, measurement ranges for IMU) + """ + pass + + def close(self): + """Clean up any resources used by the sensor.""" + pass diff --git a/gr00t_wbc/control/envs/__init__.py b/gr00t_wbc/control/envs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/g1/__init__.py b/gr00t_wbc/control/envs/g1/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/g1/g1_body.py b/gr00t_wbc/control/envs/g1/g1_body.py new file mode 100644 index 0000000..8543f80 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/g1_body.py @@ -0,0 +1,67 @@ +from typing import Any, Dict + +import gymnasium as gym +import numpy as np + +from gr00t_wbc.control.base.env import Env +from gr00t_wbc.control.envs.g1.utils.command_sender import BodyCommandSender +from gr00t_wbc.control.envs.g1.utils.state_processor import BodyStateProcessor + + +class G1Body(Env): + def __init__(self, config: Dict[str, Any]): + super().__init__() + self.body_state_processor = BodyStateProcessor(config=config) + self.body_command_sender = BodyCommandSender(config=config) + + def observe(self) -> dict[str, any]: + body_state = self.body_state_processor._prepare_low_state() # (1, 148) + assert body_state.shape == (1, 148) + body_q = body_state[ + 0, 7 : 7 + 12 + 3 + 7 + 7 + ] # leg (12) + waist (3) + left arm (7) + right arm (7) + body_dq = body_state[0, 42 : 42 + 12 + 3 + 7 + 7] + body_ddq = body_state[0, 112 : 112 + 12 + 3 + 7 + 7] + body_tau_est = body_state[0, 77 : 77 + 12 + 3 + 7 + 7] + floating_base_pose = body_state[0, 0:7] + floating_base_vel = body_state[0, 36:42] + floating_base_acc = body_state[0, 106:112] + torso_quat = body_state[0, 141:145] + torso_ang_vel = body_state[0, 145:148] + + return { + "body_q": body_q, + "body_dq": body_dq, + "body_ddq": body_ddq, + "body_tau_est": body_tau_est, + "floating_base_pose": floating_base_pose, + "floating_base_vel": floating_base_vel, + "floating_base_acc": floating_base_acc, + "torso_quat": torso_quat, + "torso_ang_vel": torso_ang_vel, + } + + def queue_action(self, action: dict[str, any]): + # action should contain body_q, body_dq, body_tau + self.body_command_sender.send_command( + action["body_q"], action["body_dq"], action["body_tau"] + ) + + def observation_space(self) -> gym.Space: + return gym.spaces.Dict( + { + "body_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)), + "body_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)), + "floating_base_pose": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)), + "floating_base_vel": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)), + } + ) + + def action_space(self) -> gym.Space: + return gym.spaces.Dict( + { + "body_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)), + "body_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)), + "body_tau": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(29,)), + } + ) diff --git a/gr00t_wbc/control/envs/g1/g1_env.py b/gr00t_wbc/control/envs/g1/g1_env.py new file mode 100644 index 0000000..f687060 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/g1_env.py @@ -0,0 +1,324 @@ +from copy import deepcopy +from typing import Dict + +import gymnasium as gym +import numpy as np +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.base.humanoid_env import Hands, HumanoidEnv +from gr00t_wbc.control.envs.g1.g1_body import G1Body +from gr00t_wbc.control.envs.g1.g1_hand import G1ThreeFingerHand +from gr00t_wbc.control.envs.g1.sim.simulator_factory import SimulatorFactory, init_channel +from gr00t_wbc.control.envs.g1.utils.joint_safety import JointSafetyMonitor +from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.utils.ros_utils import ROSManager + + +class G1Env(HumanoidEnv): + def __init__( + self, + env_name: str = "default", + robot_model: RobotModel = None, + wbc_version: str = "v2", + config: Dict[str, any] = None, + **kwargs, + ): + super().__init__() + self.robot_model = deepcopy(robot_model) # need to cache FK results + self.config = config + + # Initialize safety monitor (visualization disabled) + self.safety_monitor = JointSafetyMonitor( + robot_model, enable_viz=False, env_type=self.config.get("ENV_TYPE", "real") + ) + self.last_obs = None + self.last_safety_ok = True # Track last safety status from queue_action + + init_channel(config=self.config) + + # Initialize body and hands + self._body = G1Body(config=self.config) + + self.with_hands = config.get("with_hands", False) + + # Gravity compensation settings + self.enable_gravity_compensation = config.get("enable_gravity_compensation", False) + self.gravity_compensation_joints = config.get("gravity_compensation_joints", ["arms"]) + + if self.enable_gravity_compensation: + print( + f"Gravity compensation enabled for joint groups: {self.gravity_compensation_joints}" + ) + if self.with_hands: + self._hands = Hands() + self._hands.left = G1ThreeFingerHand(is_left=True) + self._hands.right = G1ThreeFingerHand(is_left=False) + + # Initialize simulator if in simulation mode + self.use_sim = self.config.get("ENV_TYPE") == "sim" + + if self.use_sim: + # Create simulator using factory + + kwargs.update( + { + "onscreen": self.config.get("ENABLE_ONSCREEN", True), + "offscreen": self.config.get("ENABLE_OFFSCREEN", False), + } + ) + self.sim = SimulatorFactory.create_simulator( + config=self.config, + env_name=env_name, + wbc_version=wbc_version, + body_ik_solver_settings_type=kwargs.get("body_ik_solver_settings_type", "default"), + **kwargs, + ) + else: + self.sim = None + + # using the real robot + self.calibrate_hands() + + # Initialize ROS 2 node + self.ros_manager = ROSManager(node_name="g1_env") + self.ros_node = self.ros_manager.node + + self.delay_list = [] + self.visualize_delay = False + self.print_delay_interval = 100 + self.cnt = 0 + + def start_simulator(self): + # imag epublish disabled since the sim is running in a sub-thread + SimulatorFactory.start_simulator(self.sim, as_thread=True, enable_image_publish=False) + + def step_simulator(self): + sim_num_steps = int(self.config["REWARD_DT"] / self.config["SIMULATE_DT"]) + for _ in range(sim_num_steps): + self.sim.sim_env.sim_step() + self.sim.sim_env.update_viewer() + + def body(self) -> G1Body: + return self._body + + def hands(self) -> Hands: + if not self.with_hands: + raise RuntimeError( + "Hands not initialized. Use --with_hands True to enable hand functionality." + ) + return self._hands + + def observe(self) -> Dict[str, any]: + # Get observations from body and hands + body_obs = self.body().observe() + + body_q = body_obs["body_q"] + body_dq = body_obs["body_dq"] + body_ddq = body_obs["body_ddq"] + body_tau_est = body_obs["body_tau_est"] + + if self.with_hands: + left_hand_obs = self.hands().left.observe() + right_hand_obs = self.hands().right.observe() + left_hand_q = left_hand_obs["hand_q"] + right_hand_q = right_hand_obs["hand_q"] + left_hand_dq = left_hand_obs["hand_dq"] + right_hand_dq = right_hand_obs["hand_dq"] + left_hand_ddq = left_hand_obs["hand_ddq"] + right_hand_ddq = right_hand_obs["hand_ddq"] + left_hand_tau_est = left_hand_obs["hand_tau_est"] + right_hand_tau_est = right_hand_obs["hand_tau_est"] + + # Body and hand joint measurements come in actuator order, so we need to convert them to joint order + whole_q = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_q, + left_hand_actuated_joint_values=left_hand_q, + right_hand_actuated_joint_values=right_hand_q, + ) + whole_dq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_dq, + left_hand_actuated_joint_values=left_hand_dq, + right_hand_actuated_joint_values=right_hand_dq, + ) + whole_ddq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_ddq, + left_hand_actuated_joint_values=left_hand_ddq, + right_hand_actuated_joint_values=right_hand_ddq, + ) + whole_tau_est = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_tau_est, + left_hand_actuated_joint_values=left_hand_tau_est, + right_hand_actuated_joint_values=right_hand_tau_est, + ) + else: + # Body and hand joint measurements come in actuator order, so we need to convert them to joint order + whole_q = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_q, + ) + whole_dq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_dq, + ) + whole_ddq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_ddq, + ) + whole_tau_est = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_tau_est, + ) + + eef_obs = self.get_eef_obs(whole_q) + + obs = { + "q": whole_q, + "dq": whole_dq, + "ddq": whole_ddq, + "tau_est": whole_tau_est, + "floating_base_pose": body_obs["floating_base_pose"], + "floating_base_vel": body_obs["floating_base_vel"], + "floating_base_acc": body_obs["floating_base_acc"], + "wrist_pose": np.concatenate([eef_obs["left_wrist_pose"], eef_obs["right_wrist_pose"]]), + "torso_quat": body_obs["torso_quat"], + "torso_ang_vel": body_obs["torso_ang_vel"], + } + + if self.use_sim and self.sim: + obs.update(self.sim.get_privileged_obs()) + + # Store last observation for safety checking + self.last_obs = obs + + return obs + + @property + def observation_space(self) -> gym.Space: + # @todo: check if the low and high bounds are correct for body_obs. + q_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + dq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + ddq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + tau_est_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + floating_base_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)) + floating_base_vel_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)) + floating_base_acc_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)) + wrist_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7 + 7,)) + return gym.spaces.Dict( + { + "floating_base_pose": floating_base_pose_space, + "floating_base_vel": floating_base_vel_space, + "floating_base_acc": floating_base_acc_space, + "q": q_space, + "dq": dq_space, + "ddq": ddq_space, + "tau_est": tau_est_space, + "wrist_pose": wrist_pose_space, + } + ) + + def queue_action(self, action: Dict[str, any]): + # Safety check + if self.last_obs is not None: + safety_result = self.safety_monitor.handle_violations(self.last_obs, action) + action = safety_result["action"] + + # Map action from joint order to actuator order + body_actuator_q = self.robot_model.get_body_actuated_joints(action["q"]) + + self.body().queue_action( + { + "body_q": body_actuator_q, + "body_dq": np.zeros_like(body_actuator_q), + "body_tau": np.zeros_like(body_actuator_q), + } + ) + + if self.with_hands: + left_hand_actuator_q = self.robot_model.get_hand_actuated_joints( + action["q"], side="left" + ) + right_hand_actuator_q = self.robot_model.get_hand_actuated_joints( + action["q"], side="right" + ) + self.hands().left.queue_action({"hand_q": left_hand_actuator_q}) + self.hands().right.queue_action({"hand_q": right_hand_actuator_q}) + + def action_space(self) -> gym.Space: + return gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + + def calibrate_hands(self): + """Calibrate the hand joint qpos if real robot""" + if self.with_hands: + print("calibrating left hand") + self.hands().left.calibrate_hand() + print("calibrating right hand") + self.hands().right.calibrate_hand() + else: + print("Skipping hand calibration - hands disabled") + + def set_ik_indicator(self, teleop_cmd): + """Set the IK indicators for the simulator""" + if self.config["SIMULATOR"] == "robocasa": + if "left_wrist" in teleop_cmd and "right_wrist" in teleop_cmd: + left_wrist_input_pose = teleop_cmd["left_wrist"] + right_wrist_input_pose = teleop_cmd["right_wrist"] + ik_wrapper = self.sim.env.env.unwrapped.env + ik_wrapper.set_target_poses_outside_env( + [left_wrist_input_pose, right_wrist_input_pose] + ) + else: + raise NotImplementedError("IK indicators are only implemented for robocasa simulator") + + def set_sync_mode(self, sync_mode: bool, steps_per_action: int = 4): + """When set to True, the simulator will wait for the action to be sent to it""" + if self.config["SIMULATOR"] == "robocasa": + self.sim.set_sync_mode(sync_mode, steps_per_action) + + def reset(self): + if self.sim: + self.sim.reset() + + def close(self): + if self.sim: + self.sim.close() + + def robot_model(self) -> RobotModel: + return self.robot_model + + def get_reward(self): + if self.sim: + return self.sim.get_reward() + + def reset_obj_pos(self): + if hasattr(self.sim, "base_env") and hasattr(self.sim.base_env, "reset_obj_pos"): + self.sim.base_env.reset_obj_pos() + + def get_eef_obs(self, q: np.ndarray) -> Dict[str, np.ndarray]: + self.robot_model.cache_forward_kinematics(q) + eef_obs = {} + for side in ["left", "right"]: + wrist_placement = self.robot_model.frame_placement( + self.robot_model.supplemental_info.hand_frame_names[side] + ) + wrist_pos, wrist_quat = wrist_placement.translation[:3], R.from_matrix( + wrist_placement.rotation + ).as_quat(scalar_first=True) + eef_obs[f"{side}_wrist_pose"] = np.concatenate([wrist_pos, wrist_quat]) + + return eef_obs + + def get_joint_safety_status(self) -> bool: + """Get current joint safety status from the last queue_action safety check. + + Returns: + bool: True if joints are safe (no shutdown required), False if unsafe + """ + return self.last_safety_ok + + def handle_keyboard_button(self, key): + # Only handles keyboard buttons for the mujoco simulator for now. + if self.use_sim and self.config.get("SIMULATOR", "mujoco") == "mujoco": + self.sim.handle_keyboard_button(key) + + +if __name__ == "__main__": + env = G1Env(robot_model=instantiate_g1_robot_model(), wbc_version="gear_wbc") + while True: + print(env.observe()) diff --git a/gr00t_wbc/control/envs/g1/g1_hand.py b/gr00t_wbc/control/envs/g1/g1_hand.py new file mode 100644 index 0000000..1473aa2 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/g1_hand.py @@ -0,0 +1,89 @@ +import time + +import gymnasium as gym +import numpy as np + +from gr00t_wbc.control.base.env import Env +from gr00t_wbc.control.envs.g1.utils.command_sender import HandCommandSender +from gr00t_wbc.control.envs.g1.utils.state_processor import HandStateProcessor + + +class G1ThreeFingerHand(Env): + def __init__(self, is_left: bool = True): + super().__init__() + self.is_left = is_left + self.hand_state_processor = HandStateProcessor(is_left=self.is_left) + self.hand_command_sender = HandCommandSender(is_left=self.is_left) + self.hand_q_offset = np.zeros(7) + + def observe(self) -> dict[str, any]: + hand_state = self.hand_state_processor._prepare_low_state() # (1, 28) + assert hand_state.shape == (1, 28) + + # Apply offset to the hand state + hand_state[0, :7] = hand_state[0, :7] + self.hand_q_offset + + hand_q = hand_state[0, :7] + hand_dq = hand_state[0, 7:14] + hand_ddq = hand_state[0, 21:28] + hand_tau_est = hand_state[0, 14:21] + + # Return the state for this specific hand (left or right) + return { + "hand_q": hand_q, + "hand_dq": hand_dq, + "hand_ddq": hand_ddq, + "hand_tau_est": hand_tau_est, + } + + def queue_action(self, action: dict[str, any]): + # Apply offset to the hand target + action["hand_q"] = action["hand_q"] - self.hand_q_offset + + # action should contain hand_q + self.hand_command_sender.send_command(action["hand_q"]) + + def observation_space(self) -> gym.Space: + return gym.spaces.Dict( + { + "hand_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)), + "hand_dq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)), + "hand_ddq": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)), + "hand_tau_est": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)), + } + ) + + def action_space(self) -> gym.Space: + return gym.spaces.Dict({"hand_q": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,))}) + + def calibrate_hand(self): + hand_obs = self.observe() + hand_q = hand_obs["hand_q"] + + hand_q_target = np.zeros_like(hand_q) + hand_q_target[0] = hand_q[0] + + # joint limit + hand_q0_upper_limit = np.deg2rad(60) # lower limit is -60 + + # move the figure counterclockwise until the limit + while True: + + if hand_q_target[0] - hand_q[0] < np.deg2rad(60): + hand_q_target[0] += np.deg2rad(10) + else: + self.hand_q_offset[0] = hand_q0_upper_limit - hand_q[0] + break + + self.queue_action({"hand_q": hand_q_target}) + + hand_obs = self.observe() + hand_q = hand_obs["hand_q"] + + time.sleep(0.1) + + print("done calibration, q0 offset (deg):", np.rad2deg(self.hand_q_offset[0])) + + # done calibrating, set target to zero + self.hand_q_target = np.zeros_like(hand_q) + self.queue_action({"hand_q": self.hand_q_target}) diff --git a/gr00t_wbc/control/envs/g1/sim/__init__.py b/gr00t_wbc/control/envs/g1/sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/g1/sim/base_sim.py b/gr00t_wbc/control/envs/g1/sim/base_sim.py new file mode 100644 index 0000000..6848eb6 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/base_sim.py @@ -0,0 +1,772 @@ +import argparse +import pathlib +from pathlib import Path +import threading +from threading import Lock, Thread +from typing import Dict + +import mujoco +import mujoco.viewer +import numpy as np +import rclpy +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +import yaml + +from gr00t_wbc.control.envs.g1.sim.image_publish_utils import ImagePublishProcess +from gr00t_wbc.control.envs.g1.sim.metric_utils import check_contact, check_height +from gr00t_wbc.control.envs.g1.sim.sim_utilts import get_subtree_body_names +from gr00t_wbc.control.envs.g1.sim.unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge + +GR00T_WBC_ROOT = Path(__file__).resolve().parent.parent.parent.parent.parent.parent + + +class DefaultEnv: + """Base environment class that handles simulation environment setup and step""" + + def __init__( + self, + config: Dict[str, any], + env_name: str = "default", + camera_configs: Dict[str, any] = {}, + onscreen: bool = False, + offscreen: bool = False, + enable_image_publish: bool = False, + ): + # global_view is only set up for this specifc scene for now. + if config["ROBOT_SCENE"] == "gr00t_wbc/control/robot_model/model_data/g1/scene_29dof.xml": + camera_configs["global_view"] = { + "height": 400, + "width": 400, + } + self.config = config + self.env_name = env_name + self.num_body_dof = self.config["NUM_JOINTS"] + self.num_hand_dof = self.config["NUM_HAND_JOINTS"] + self.sim_dt = self.config["SIMULATE_DT"] + self.obs = None + self.torques = np.zeros(self.num_body_dof + self.num_hand_dof * 2) + self.torque_limit = np.array(self.config["motor_effort_limit_list"]) + self.camera_configs = camera_configs + + # Thread safety lock + self.reward_lock = Lock() + + # Unitree bridge will be initialized by the simulator + self.unitree_bridge = None + + # Store display mode + self.onscreen = onscreen + + # Initialize scene (defined in subclasses) + self.init_scene() + self.last_reward = 0 + + # Setup offscreen rendering if needed + self.offscreen = offscreen + if self.offscreen: + self.init_renderers() + self.image_dt = self.config.get("IMAGE_DT", 0.033333) + self.image_publish_process = None + + def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555): + # Use spawn method for better GIL isolation, or configured method + if len(self.camera_configs) == 0: + print( + "Warning: No camera configs provided, image publishing subprocess will not be started" + ) + return + start_method = self.config.get("MP_START_METHOD", "spawn") + self.image_publish_process = ImagePublishProcess( + camera_configs=self.camera_configs, + image_dt=self.image_dt, + zmq_port=camera_port, + start_method=start_method, + verbose=self.config.get("verbose", False), + ) + self.image_publish_process.start_process() + + def init_scene(self): + """Initialize the default robot scene""" + self.mj_model = mujoco.MjModel.from_xml_path( + str(pathlib.Path(GR00T_WBC_ROOT) / self.config["ROBOT_SCENE"]) + ) + self.mj_data = mujoco.MjData(self.mj_model) + self.mj_model.opt.timestep = self.sim_dt + self.torso_index = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, "torso_link") + self.root_body = "pelvis" + # Enable the elastic band + if self.config["ENABLE_ELASTIC_BAND"]: + self.elastic_band = ElasticBand() + if "g1" in self.config["ROBOT_TYPE"]: + if self.config["enable_waist"]: + self.band_attached_link = self.mj_model.body("pelvis").id + else: + self.band_attached_link = self.mj_model.body("torso_link").id + elif "h1" in self.config["ROBOT_TYPE"]: + self.band_attached_link = self.mj_model.body("torso_link").id + else: + self.band_attached_link = self.mj_model.body("base_link").id + + if self.onscreen: + self.viewer = mujoco.viewer.launch_passive( + self.mj_model, + self.mj_data, + key_callback=self.elastic_band.MujuocoKeyCallback, + show_left_ui=False, + show_right_ui=False, + ) + else: + mujoco.mj_forward(self.mj_model, self.mj_data) + self.viewer = None + else: + if self.onscreen: + self.viewer = mujoco.viewer.launch_passive( + self.mj_model, self.mj_data, show_left_ui=False, show_right_ui=False + ) + else: + mujoco.mj_forward(self.mj_model, self.mj_data) + self.viewer = None + + if self.viewer: + # viewer camera + self.viewer.cam.azimuth = 120 # Horizontal rotation in degrees + self.viewer.cam.elevation = -30 # Vertical tilt in degrees + self.viewer.cam.distance = 2.0 # Distance from camera to target + self.viewer.cam.lookat = np.array([0, 0, 0.5]) # Point the camera is looking at + + # Note that the actuator order is the same as the joint order in the mujoco model. + self.body_joint_index = [] + self.left_hand_index = [] + self.right_hand_index = [] + for i in range(self.mj_model.njnt): + name = self.mj_model.joint(i).name + if any( + [ + part_name in name + for part_name in ["hip", "knee", "ankle", "waist", "shoulder", "elbow", "wrist"] + ] + ): + self.body_joint_index.append(i) + elif "left_hand" in name: + self.left_hand_index.append(i) + elif "right_hand" in name: + self.right_hand_index.append(i) + + assert len(self.body_joint_index) == self.config["NUM_JOINTS"] + assert len(self.left_hand_index) == self.config["NUM_HAND_JOINTS"] + assert len(self.right_hand_index) == self.config["NUM_HAND_JOINTS"] + + self.body_joint_index = np.array(self.body_joint_index) + self.left_hand_index = np.array(self.left_hand_index) + self.right_hand_index = np.array(self.right_hand_index) + + def init_renderers(self): + # Initialize camera renderers + self.renderers = {} + for camera_name, camera_config in self.camera_configs.items(): + renderer = mujoco.Renderer( + self.mj_model, height=camera_config["height"], width=camera_config["width"] + ) + self.renderers[camera_name] = renderer + + def compute_body_torques(self) -> np.ndarray: + """Compute body torques based on the current robot state""" + body_torques = np.zeros(self.num_body_dof) + if self.unitree_bridge is not None and self.unitree_bridge.low_cmd: + for i in range(self.unitree_bridge.num_body_motor): + if self.unitree_bridge.use_sensor: + body_torques[i] = ( + self.unitree_bridge.low_cmd.motor_cmd[i].tau + + self.unitree_bridge.low_cmd.motor_cmd[i].kp + * (self.unitree_bridge.low_cmd.motor_cmd[i].q - self.mj_data.sensordata[i]) + + self.unitree_bridge.low_cmd.motor_cmd[i].kd + * ( + self.unitree_bridge.low_cmd.motor_cmd[i].dq + - self.mj_data.sensordata[i + self.unitree_bridge.num_body_motor] + ) + ) + else: + body_torques[i] = ( + self.unitree_bridge.low_cmd.motor_cmd[i].tau + + self.unitree_bridge.low_cmd.motor_cmd[i].kp + * ( + self.unitree_bridge.low_cmd.motor_cmd[i].q + - self.mj_data.qpos[self.body_joint_index[i] + 7 - 1] + ) + + self.unitree_bridge.low_cmd.motor_cmd[i].kd + * ( + self.unitree_bridge.low_cmd.motor_cmd[i].dq + - self.mj_data.qvel[self.body_joint_index[i] + 6 - 1] + ) + ) + return body_torques + + def compute_hand_torques(self) -> np.ndarray: + """Compute hand torques based on the current robot state""" + left_hand_torques = np.zeros(self.num_hand_dof) + right_hand_torques = np.zeros(self.num_hand_dof) + if self.unitree_bridge is not None and self.unitree_bridge.low_cmd: + for i in range(self.unitree_bridge.num_hand_motor): + left_hand_torques[i] = ( + self.unitree_bridge.left_hand_cmd.motor_cmd[i].tau + + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kp + * ( + self.unitree_bridge.left_hand_cmd.motor_cmd[i].q + - self.mj_data.qpos[self.left_hand_index[i] + 7 - 1] + ) + + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kd + * ( + self.unitree_bridge.left_hand_cmd.motor_cmd[i].dq + - self.mj_data.qvel[self.left_hand_index[i] + 6 - 1] + ) + ) + right_hand_torques[i] = ( + self.unitree_bridge.right_hand_cmd.motor_cmd[i].tau + + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kp + * ( + self.unitree_bridge.right_hand_cmd.motor_cmd[i].q + - self.mj_data.qpos[self.right_hand_index[i] + 7 - 1] + ) + + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kd + * ( + self.unitree_bridge.right_hand_cmd.motor_cmd[i].dq + - self.mj_data.qvel[self.right_hand_index[i] + 6 - 1] + ) + ) + return np.concatenate((left_hand_torques, right_hand_torques)) + + def compute_body_qpos(self) -> np.ndarray: + """Compute body joint positions based on the current command""" + body_qpos = np.zeros(self.num_body_dof) + if self.unitree_bridge is not None and self.unitree_bridge.low_cmd: + for i in range(self.unitree_bridge.num_body_motor): + body_qpos[i] = self.unitree_bridge.low_cmd.motor_cmd[i].q + return body_qpos + + def compute_hand_qpos(self) -> np.ndarray: + """Compute hand joint positions based on the current command""" + hand_qpos = np.zeros(self.num_hand_dof * 2) + if self.unitree_bridge is not None and self.unitree_bridge.low_cmd: + for i in range(self.unitree_bridge.num_hand_motor): + hand_qpos[i] = self.unitree_bridge.left_hand_cmd.motor_cmd[i].q + hand_qpos[i + self.num_hand_dof] = self.unitree_bridge.right_hand_cmd.motor_cmd[i].q + return hand_qpos + + def prepare_obs(self) -> Dict[str, any]: + """Prepare observation dictionary from the current robot state""" + obs = {} + obs["floating_base_pose"] = self.mj_data.qpos[:7] + obs["floating_base_vel"] = self.mj_data.qvel[:6] + obs["floating_base_acc"] = self.mj_data.qacc[:6] + obs["secondary_imu_quat"] = self.mj_data.xquat[self.torso_index] + obs["secondary_imu_vel"] = self.mj_data.cvel[self.torso_index] + obs["body_q"] = self.mj_data.qpos[self.body_joint_index + 7 - 1] + obs["body_dq"] = self.mj_data.qvel[self.body_joint_index + 6 - 1] + obs["body_ddq"] = self.mj_data.qacc[self.body_joint_index + 6 - 1] + obs["body_tau_est"] = self.mj_data.actuator_force[self.body_joint_index - 1] + if self.num_hand_dof > 0: + obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_index + 7 - 1] + obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_index + 6 - 1] + obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_index + 6 - 1] + obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_index - 1] + obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_index + 7 - 1] + obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_index + 6 - 1] + obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_index + 6 - 1] + obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_index - 1] + obs["time"] = self.mj_data.time + return obs + + def sim_step(self): + self.obs = self.prepare_obs() + self.unitree_bridge.PublishLowState(self.obs) + if self.unitree_bridge.joystick: + self.unitree_bridge.PublishWirelessController() + if self.config["ENABLE_ELASTIC_BAND"]: + if self.elastic_band.enable: + # Get Cartesian pose and velocity of the band_attached_link + pose = np.concatenate( + [ + self.mj_data.xpos[self.band_attached_link], # link position in world + self.mj_data.xquat[ + self.band_attached_link + ], # link quaternion in world [w,x,y,z] + np.zeros(6), # placeholder for velocity + ] + ) + + # Get velocity in world frame + mujoco.mj_objectVelocity( + self.mj_model, + self.mj_data, + mujoco.mjtObj.mjOBJ_BODY, + self.band_attached_link, + pose[7:13], + 0, # 0 for world frame + ) + + # Reorder velocity from [ang, lin] to [lin, ang] + pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy() + self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose) + else: + # explicitly resetting the force when the band is not enabled + self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6) + body_torques = self.compute_body_torques() + hand_torques = self.compute_hand_torques() + self.torques[self.body_joint_index - 1] = body_torques + if self.num_hand_dof > 0: + self.torques[self.left_hand_index - 1] = hand_torques[: self.num_hand_dof] + self.torques[self.right_hand_index - 1] = hand_torques[self.num_hand_dof :] + + self.torques = np.clip(self.torques, -self.torque_limit, self.torque_limit) + + if self.config["FREE_BASE"]: + self.mj_data.ctrl = np.concatenate((np.zeros(6), self.torques)) + else: + self.mj_data.ctrl = self.torques + mujoco.mj_step(self.mj_model, self.mj_data) + # self.check_self_collision() + + def kinematics_step(self): + """ + Run kinematics only: compute the qpos of the robot and directly set the qpos. + For debugging purposes. + """ + if self.unitree_bridge is not None: + self.unitree_bridge.PublishLowState(self.prepare_obs()) + if self.unitree_bridge.joystick: + self.unitree_bridge.PublishWirelessController() + + if self.config["ENABLE_ELASTIC_BAND"]: + if self.elastic_band.enable: + # Get Cartesian pose and velocity of the band_attached_link + pose = np.concatenate( + [ + self.mj_data.xpos[self.band_attached_link], # link position in world + self.mj_data.xquat[ + self.band_attached_link + ], # link quaternion in world [w,x,y,z] + np.zeros(6), # placeholder for velocity + ] + ) + + # Get velocity in world frame + mujoco.mj_objectVelocity( + self.mj_model, + self.mj_data, + mujoco.mjtObj.mjOBJ_BODY, + self.band_attached_link, + pose[7:13], + 0, # 0 for world frame + ) + + # Reorder velocity from [ang, lin] to [lin, ang] + pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy() + + self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose) + else: + # explicitly resetting the force when the band is not enabled + self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6) + + body_qpos = self.compute_body_qpos() # (num_body_dof,) + hand_qpos = self.compute_hand_qpos() # (num_hand_dof * 2,) + + self.mj_data.qpos[self.body_joint_index + 7 - 1] = body_qpos + self.mj_data.qpos[self.left_hand_index + 7 - 1] = hand_qpos[: self.num_hand_dof] + self.mj_data.qpos[self.right_hand_index + 7 - 1] = hand_qpos[self.num_hand_dof :] + + mujoco.mj_kinematics(self.mj_model, self.mj_data) + mujoco.mj_comPos(self.mj_model, self.mj_data) + + def apply_perturbation(self, key): + """Apply perturbation to the robot""" + # Add velocity perturbations in body frame + perturbation_x_body = 0.0 # forward/backward in body frame + perturbation_y_body = 0.0 # left/right in body frame + if key == "up": + perturbation_x_body = 1.0 # forward + elif key == "down": + perturbation_x_body = -1.0 # backward + elif key == "left": + perturbation_y_body = 1.0 # left + elif key == "right": + perturbation_y_body = -1.0 # right + + # Transform body frame velocity to world frame using MuJoCo's rotation + vel_body = np.array([perturbation_x_body, perturbation_y_body, 0.0]) + vel_world = np.zeros(3) + base_quat = self.mj_data.qpos[3:7] # [w, x, y, z] quaternion + + # Use MuJoCo's robust quaternion rotation (handles invalid quaternions automatically) + mujoco.mju_rotVecQuat(vel_world, vel_body, base_quat) + + # Apply to base linear velocity in world frame + self.mj_data.qvel[0] += vel_world[0] # world X velocity + self.mj_data.qvel[1] += vel_world[1] # world Y velocity + + # Update dynamics after velocity change + mujoco.mj_forward(self.mj_model, self.mj_data) + + def update_viewer(self): + if self.viewer is not None: + self.viewer.sync() + + def update_viewer_camera(self): + if self.viewer is not None: + if self.viewer.cam.type == mujoco.mjtCamera.mjCAMERA_TRACKING: + self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE + else: + self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING + + def update_reward(self): + """Calculate reward. Should be implemented by subclasses.""" + with self.reward_lock: + self.last_reward = 0 + + def get_reward(self): + """Thread-safe way to get the last calculated reward.""" + with self.reward_lock: + return self.last_reward + + def set_unitree_bridge(self, unitree_bridge): + """Set the unitree bridge from the simulator""" + self.unitree_bridge = unitree_bridge + + def get_privileged_obs(self): + """Get privileged observation. Should be implemented by subclasses.""" + return {} + + def update_render_caches(self): + """Update render cache and shared memory for subprocess.""" + render_caches = {} + for camera_name, camera_config in self.camera_configs.items(): + renderer = self.renderers[camera_name] + if "params" in camera_config: + renderer.update_scene(self.mj_data, camera=camera_config["params"]) + else: + renderer.update_scene(self.mj_data, camera=camera_name) + render_caches[camera_name + "_image"] = renderer.render() + + # Update shared memory if image publishing process is available + if self.image_publish_process is not None: + self.image_publish_process.update_shared_memory(render_caches) + + return render_caches + + def handle_keyboard_button(self, key): + if self.elastic_band is not None: + self.elastic_band.handle_keyboard_button(key) + + if key == "backspace": + self.reset() + if key == "v": + self.update_viewer_camera() + if key in ["up", "down", "left", "right"]: + self.apply_perturbation(key) + + def check_fall(self): + """Check if the robot has fallen""" + self.fall = False + if self.mj_data.qpos[2] < 0.2: + self.fall = True + print(f"Warning: Robot has fallen, height: {self.mj_data.qpos[2]:.3f} m") + + if self.fall: + self.reset() + + def check_self_collision(self): + """Check for self-collision of the robot""" + robot_bodies = get_subtree_body_names(self.mj_model, self.mj_model.body(self.root_body).id) + self_collision, contact_bodies = check_contact( + self.mj_model, self.mj_data, robot_bodies, robot_bodies, return_all_contact_bodies=True + ) + if self_collision: + print(f"Warning: Self-collision detected: {contact_bodies}") + return self_collision + + def reset(self): + mujoco.mj_resetData(self.mj_model, self.mj_data) + + +class CubeEnv(DefaultEnv): + """Environment with a cube object for pick and place tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + enable_image_publish: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml" + super().__init__(config, "cube", {}, onscreen, offscreen, enable_image_publish) + + def update_reward(self): + """Calculate reward based on gripper contact with cube and cube height""" + right_hand_body = [ + "right_hand_thumb_2_link", + "right_hand_middle_1_link", + "right_hand_index_1_link", + ] + gripper_cube_contact = check_contact( + self.mj_model, self.mj_data, right_hand_body, "cube_body" + ) + cube_lifted = check_height(self.mj_model, self.mj_data, "cube", 0.85, 2.0) + + with self.reward_lock: + self.last_reward = gripper_cube_contact & cube_lifted + + +class BoxEnv(DefaultEnv): + """Environment with a box object for manipulation tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + enable_image_publish: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml" + super().__init__(config, "box", {}, onscreen, offscreen, enable_image_publish) + + def reward(self): + """Calculate reward based on gripper contact with cube and cube height""" + left_hand_body = [ + "left_hand_thumb_2_link", + "left_hand_middle_1_link", + "left_hand_index_1_link", + ] + right_hand_body = [ + "right_hand_thumb_2_link", + "right_hand_middle_1_link", + "right_hand_index_1_link", + ] + gripper_box_contact = check_contact(self.mj_model, self.mj_data, left_hand_body, "box_body") + gripper_box_contact &= check_contact( + self.mj_model, self.mj_data, right_hand_body, "box_body" + ) + box_lifted = check_height(self.mj_model, self.mj_data, "box", 0.92, 2.0) + + print("gripper_box_contact: ", gripper_box_contact, "box_lifted: ", box_lifted) + + with self.reward_lock: + self.last_reward = gripper_box_contact & box_lifted + return self.last_reward + + +class BottleEnv(DefaultEnv): + """Environment with a cylinder object for manipulation tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + enable_image_publish: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml" + camera_configs = { + "egoview": { + "height": 400, + "width": 400, + }, + } + super().__init__( + config, "cylinder", camera_configs, onscreen, offscreen, enable_image_publish + ) + + self.bottle_body = self.mj_model.body("bottle_body") + self.bottle_geom = self.mj_model.geom("bottle") + + if self.viewer is not None: + self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED + self.viewer.cam.fixedcamid = self.mj_model.camera("egoview").id + + def update_reward(self): + """Calculate reward based on gripper contact with cylinder and cylinder height""" + pass + + def get_privileged_obs(self): + obs_pos = self.mj_data.xpos[self.bottle_body.id] + obs_quat = self.mj_data.xquat[self.bottle_body.id] + return {"bottle_pos": obs_pos, "bottle_quat": obs_quat} + + +class BaseSimulator: + """Base simulator class that handles initialization and running of simulations""" + + def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs): + self.config = config + self.env_name = env_name + + # Initialize ROS 2 node + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node("sim_mujoco") + self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) + self.thread.start() + else: + self.thread = None + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] # will only take the first node + + # Create rate objects for different update frequencies + self.sim_dt = self.config["SIMULATE_DT"] + self.reward_dt = self.config.get("REWARD_DT", 0.02) + self.image_dt = self.config.get("IMAGE_DT", 0.033333) + self.viewer_dt = self.config.get("VIEWER_DT", 0.02) + self.rate = self.node.create_rate(1 / self.sim_dt) + + # Create the appropriate environment based on name + if env_name == "default": + self.sim_env = DefaultEnv(config, env_name, **kwargs) + elif env_name == "pnp_cube": + self.sim_env = CubeEnv(config, **kwargs) + elif env_name == "lift_box": + self.sim_env = BoxEnv(config, **kwargs) + elif env_name == "pnp_bottle": + self.sim_env = BottleEnv(config, **kwargs) + else: + raise ValueError(f"Invalid environment name: {env_name}") + + # Initialize the DDS communication layer - should be safe to call multiple times + + try: + if self.config.get("INTERFACE", None): + ChannelFactoryInitialize(self.config["DOMAIN_ID"], self.config["INTERFACE"]) + else: + ChannelFactoryInitialize(self.config["DOMAIN_ID"]) + except Exception as e: + # If it fails because it's already initialized, that's okay + print(f"Note: Channel factory initialization attempt: {e}") + + # Initialize the unitree bridge and pass it to the environment + self.init_unitree_bridge() + self.sim_env.set_unitree_bridge(self.unitree_bridge) + + # Initialize additional components + self.init_subscriber() + self.init_publisher() + + self.sim_thread = None + + def start_as_thread(self): + # Create simulation thread + self.sim_thread = Thread(target=self.start) + self.sim_thread.start() + + def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555): + """Start the image publish subprocess""" + self.sim_env.start_image_publish_subprocess(start_method, camera_port) + + def init_subscriber(self): + """Initialize subscribers. Can be overridden by subclasses.""" + pass + + def init_publisher(self): + """Initialize publishers. Can be overridden by subclasses.""" + pass + + def init_unitree_bridge(self): + """Initialize the unitree SDK bridge""" + self.unitree_bridge = UnitreeSdk2Bridge(self.config) + if self.config["USE_JOYSTICK"]: + self.unitree_bridge.SetupJoystick( + device_id=self.config["JOYSTICK_DEVICE"], js_type=self.config["JOYSTICK_TYPE"] + ) + + def start(self): + """Main simulation loop""" + sim_cnt = 0 + + try: + while ( + self.sim_env.viewer and self.sim_env.viewer.is_running() + ) or self.sim_env.viewer is None: + # Run simulation step + self.sim_env.sim_step() + + # Update viewer at viewer rate + if sim_cnt % int(self.viewer_dt / self.sim_dt) == 0: + self.sim_env.update_viewer() + + # Calculate reward at reward rate + if sim_cnt % int(self.reward_dt / self.sim_dt) == 0: + self.sim_env.update_reward() + + # Update render caches at image rate + if sim_cnt % int(self.image_dt / self.sim_dt) == 0: + self.sim_env.update_render_caches() + + # Sleep to maintain correct rate + self.rate.sleep() + + sim_cnt += 1 + except rclpy.exceptions.ROSInterruptException: + # This is expected when ROS shuts down - exit cleanly + pass + except Exception: + self.close() + + def __del__(self): + """Clean up resources when simulator is deleted""" + self.close() + + def reset(self): + """Reset the simulation. Can be overridden by subclasses.""" + self.sim_env.reset() + + def close(self): + """Close the simulation. Can be overridden by subclasses.""" + try: + # Stop image publishing subprocess + if self.sim_env.image_publish_process is not None: + self.sim_env.image_publish_process.stop() + + # Close viewer + if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None: + self.sim_env.viewer.close() + + # Shutdown ROS + if rclpy.ok(): + rclpy.shutdown() + except Exception as e: + print(f"Warning during close: {e}") + + def get_privileged_obs(self): + obs = self.sim_env.get_privileged_obs() + # TODO: add ros2 topic to get privileged obs + return obs + + def handle_keyboard_button(self, key): + # Only handles keyboard buttons for default env. + if self.env_name == "default": + self.sim_env.handle_keyboard_button(key) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Robot") + parser.add_argument( + "--config", + type=str, + default="./gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml", + help="config file", + ) + args = parser.parse_args() + + with open(args.config, "r") as file: + config = yaml.load(file, Loader=yaml.FullLoader) + + if config.get("INTERFACE", None): + ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"]) + else: + ChannelFactoryInitialize(config["DOMAIN_ID"]) + + simulation = BaseSimulator(config) + simulation.start_as_thread() diff --git a/gr00t_wbc/control/envs/g1/sim/image_publish_utils.py b/gr00t_wbc/control/envs/g1/sim/image_publish_utils.py new file mode 100644 index 0000000..feed08c --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/image_publish_utils.py @@ -0,0 +1,256 @@ +import multiprocessing as mp +from multiprocessing import shared_memory +import time +from typing import Any, Dict + +import numpy as np + +from gr00t_wbc.control.sensor.sensor_server import ImageMessageSchema, SensorServer + + +def get_multiprocessing_info(verbose: bool = True): + """Get information about multiprocessing start methods""" + + if verbose: + print(f"Available start methods: {mp.get_all_start_methods()}") + return mp.get_start_method() + + +class ImagePublishProcess: + """Subprocess for publishing images using shared memory and ZMQ""" + + def __init__( + self, + camera_configs: Dict[str, Any], + image_dt: float, + zmq_port: int = 5555, + start_method: str = "spawn", + verbose: bool = False, + ): + self.camera_configs = camera_configs + self.image_dt = image_dt + self.zmq_port = zmq_port + self.verbose = verbose + self.shared_memory_blocks = {} + self.shared_memory_info = {} + self.process = None + + # Use specific context to avoid global state pollution + self.mp_context = mp.get_context(start_method) + if self.verbose: + print(f"Using multiprocessing context: {start_method}") + + self.stop_event = self.mp_context.Event() + self.data_ready_event = self.mp_context.Event() + + # Ensure events start in correct state + self.stop_event.clear() + self.data_ready_event.clear() + + if self.verbose: + print(f"Initial stop_event state: {self.stop_event.is_set()}") + print(f"Initial data_ready_event state: {self.data_ready_event.is_set()}") + + # Calculate shared memory requirements for each camera + for camera_name, camera_config in camera_configs.items(): + height = camera_config["height"] + width = camera_config["width"] + # RGB image: height * width * 3 (uint8) + size = height * width * 3 + + # Create shared memory block + shm = shared_memory.SharedMemory(create=True, size=size) + self.shared_memory_blocks[camera_name] = shm + self.shared_memory_info[camera_name] = { + "name": shm.name, + "size": size, + "shape": (height, width, 3), + "dtype": np.uint8, + } + + def start_process(self): + """Start the image publishing subprocess""" + if self.verbose: + print(f"Starting subprocess with stop_event state: {self.stop_event.is_set()}") + self.process = self.mp_context.Process( + target=self._image_publish_worker, + args=( + self.shared_memory_info, + self.image_dt, + self.zmq_port, + self.stop_event, + self.data_ready_event, + self.verbose, + ), + ) + self.process.start() + if self.verbose: + print(f"Subprocess started, PID: {self.process.pid}") + + def update_shared_memory(self, render_caches: Dict[str, np.ndarray]): + """Update shared memory with new rendered images""" + images_updated = 0 + for camera_name in self.camera_configs.keys(): + image_key = f"{camera_name}_image" + if image_key in render_caches: + image = render_caches[image_key] + + # Ensure image is uint8 and has correct shape + if image.dtype != np.uint8: + image = (image * 255).astype(np.uint8) + + # Get shared memory array + shm = self.shared_memory_blocks[camera_name] + shared_array = np.ndarray( + self.shared_memory_info[camera_name]["shape"], + dtype=self.shared_memory_info[camera_name]["dtype"], + buffer=shm.buf, + ) + + # Copy image data to shared memory atomically + np.copyto(shared_array, image) + images_updated += 1 + + # Signal that new data is ready only after all images are written + if images_updated > 0: + if self.verbose: + print(f"Main process: Updated {images_updated} images, setting data_ready_event") + self.data_ready_event.set() + elif self.verbose: + print( + "Main process: No images to update. " + "please check if camera configs are provided and the renderer is properly initialized" + ) + + def stop(self): + """Stop the image publishing subprocess""" + if self.verbose: + print("Stopping image publishing subprocess...") + self.stop_event.set() + + if self.process and self.process.is_alive(): + # Give the process time to clean up gracefully + self.process.join(timeout=5) + if self.process.is_alive(): + if self.verbose: + print("Subprocess didn't stop gracefully, terminating...") + self.process.terminate() + self.process.join(timeout=2) + if self.process.is_alive(): + if self.verbose: + print("Force killing subprocess...") + self.process.kill() + self.process.join() + + # Clean up shared memory + for camera_name, shm in self.shared_memory_blocks.items(): + try: + shm.close() + shm.unlink() + if self.verbose: + print(f"Cleaned up shared memory for {camera_name}") + except Exception as e: + if self.verbose: + print(f"Warning: Failed to cleanup shared memory for {camera_name}: {e}") + + self.shared_memory_blocks.clear() + if self.verbose: + print("Image publishing subprocess stopped and cleaned up") + + @staticmethod + def _image_publish_worker( + shared_memory_info, image_dt, zmq_port, stop_event, data_ready_event, verbose + ): + """Worker function that runs in the subprocess""" + if verbose: + print(f"Worker started! PID: {__import__('os').getpid()}") + print(f"Worker stop_event state at start: {stop_event.is_set()}") + print(f"Worker data_ready_event state at start: {data_ready_event.is_set()}") + + try: + # Initialize ZMQ sensor server + sensor_server = SensorServer() + sensor_server.start_server(port=zmq_port) + + # Connect to shared memory blocks + shared_arrays = {} + shm_blocks = {} + for camera_name, info in shared_memory_info.items(): + shm = shared_memory.SharedMemory(name=info["name"]) + shm_blocks[camera_name] = shm + shared_arrays[camera_name] = np.ndarray( + info["shape"], dtype=info["dtype"], buffer=shm.buf + ) + + print( + f"Image publishing subprocess started with {len(shared_arrays)} cameras on ZMQ port {zmq_port}" + ) + + loop_count = 0 + last_data_time = time.time() + + while not stop_event.is_set(): + loop_count += 1 + + # Wait for new data with shorter timeout for better responsiveness + timeout = min(image_dt, 0.1) # Max 100ms timeout + data_available = data_ready_event.wait(timeout=timeout) + + current_time = time.time() + + if data_available: + data_ready_event.clear() + if loop_count % 50 == 0: + print("Image publish frequency: ", 1 / (current_time - last_data_time)) + last_data_time = current_time + + # Collect all camera images and serialize them + try: + from gr00t_wbc.control.sensor.sensor_server import ImageUtils + + # Copy all images atomically at once + image_copies = {name: arr.copy() for name, arr in shared_arrays.items()} + + # Create message with all camera images + message_dict = { + "images": image_copies, + "timestamps": {name: current_time for name in image_copies.keys()}, + } + + # Create ImageMessageSchema and serialize + image_msg = ImageMessageSchema( + timestamps=message_dict.get("timestamps"), + images=message_dict.get("images", None), + ) + + # Serialize and send via ZMQ + serialized_data = image_msg.serialize() + + # Add individual camera images to the message + for camera_name, image_copy in image_copies.items(): + serialized_data[f"{camera_name}"] = ImageUtils.encode_image(image_copy) + + sensor_server.send_message(serialized_data) + + except Exception as e: + print(f"Error publishing images: {e}") + + elif verbose and loop_count % 10 == 0: + print(f"Subprocess: Still waiting for data... (iteration {loop_count})") + + # Small sleep to prevent busy waiting when no data + if not data_available: + time.sleep(0.001) + + except KeyboardInterrupt: + print("Image publisher interrupted by user") + finally: + # Clean up + try: + for shm in shm_blocks.values(): + shm.close() + sensor_server.stop_server() + except Exception as e: + print(f"Error during subprocess cleanup: {e}") + if verbose: + print("Image publish subprocess stopped") diff --git a/gr00t_wbc/control/envs/g1/sim/metric_utils.py b/gr00t_wbc/control/envs/g1/sim/metric_utils.py new file mode 100644 index 0000000..454577f --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/metric_utils.py @@ -0,0 +1,71 @@ +from typing import List, Tuple + +import mujoco + +from gr00t_wbc.control.envs.g1.sim.sim_utilts import get_body_geom_ids + + +def check_contact( + mj_model: mujoco.MjModel, + mj_data: mujoco.MjData, + bodies_1: List[str] | str, + bodies_2: List[str] | str, + return_all_contact_bodies: bool = False, +) -> Tuple[bool, List[Tuple[str, str]]] | bool: + """ + Finds contact between two body groups. Any geom in the body is considered to be in contact. + Args: + mj_model (MujocoModel): Current simulation object + mj_data (MjData): Current simulation data + bodies_1 (str or list of int): an individual body name or list of body names. + bodies_2 (str or list of int): another individual body name or list of body names. + Returns: + bool: True if any body in @bodies_1 is in contact with any body in @bodies_2. + """ + if isinstance(bodies_1, str): + bodies_1 = [bodies_1] + if isinstance(bodies_2, str): + bodies_2 = [bodies_2] + + geoms_1 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_1] + geoms_1 = [g for geom_list in geoms_1 for g in geom_list] + geoms_2 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_2] + geoms_2 = [g for geom_list in geoms_2 for g in geom_list] + contact_bodies = [] + for i in range(mj_data.ncon): + contact = mj_data.contact[i] + # check contact geom in geoms + c1_in_g1 = contact.geom1 in geoms_1 + c2_in_g2 = contact.geom2 in geoms_2 if geoms_2 is not None else True + # check contact geom in geoms (flipped) + c2_in_g1 = contact.geom2 in geoms_1 + c1_in_g2 = contact.geom1 in geoms_2 if geoms_2 is not None else True + if (c1_in_g1 and c2_in_g2) or (c1_in_g2 and c2_in_g1): + contact_bodies.append( + ( + mj_model.body(mj_model.geom(contact.geom1).bodyid).name, + mj_model.body(mj_model.geom(contact.geom2).bodyid).name, + ) + ) + if not return_all_contact_bodies: + break + if return_all_contact_bodies: + return len(contact_bodies) > 0, set(contact_bodies) + else: + return len(contact_bodies) > 0 + + +def check_height( + mj_model: mujoco.MjModel, + mj_data: mujoco.MjData, + geom_name: str, + lower_bound: float = -float("inf"), + upper_bound: float = float("inf"), +): + """ + Checks if the height of a geom is greater than a given height. + """ + geom_id = mj_model.geom(geom_name).id + return ( + mj_data.geom_xpos[geom_id][2] < upper_bound and mj_data.geom_xpos[geom_id][2] > lower_bound + ) diff --git a/gr00t_wbc/control/envs/g1/sim/robocasa_sim.py b/gr00t_wbc/control/envs/g1/sim/robocasa_sim.py new file mode 100644 index 0000000..a470018 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/robocasa_sim.py @@ -0,0 +1,63 @@ +from typing import Any, Dict, Tuple + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize + +from gr00t_wbc.control.envs.g1.sim.unitree_sdk2py_bridge import UnitreeSdk2Bridge +from gr00t_wbc.control.envs.robocasa.async_env_server import RoboCasaEnvServer +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model + + +class RoboCasaG1EnvServer(RoboCasaEnvServer): + def __init__( + self, env_name: str, wbc_config: Dict[str, Any], env_kwargs: Dict[str, Any], **kwargs + ): + if UnitreeSdk2Bridge is None: + raise ImportError("UnitreeSdk2Bridge is required for RoboCasaG1EnvServer") + self.wbc_config = wbc_config + _, robot_model = get_robot_type_and_model( + "G1", + enable_waist_ik=wbc_config["enable_waist"], + ) + if env_kwargs.get("camera_names", None) is None: + env_kwargs["camera_names"] = [ + "robot0_oak_egoview", + "robot0_oak_left_monoview", + "robot0_oak_right_monoview", + "robot0_rs_tppview", + ] + if env_kwargs.get("render_camera", None) is None: + if env_kwargs.get("renderer", "mjviewer") == "mjviewer": + env_kwargs["render_camera"] = "robot0_oak_egoview" + else: + env_kwargs["render_camera"] = [ + "robot0_oak_egoview", + "robot0_rs_tppview", + ] + + super().__init__(env_name, "G1", robot_model, env_kwargs=env_kwargs, **kwargs) + + def init_channel(self): + + try: + if self.wbc_config.get("INTERFACE", None): + ChannelFactoryInitialize(self.wbc_config["DOMAIN_ID"], self.wbc_config["INTERFACE"]) + else: + ChannelFactoryInitialize(self.wbc_config["DOMAIN_ID"]) + except Exception: + # If it fails because it's already initialized, that's okay + pass + + self.channel_bridge = UnitreeSdk2Bridge(config=self.wbc_config) + + def publish_obs(self): + # with self.cache_lock: + obs = self.caches["obs"] + self.channel_bridge.PublishLowState(obs) + + def get_action(self) -> Tuple[Dict[str, Any], bool, bool]: + q, ready, is_new_action = self.channel_bridge.GetAction() + return {"q": q}, ready, is_new_action + + def reset(self): + super().reset() + self.channel_bridge.reset() diff --git a/gr00t_wbc/control/envs/g1/sim/sim_utilts.py b/gr00t_wbc/control/envs/g1/sim/sim_utilts.py new file mode 100644 index 0000000..e032eb6 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/sim_utilts.py @@ -0,0 +1,96 @@ +""" +Utility functions for working with Mujoco models. +copied from https://github.com/kevinzakka/mink/blob/main/mink/utils.py +""" + +from typing import List + +import mujoco + + +def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]: + """Get immediate children bodies belonging to a given body. + + Args: + model: Mujoco model. + body_id: ID of body. + + Returns: + A List containing all child body ids. + """ + return [ + i + for i in range(model.nbody) + if model.body_parentid[i] == body_id and body_id != i # Exclude the body itself. + ] + + +def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]: + """Get all bodies belonging to subtree starting at a given body. + + Args: + model: Mujoco model. + body_id: ID of body where subtree starts. + + Returns: + A List containing all subtree body ids. + """ + body_ids: List[int] = [] + stack = [body_id] + while stack: + body_id = stack.pop() + body_ids.append(body_id) + stack += get_body_body_ids(model, body_id) + return body_ids + + +def get_subtree_body_names(model: mujoco.MjModel, body_id: int) -> List[str]: + """Get all bodies belonging to subtree starting at a given body. + Args: + model: Mujoco model. + body_id: ID of body where subtree starts. + + Returns: + A List containing all subtree body names. + """ + return [model.body(i).name for i in get_subtree_body_ids(model, body_id)] + + +def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]: + """Get immediate geoms belonging to a given body. + + Here, immediate geoms are those directly attached to the body and not its + descendants. + + Args: + model: Mujoco model. + body_id: ID of body. + + Returns: + A list containing all body geom ids. + """ + geom_start = model.body_geomadr[body_id] + geom_end = geom_start + model.body_geomnum[body_id] + return list(range(geom_start, geom_end)) + + +def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]: + """Get all geoms belonging to subtree starting at a given body. + + Here, a subtree is defined as the kinematic tree starting at the body and including + all its descendants. + + Args: + model: Mujoco model. + body_id: ID of body where subtree starts. + + Returns: + A list containing all subtree geom ids. + """ + geom_ids: List[int] = [] + stack = [body_id] + while stack: + body_id = stack.pop() + geom_ids.extend(get_body_geom_ids(model, body_id)) + stack += get_body_body_ids(model, body_id) + return geom_ids diff --git a/gr00t_wbc/control/envs/g1/sim/simulator_factory.py b/gr00t_wbc/control/envs/g1/sim/simulator_factory.py new file mode 100644 index 0000000..690d2f9 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/simulator_factory.py @@ -0,0 +1,144 @@ +import time +from typing import Any, Dict + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize + +from gr00t_wbc.control.envs.g1.sim.base_sim import BaseSimulator + + +def init_channel(config: Dict[str, Any]) -> None: + """ + Initialize the communication channel for simulator/robot communication. + + Args: + config: Configuration dictionary containing DOMAIN_ID and optionally INTERFACE + """ + if config.get("INTERFACE", None): + ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"]) + else: + ChannelFactoryInitialize(config["DOMAIN_ID"]) + + +class SimulatorFactory: + """Factory class for creating different types of simulators.""" + + @staticmethod + def create_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs): + """ + Create a simulator based on the configuration. + + Args: + config: Configuration dictionary containing SIMULATOR type + env_name: Environment name + **kwargs: Additional keyword arguments for specific simulators + """ + simulator_type = config.get("SIMULATOR", "mujoco") + if simulator_type == "mujoco": + return SimulatorFactory._create_mujoco_simulator(config, env_name, **kwargs) + elif simulator_type == "robocasa": + return SimulatorFactory._create_robocasa_simulator(config, env_name, **kwargs) + else: + print( + f"Warning: Invalid simulator type: {simulator_type}. " + "If you are using run_sim_loop, please ignore this warning." + ) + return None + + @staticmethod + def _create_mujoco_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs): + """Create a MuJoCo simulator instance.""" + env_kwargs = dict( + onscreen=kwargs.pop("onscreen", True), + offscreen=kwargs.pop("offscreen", False), + enable_image_publish=kwargs.get("enable_image_publish", False), + ) + return BaseSimulator(config=config, env_name=env_name, **env_kwargs) + + @staticmethod + def _create_robocasa_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs): + """Create a RoboCasa simulator instance.""" + from gr00t_wbc.control.envs.g1.sim.robocasa_sim import RoboCasaG1EnvServer + from gr00t_wbc.control.envs.robocasa.utils.controller_utils import ( + update_robosuite_controller_configs, + ) + from gr00t_wbc.control.envs.robocasa.utils.sim_utils import change_simulation_timestep + + change_simulation_timestep(config["SIMULATE_DT"]) + + # Use default environment if not specified + if env_name == "default": + env_name = "GroundOnly" + + # Get or create controller configurations + controller_configs = kwargs.get("controller_configs") + if controller_configs is None: + wbc_version = kwargs.get("wbc_version", "gear_wbc") + controller_configs = update_robosuite_controller_configs("G1", wbc_version) + + # Build environment kwargs + env_kwargs = dict( + onscreen=kwargs.pop("onscreen", True), + offscreen=kwargs.pop("offscreen", False), + camera_names=kwargs.pop("camera_names", None), + camera_heights=kwargs.pop("camera_heights", None), + camera_widths=kwargs.pop("camera_widths", None), + control_freq=kwargs.pop("control_freq", 50), + controller_configs=controller_configs, + ik_indicator=kwargs.pop("ik_indicator", False), + randomize_cameras=kwargs.pop("randomize_cameras", True), + ) + + kwargs.update( + { + "verbose": config.pop("verbose", False), + "sim_freq": 1 / config.pop("SIMULATE_DT"), + } + ) + + return RoboCasaG1EnvServer( + env_name=env_name, + wbc_config=config, + env_kwargs=env_kwargs, + **kwargs, + ) + + @staticmethod + def start_simulator( + simulator, + as_thread: bool = True, + enable_image_publish: bool = False, + mp_start_method: str = "spawn", + camera_port: int = 5555, + ): + """ + Start the simulator either as a thread or as a separate process. + + Args: + simulator: The simulator instance to start + config: Configuration dictionary + as_thread: If True, start as thread; if False, start as subprocess + enable_offscreen: If True and not as_thread, start image publishing + """ + + if as_thread: + simulator.start_as_thread() + else: + # Wrap in try-except to make sure simulator is properly closed upon exit. + try: + if enable_image_publish: + simulator.start_image_publish_subprocess( + start_method=mp_start_method, + camera_port=camera_port, + ) + time.sleep(1) + simulator.start() + except KeyboardInterrupt: + print("+++++Simulator interrupted by user.") + except Exception as e: + print(f"++++error in simulator: {e} ++++") + finally: + print("++++closing simulator ++++") + simulator.close() + + # Allow simulator to initialize + time.sleep(1) diff --git a/gr00t_wbc/control/envs/g1/sim/unitree_sdk2py_bridge.py b/gr00t_wbc/control/envs/g1/sim/unitree_sdk2py_bridge.py new file mode 100644 index 0000000..ad0ed90 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/sim/unitree_sdk2py_bridge.py @@ -0,0 +1,459 @@ +import sys +import threading +from typing import Dict, Tuple + +import glfw +from loguru import logger +import mujoco +import numpy as np +import pygame +import scipy.spatial.transform +from termcolor import colored +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from unitree_sdk2py.idl.default import ( + unitree_go_msg_dds__WirelessController_, + unitree_hg_msg_dds__HandCmd_ as HandCmd_default, + unitree_hg_msg_dds__HandState_ as HandState_default, +) +from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_, OdoState_ + + +class UnitreeSdk2Bridge: + """ + This class is responsible for bridging the Unitree SDK2 with the Gr00t environment. + It is responsible for sending and receiving messages to and from the Unitree SDK2. + Both the body and hand are supported. + """ + + def __init__(self, config): + # Note that we do not give the mjdata and mjmodel to the UnitreeSdk2Bridge. + # It is unsafe and would be unflexible if we use a hand-plugged robot model + + robot_type = config["ROBOT_TYPE"] + if "g1" in robot_type or "h1-2" in robot_type: + from unitree_sdk2py.idl.default import ( + unitree_hg_msg_dds__IMUState_ as IMUState_default, + unitree_hg_msg_dds__LowCmd_, + unitree_hg_msg_dds__LowState_ as LowState_default, + unitree_hg_msg_dds__OdoState_ as OdoState_default, + ) + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import IMUState_, LowCmd_, LowState_ + + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + elif "h1" == robot_type or "go2" == robot_type: + from unitree_sdk2py.idl.default import ( + unitree_go_msg_dds__LowCmd_, + unitree_go_msg_dds__LowState_ as LowState_default, + unitree_hg_msg_dds__IMUState_ as IMUState_default, + ) + from unitree_sdk2py.idl.unitree_go.msg.dds_ import IMUState_, LowCmd_, LowState_ + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + else: + raise ValueError(f"Invalid robot type '{robot_type}'. Expected 'g1', 'h1', or 'go2'.") + + self.num_body_motor = config["NUM_MOTORS"] + self.num_hand_motor = config.get("NUM_HAND_MOTORS", 0) + self.use_sensor = config["USE_SENSOR"] + + self.have_imu_ = False + self.have_frame_sensor_ = False + # if self.use_sensor: + # MOTOR_SENSOR_NUM = 3 + # self.dim_motor_sensor = MOTOR_SENSOR_NUM * self.num_motor + # # Check sensor + # for i in range(self.dim_motor_sensor, self.mj_model.nsensor): + # name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i) + # if name == "imu_quat": + # self.have_imu_ = True + # if name == "frame_pos": + # self.have_frame_sensor_ = True + + # Unitree sdk2 message + self.low_state = LowState_default() + self.low_state_puber = ChannelPublisher("rt/lowstate", LowState_) + self.low_state_puber.Init() + + # Only create odo_state for supported robot types + if "g1" in robot_type or "h1-2" in robot_type: + self.odo_state = OdoState_default() + self.odo_state_puber = ChannelPublisher("rt/odostate", OdoState_) + self.odo_state_puber.Init() + else: + self.odo_state = None + self.odo_state_puber = None + self.torso_imu_state = IMUState_default() + self.torso_imu_puber = ChannelPublisher("rt/secondary_imu", IMUState_) + self.torso_imu_puber.Init() + + self.left_hand_state = HandState_default() + self.left_hand_state_puber = ChannelPublisher("rt/dex3/left/state", HandState_) + self.left_hand_state_puber.Init() + self.right_hand_state = HandState_default() + self.right_hand_state_puber = ChannelPublisher("rt/dex3/right/state", HandState_) + self.right_hand_state_puber.Init() + + self.low_cmd_suber = ChannelSubscriber("rt/lowcmd", LowCmd_) + self.low_cmd_suber.Init(self.LowCmdHandler, 1) + + self.left_hand_cmd = HandCmd_default() + self.left_hand_cmd_suber = ChannelSubscriber("rt/dex3/left/cmd", HandCmd_) + self.left_hand_cmd_suber.Init(self.LeftHandCmdHandler, 1) + self.right_hand_cmd = HandCmd_default() + self.right_hand_cmd_suber = ChannelSubscriber("rt/dex3/right/cmd", HandCmd_) + self.right_hand_cmd_suber.Init(self.RightHandCmdHandler, 1) + + self.low_cmd_lock = threading.Lock() + self.left_hand_cmd_lock = threading.Lock() + self.right_hand_cmd_lock = threading.Lock() + + self.wireless_controller = unitree_go_msg_dds__WirelessController_() + self.wireless_controller_puber = ChannelPublisher( + "rt/wirelesscontroller", WirelessController_ + ) + self.wireless_controller_puber.Init() + + # joystick + self.key_map = { + "R1": 0, + "L1": 1, + "start": 2, + "select": 3, + "R2": 4, + "L2": 5, + "F1": 6, + "F2": 7, + "A": 8, + "B": 9, + "X": 10, + "Y": 11, + "up": 12, + "right": 13, + "down": 14, + "left": 15, + } + self.joystick = None + + self.reset() + + def reset(self): + with self.low_cmd_lock: + self.low_cmd_received = False + self.new_low_cmd = False + with self.left_hand_cmd_lock: + self.left_hand_cmd_received = False + self.new_left_hand_cmd = False + with self.right_hand_cmd_lock: + self.right_hand_cmd_received = False + self.new_right_hand_cmd = False + + def LowCmdHandler(self, msg): + with self.low_cmd_lock: + self.low_cmd = msg + self.low_cmd_received = True + self.new_low_cmd = True + + def LeftHandCmdHandler(self, msg): + with self.left_hand_cmd_lock: + self.left_hand_cmd = msg + self.left_hand_cmd_received = True + self.new_left_hand_cmd = True + + def RightHandCmdHandler(self, msg): + with self.right_hand_cmd_lock: + self.right_hand_cmd = msg + self.right_hand_cmd_received = True + self.new_right_hand_cmd = True + + def cmd_received(self): + with self.low_cmd_lock: + low_cmd_received = self.low_cmd_received + with self.left_hand_cmd_lock: + left_hand_cmd_received = self.left_hand_cmd_received + with self.right_hand_cmd_lock: + right_hand_cmd_received = self.right_hand_cmd_received + return low_cmd_received or left_hand_cmd_received or right_hand_cmd_received + + def PublishLowState(self, obs: Dict[str, any]): + # publish body state + if self.use_sensor: + raise NotImplementedError("Sensor data is not implemented yet.") + else: + for i in range(self.num_body_motor): + self.low_state.motor_state[i].q = obs["body_q"][i] + self.low_state.motor_state[i].dq = obs["body_dq"][i] + self.low_state.motor_state[i].ddq = obs["body_ddq"][i] + self.low_state.motor_state[i].tau_est = obs["body_tau_est"][i] + + if self.use_sensor and self.have_frame_sensor_: + raise NotImplementedError("Frame sensor data is not implemented yet.") + else: + # Get data from ground truth + self.odo_state.position[:] = obs["floating_base_pose"][:3] + self.odo_state.linear_velocity[:] = obs["floating_base_vel"][:3] + self.odo_state.orientation[:] = obs["floating_base_pose"][3:7] + self.odo_state.angular_velocity[:] = obs["floating_base_vel"][3:6] + # quaternion: w, x, y, z + self.low_state.imu_state.quaternion[:] = obs["floating_base_pose"][3:7] + # angular velocity + self.low_state.imu_state.gyroscope[:] = obs["floating_base_vel"][3:6] + # linear acceleration + self.low_state.imu_state.accelerometer[:] = obs["floating_base_acc"][:3] + + self.torso_imu_state.quaternion[:] = obs["secondary_imu_quat"] + self.torso_imu_state.gyroscope[:] = obs["secondary_imu_vel"][3:6] + + # acceleration: x, y, z (only available when frame sensor is enabled) + if self.have_frame_sensor_: + raise NotImplementedError("Frame sensor data is not implemented yet.") + self.low_state.tick = int(obs["time"] * 1e3) + self.low_state_puber.Write(self.low_state) + + self.odo_state.tick = int(obs["time"] * 1e3) + self.odo_state_puber.Write(self.odo_state) + + self.torso_imu_puber.Write(self.torso_imu_state) + + # publish hand state + for i in range(self.num_hand_motor): + self.left_hand_state.motor_state[i].q = obs["left_hand_q"][i] + self.left_hand_state.motor_state[i].dq = obs["left_hand_dq"][i] + self.left_hand_state_puber.Write(self.left_hand_state) + + for i in range(self.num_hand_motor): + self.right_hand_state.motor_state[i].q = obs["right_hand_q"][i] + self.right_hand_state.motor_state[i].dq = obs["right_hand_dq"][i] + self.right_hand_state_puber.Write(self.right_hand_state) + + def GetAction(self) -> Tuple[np.ndarray, bool, bool]: + with self.low_cmd_lock: + body_q = [self.low_cmd.motor_cmd[i].q for i in range(self.num_body_motor)] + with self.left_hand_cmd_lock: + left_hand_q = [self.left_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)] + with self.right_hand_cmd_lock: + right_hand_q = [self.right_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)] + with self.low_cmd_lock and self.left_hand_cmd_lock and self.right_hand_cmd_lock: + is_new_action = self.new_low_cmd and self.new_left_hand_cmd and self.new_right_hand_cmd + if is_new_action: + self.new_low_cmd = False + self.new_left_hand_cmd = False + self.new_right_hand_cmd = False + + return ( + np.concatenate([body_q[:-7], left_hand_q, body_q[-7:], right_hand_q]), + self.cmd_received(), + is_new_action, + ) + + def PublishWirelessController(self): + if self.joystick is not None: + pygame.event.get() + key_state = [0] * 16 + key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"]) + key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"]) + key_state[self.key_map["start"]] = self.joystick.get_button(self.button_id["START"]) + key_state[self.key_map["select"]] = self.joystick.get_button(self.button_id["SELECT"]) + key_state[self.key_map["R2"]] = self.joystick.get_axis(self.axis_id["RT"]) > 0 + key_state[self.key_map["L2"]] = self.joystick.get_axis(self.axis_id["LT"]) > 0 + key_state[self.key_map["F1"]] = 0 + key_state[self.key_map["F2"]] = 0 + key_state[self.key_map["A"]] = self.joystick.get_button(self.button_id["A"]) + key_state[self.key_map["B"]] = self.joystick.get_button(self.button_id["B"]) + key_state[self.key_map["X"]] = self.joystick.get_button(self.button_id["X"]) + key_state[self.key_map["Y"]] = self.joystick.get_button(self.button_id["Y"]) + key_state[self.key_map["up"]] = self.joystick.get_hat(0)[1] > 0 + key_state[self.key_map["right"]] = self.joystick.get_hat(0)[0] > 0 + key_state[self.key_map["down"]] = self.joystick.get_hat(0)[1] < 0 + key_state[self.key_map["left"]] = self.joystick.get_hat(0)[0] < 0 + + key_value = 0 + for i in range(16): + key_value += key_state[i] << i + + self.wireless_controller.keys = key_value + self.wireless_controller.lx = self.joystick.get_axis(self.axis_id["LX"]) + self.wireless_controller.ly = -self.joystick.get_axis(self.axis_id["LY"]) + self.wireless_controller.rx = self.joystick.get_axis(self.axis_id["RX"]) + self.wireless_controller.ry = -self.joystick.get_axis(self.axis_id["RY"]) + + self.wireless_controller_puber.Write(self.wireless_controller) + + def SetupJoystick(self, device_id=0, js_type="xbox"): + pygame.init() + pygame.joystick.init() + joystick_count = pygame.joystick.get_count() + if joystick_count > 0: + self.joystick = pygame.joystick.Joystick(device_id) + self.joystick.init() + else: + print("No gamepad detected.") + sys.exit() + + if js_type == "xbox": + if sys.platform.startswith("linux"): + self.axis_id = { + "LX": 0, # Left stick axis x + "LY": 1, # Left stick axis y + "RX": 3, # Right stick axis x + "RY": 4, # Right stick axis y + "LT": 2, # Left trigger + "RT": 5, # Right trigger + "DX": 6, # Directional pad x + "DY": 7, # Directional pad y + } + self.button_id = { + "X": 2, + "Y": 3, + "B": 1, + "A": 0, + "LB": 4, + "RB": 5, + "SELECT": 6, + "START": 7, + "XBOX": 8, + "LSB": 9, + "RSB": 10, + } + elif sys.platform == "darwin": + self.axis_id = { + "LX": 0, # Left stick axis x + "LY": 1, # Left stick axis y + "RX": 2, # Right stick axis x + "RY": 3, # Right stick axis y + "LT": 4, # Left trigger + "RT": 5, # Right trigger + } + self.button_id = { + "X": 2, + "Y": 3, + "B": 1, + "A": 0, + "LB": 9, + "RB": 10, + "SELECT": 4, + "START": 6, + "XBOX": 5, + "LSB": 7, + "RSB": 8, + "DYU": 11, + "DYD": 12, + "DXL": 13, + "DXR": 14, + } + else: + print("Unsupported OS. ") + + elif js_type == "switch": + # Yuanhang: may differ for different OS, need to be checked + self.axis_id = { + "LX": 0, # Left stick axis x + "LY": 1, # Left stick axis y + "RX": 2, # Right stick axis x + "RY": 3, # Right stick axis y + "LT": 5, # Left trigger + "RT": 4, # Right trigger + "DX": 6, # Directional pad x + "DY": 7, # Directional pad y + } + + self.button_id = { + "X": 3, + "Y": 4, + "B": 1, + "A": 0, + "LB": 6, + "RB": 7, + "SELECT": 10, + "START": 11, + } + else: + print("Unsupported gamepad. ") + + def PrintSceneInformation(self): + print(" ") + logger.info(colored("<<------------- Link ------------->>", "green")) + for i in range(self.mj_model.nbody): + name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_BODY, i) + if name: + logger.info(f"link_index: {i}, name: {name}") + print(" ") + + logger.info(colored("<<------------- Joint ------------->>", "green")) + for i in range(self.mj_model.njnt): + name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_JOINT, i) + if name: + logger.info(f"joint_index: {i}, name: {name}") + print(" ") + + logger.info(colored("<<------------- Actuator ------------->>", "green")) + for i in range(self.mj_model.nu): + name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_ACTUATOR, i) + if name: + logger.info(f"actuator_index: {i}, name: {name}") + print(" ") + + logger.info(colored("<<------------- Sensor ------------->>", "green")) + index = 0 + for i in range(self.mj_model.nsensor): + name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i) + if name: + logger.info( + f"sensor_index: {index}, name: {name}, dim: {self.mj_model.sensor_dim[i]}" + ) + index = index + self.mj_model.sensor_dim[i] + print(" ") + + +class ElasticBand: + """ + ref: https://github.com/unitreerobotics/unitree_mujoco + """ + + def __init__(self): + self.kp_pos = 10000 + self.kd_pos = 1000 + self.kp_ang = 1000 + self.kd_ang = 10 + self.point = np.array([0, 0, 1]) + self.length = 0 + self.enable = True + + def Advance(self, pose): + """ + Args: + pose: 13D array containing: + - pose[0:3]: position in world frame + - pose[3:7]: quaternion [w,x,y,z] in world frame + - pose[7:10]: linear velocity in world frame + - pose[10:13]: angular velocity in world frame + Returns: + np.ndarray: 6D vector [fx, fy, fz, tx, ty, tz] + """ + pos = pose[0:3] + quat = pose[3:7] + lin_vel = pose[7:10] + ang_vel = pose[10:13] + + δx = self.point - pos + f = self.kp_pos * (δx + np.array([0, 0, self.length])) + self.kd_pos * (0 - lin_vel) + + # --- Orientation PD control for torque --- + quat = np.array([quat[1], quat[2], quat[3], quat[0]]) # reorder to [x,y,z,w] for scipy + rot = scipy.spatial.transform.Rotation.from_quat(quat) + rotvec = rot.as_rotvec() # axis-angle error + torque = -self.kp_ang * rotvec - self.kd_ang * ang_vel + + return np.concatenate([f, torque]) + + def MujuocoKeyCallback(self, key): + if key == glfw.KEY_7: + self.length -= 0.1 + if key == glfw.KEY_8: + self.length += 0.1 + if key == glfw.KEY_9: + self.enable = not self.enable + + def handle_keyboard_button(self, key): + if key == "9": + self.enable = not self.enable diff --git a/gr00t_wbc/control/envs/g1/utils/__init__.py b/gr00t_wbc/control/envs/g1/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/g1/utils/command_sender.py b/gr00t_wbc/control/envs/g1/utils/command_sender.py new file mode 100644 index 0000000..41f60b3 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/utils/command_sender.py @@ -0,0 +1,146 @@ +from typing import Dict + +import numpy as np +from unitree_sdk2py.core.channel import ChannelPublisher +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_ +from unitree_sdk2py.utils.crc import CRC + + +class BodyCommandSender: + def __init__(self, config: Dict): + self.config = config + if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2": + from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ + from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ + + self.low_cmd = unitree_go_msg_dds__LowCmd_() + elif ( + self.config["ROBOT_TYPE"] == "g1_29dof" + or self.config["ROBOT_TYPE"] == "h1-2_21dof" + or self.config["ROBOT_TYPE"] == "h1-2_27dof" + ): + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ + + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + else: + raise NotImplementedError( + f"Robot type {self.config['ROBOT_TYPE']} is not supported yet" + ) + # init kp kd + self.kp_level = 1.0 + self.waist_kp_level = 1.0 + self.robot_kp = np.zeros(self.config["NUM_MOTORS"]) + self.robot_kd = np.zeros(self.config["NUM_MOTORS"]) + # set kp level + for i in range(len(self.config["MOTOR_KP"])): + self.robot_kp[i] = self.config["MOTOR_KP"][i] * self.kp_level + for i in range(len(self.config["MOTOR_KD"])): + self.robot_kd[i] = self.config["MOTOR_KD"][i] * 1.0 + self.weak_motor_joint_index = [] + for _, value in self.config["WeakMotorJointIndex"].items(): + self.weak_motor_joint_index.append(value) + # init low cmd publisher + self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_) + self.lowcmd_publisher_.Init() + self.InitLowCmd() + self.low_state = None + self.crc = CRC() + + def InitLowCmd(self): + # h1/go2: + if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2": + self.low_cmd.head[0] = 0xFE + self.low_cmd.head[1] = 0xEF + else: + pass + + self.low_cmd.level_flag = 0xFF + self.low_cmd.gpio = 0 + for i in range(self.config["NUM_MOTORS"]): + if self.is_weak_motor(i): + self.low_cmd.motor_cmd[i].mode = 0x01 + else: + self.low_cmd.motor_cmd[i].mode = 0x0A + self.low_cmd.motor_cmd[i].q = self.config["UNITREE_LEGGED_CONST"]["PosStopF"] + self.low_cmd.motor_cmd[i].kp = 0 + self.low_cmd.motor_cmd[i].dq = self.config["UNITREE_LEGGED_CONST"]["VelStopF"] + self.low_cmd.motor_cmd[i].kd = 0 + self.low_cmd.motor_cmd[i].tau = 0 + if ( + self.config["ROBOT_TYPE"] == "g1_29dof" + or self.config["ROBOT_TYPE"] == "h1-2_21dof" + or self.config["ROBOT_TYPE"] == "h1-2_27dof" + ): + self.low_cmd.mode_machine = self.config["UNITREE_LEGGED_CONST"]["MODE_MACHINE"] + self.low_cmd.mode_pr = self.config["UNITREE_LEGGED_CONST"]["MODE_PR"] + else: + pass + + def is_weak_motor(self, motor_index: int) -> bool: + return motor_index in self.weak_motor_joint_index + + def send_command(self, cmd_q: np.ndarray, cmd_dq: np.ndarray, cmd_tau: np.ndarray): + for i in range(self.config["NUM_MOTORS"]): + motor_index = self.config["JOINT2MOTOR"][i] + joint_index = self.config["MOTOR2JOINT"][i] + # print(f"motor_index: {motor_index}, joint_index: {joint_index}") + if joint_index == -1: + # send default joint position command + self.low_cmd.motor_cmd[motor_index].q = self.config["DEFAULT_MOTOR_ANGLES"][ + motor_index + ] + self.low_cmd.motor_cmd[motor_index].dq = 0.0 + self.low_cmd.motor_cmd[motor_index].tau = 0.0 + else: + self.low_cmd.motor_cmd[motor_index].q = cmd_q[joint_index] + self.low_cmd.motor_cmd[motor_index].dq = cmd_dq[joint_index] + self.low_cmd.motor_cmd[motor_index].tau = cmd_tau[joint_index] + # kp kd + self.low_cmd.motor_cmd[motor_index].kp = self.robot_kp[motor_index] + self.low_cmd.motor_cmd[motor_index].kd = self.robot_kd[motor_index] + + self.low_cmd.crc = self.crc.Crc(self.low_cmd) + self.lowcmd_publisher_.Write(self.low_cmd) + + +def make_hand_mode(motor_index: int) -> int: + status = 0x01 + timeout = 0x01 + mode = motor_index & 0x0F + mode |= status << 4 # bits [4..6] + mode |= timeout << 7 # bit 7 + return mode + + +class HandCommandSender: + def __init__(self, is_left: bool = True): + self.is_left = is_left + if self.is_left: + self.cmd_pub = ChannelPublisher("rt/dex3/left/cmd", HandCmd_) + else: + self.cmd_pub = ChannelPublisher("rt/dex3/right/cmd", HandCmd_) + + self.cmd_pub.Init() + self.cmd = unitree_hg_msg_dds__HandCmd_() + + self.hand_dof = 7 + + self.kp = [1.0] * self.hand_dof + self.kd = [0.2] * self.hand_dof + self.kp[0] = 2.0 + self.kd[0] = 0.5 + + def send_command(self, cmd: np.ndarray): + for i in range(self.hand_dof): + # Build the bitfield mode (see your C++ example) + mode_val = make_hand_mode(i) + self.cmd.motor_cmd[i].mode = mode_val + self.cmd.motor_cmd[i].q = cmd[i] + self.cmd.motor_cmd[i].dq = 0.0 + self.cmd.motor_cmd[i].tau = 0.0 + self.cmd.motor_cmd[i].kp = self.kp[i] + self.cmd.motor_cmd[i].kd = self.kd[i] + + self.cmd_pub.Write(self.cmd) diff --git a/gr00t_wbc/control/envs/g1/utils/joint_safety.py b/gr00t_wbc/control/envs/g1/utils/joint_safety.py new file mode 100644 index 0000000..113bd8c --- /dev/null +++ b/gr00t_wbc/control/envs/g1/utils/joint_safety.py @@ -0,0 +1,534 @@ +"""Joint safety monitor for G1 robot. + +This module implements safety monitoring for arm and finger joint velocities using +joint groups defined in the robot model's supplemental info. Leg joints are not monitored. +""" + +from datetime import datetime +import sys +import time +from typing import Dict, List, Optional, Tuple + +import numpy as np + +from gr00t_wbc.data.viz.rerun_viz import RerunViz + + +class JointSafetyMonitor: + """Monitor joint velocities for G1 robot arms and hands.""" + + # Velocity limits in rad/s + ARM_VELOCITY_LIMIT = 6.0 # rad/s for arm joints + HAND_VELOCITY_LIMIT = 50.0 # rad/s for finger joints + + def __init__(self, robot_model, enable_viz: bool = False, env_type: str = "real"): + """Initialize joint safety monitor. + + Args: + robot_model: The robot model containing joint information + enable_viz: If True, enable rerun visualization (default False) + env_type: Environment type - "sim" or "real" (default "real") + """ + self.robot_model = robot_model + self.safety_margin = 1.0 # Hardcoded safety margin + self.enable_viz = enable_viz + self.env_type = env_type + + # Startup ramping parameters + self.control_frequency = 50 # Hz, hardcoded from run_g1_control_loop.py + self.ramp_duration_steps = int(2.0 * self.control_frequency) # 2 seconds * 50Hz = 100 steps + self.startup_counter = 0 + self.initial_positions = None + self.startup_complete = False + + # Initialize velocity and position limits for monitored joints + self.velocity_limits = {} + self.position_limits = {} + self._initialize_limits() + + # Track violations for reporting + self.violations = [] + + # Initialize visualization + self.right_arm_indices = None + self.right_arm_joint_names = [] + self.left_arm_indices = None + self.left_arm_joint_names = [] + self.right_hand_indices = None + self.right_hand_joint_names = [] + self.left_hand_indices = None + self.left_hand_joint_names = [] + try: + arm_indices = self.robot_model.get_joint_group_indices("arms") + all_joint_names = [self.robot_model.joint_names[i] for i in arm_indices] + # Filter for right and left arm joints + self.right_arm_joint_names = [ + name for name in all_joint_names if name.startswith("right_") + ] + self.right_arm_indices = [ + self.robot_model.joint_to_dof_index[name] for name in self.right_arm_joint_names + ] + self.left_arm_joint_names = [ + name for name in all_joint_names if name.startswith("left_") + ] + self.left_arm_indices = [ + self.robot_model.joint_to_dof_index[name] for name in self.left_arm_joint_names + ] + # Hand joints + hand_indices = self.robot_model.get_joint_group_indices("hands") + all_hand_names = [self.robot_model.joint_names[i] for i in hand_indices] + self.right_hand_joint_names = [ + name for name in all_hand_names if name.startswith("right_") + ] + self.right_hand_indices = [ + self.robot_model.joint_to_dof_index[name] for name in self.right_hand_joint_names + ] + self.left_hand_joint_names = [ + name for name in all_hand_names if name.startswith("left_") + ] + self.left_hand_indices = [ + self.robot_model.joint_to_dof_index[name] for name in self.left_hand_joint_names + ] + except ValueError as e: + print(f"[JointSafetyMonitor] Warning: Could not initialize arm/hand visualization: {e}") + except Exception: + pass + + # Use single tensor_key for each plot + self.right_arm_pos_key = "right_arm_qpos" + self.left_arm_pos_key = "left_arm_qpos" + self.right_arm_vel_key = "right_arm_dq" + self.left_arm_vel_key = "left_arm_dq" + self.right_hand_pos_key = "right_hand_qpos" + self.left_hand_pos_key = "left_hand_qpos" + self.right_hand_vel_key = "right_hand_dq" + self.left_hand_vel_key = "left_hand_dq" + + # Define a consistent color palette for up to 8 joints (tab10 + extra) + self.joint_colors = [ + [31, 119, 180], # blue + [255, 127, 14], # orange + [44, 160, 44], # green + [214, 39, 40], # red + [148, 103, 189], # purple + [140, 86, 75], # brown + [227, 119, 194], # pink + [127, 127, 127], # gray (for 8th joint if needed) + ] + + # Initialize Rerun visualization only if enabled + self.viz = None + if self.enable_viz: + try: + self.viz = RerunViz( + image_keys=[], + tensor_keys=[ + self.right_arm_pos_key, + self.left_arm_pos_key, + self.right_arm_vel_key, + self.left_arm_vel_key, + self.right_hand_pos_key, + self.left_hand_pos_key, + self.right_hand_vel_key, + self.left_hand_vel_key, + ], + window_size=10.0, + app_name="joint_safety_monitor", + ) + except Exception: + self.viz = None + + def _initialize_limits(self): + """Initialize velocity and position limits for arm and hand joints using robot model joint groups.""" + if self.robot_model.supplemental_info is None: + raise ValueError("Robot model must have supplemental_info to use joint groups") + + # Get arm joint indices from robot model joint groups + try: + arm_indices = self.robot_model.get_joint_group_indices("arms") + arm_joint_names = [self.robot_model.joint_names[i] for i in arm_indices] + + for joint_name in arm_joint_names: + # Set velocity limits + vel_limit = self.ARM_VELOCITY_LIMIT * self.safety_margin + self.velocity_limits[joint_name] = {"min": -vel_limit, "max": vel_limit} + + # Set position limits from robot model + if joint_name in self.robot_model.joint_to_dof_index: + joint_idx = self.robot_model.joint_to_dof_index[joint_name] + # Adjust index for floating base if present + limit_idx = joint_idx - (7 if self.robot_model.is_floating_base_model else 0) + + if 0 <= limit_idx < len(self.robot_model.lower_joint_limits): + pos_min = self.robot_model.lower_joint_limits[limit_idx] + pos_max = self.robot_model.upper_joint_limits[limit_idx] + + # Apply safety margin to position limits + pos_range = pos_max - pos_min + margin = pos_range * (1.0 - self.safety_margin) / 2.0 + + self.position_limits[joint_name] = { + "min": pos_min + margin, + "max": pos_max - margin, + } + except ValueError as e: + print(f"[JointSafetyMonitor] Warning: Could not find 'arms' joint group: {e}") + + # Get hand joint indices from robot model joint groups + try: + hand_indices = self.robot_model.get_joint_group_indices("hands") + hand_joint_names = [self.robot_model.joint_names[i] for i in hand_indices] + + for joint_name in hand_joint_names: + # Set velocity limits only for hands (no position limits for now) + vel_limit = self.HAND_VELOCITY_LIMIT * self.safety_margin + self.velocity_limits[joint_name] = {"min": -vel_limit, "max": vel_limit} + except ValueError as e: + print(f"[JointSafetyMonitor] Warning: Could not find 'hands' joint group: {e}") + + def check_safety(self, obs: Dict, action: Dict) -> Tuple[bool, List[Dict]]: + """Check if current velocities and positions are within safe bounds. + + Args: + obs: Observation dictionary containing joint positions and velocities + action: Action dictionary containing target positions + + Returns: + (is_safe, violations): Tuple of safety status and list of violations + Note: is_safe=False only for velocity violations (triggers shutdown) + Position violations are warnings only (don't affect is_safe) + """ + self.violations = [] + is_safe = True + joint_names = self.robot_model.joint_names + + # Check current joint velocities (critical - triggers shutdown) + if "dq" in obs: + joint_velocities = obs["dq"] + + for i, joint_name in enumerate(joint_names): + # Only check monitored joints + if joint_name not in self.velocity_limits: + continue + + if i < len(joint_velocities): + velocity = joint_velocities[i] + limits = self.velocity_limits[joint_name] + + if velocity < limits["min"] or velocity > limits["max"]: + violation = { + "joint": joint_name, + "type": "velocity", + "value": velocity, + "limit_min": limits["min"], + "limit_max": limits["max"], + "exceeded_by": self._calculate_exceeded_percentage( + velocity, limits["min"], limits["max"] + ), + "critical": True, # Velocity violations are critical + } + self.violations.append(violation) + is_safe = False + + # Check current joint positions (warning only - no shutdown) + if "q" in obs: + joint_positions = obs["q"] + + for i, joint_name in enumerate(joint_names): + # Only check joints with position limits (arms) + if joint_name not in self.position_limits: + continue + + if i < len(joint_positions): + position = joint_positions[i] + limits = self.position_limits[joint_name] + + if position < limits["min"] or position > limits["max"]: + violation = { + "joint": joint_name, + "type": "position", + "value": position, + "limit_min": limits["min"], + "limit_max": limits["max"], + "exceeded_by": self._calculate_exceeded_percentage( + position, limits["min"], limits["max"] + ), + "critical": False, # Position violations are warnings only + } + self.violations.append(violation) + # Don't set is_safe = False for position violations + + return is_safe, self.violations + + def _calculate_exceeded_percentage( + self, value: float, limit_min: float, limit_max: float + ) -> float: + """Calculate by how much percentage a value exceeds the limits.""" + if value < limit_min: + return abs((value - limit_min) / limit_min) * 100 + elif value > limit_max: + return abs((value - limit_max) / limit_max) * 100 + return 0.0 + + def get_safe_action(self, obs: Dict, original_action: Dict) -> Dict: + """Generate a safe action with startup ramping for smooth initialization. + + Args: + obs: Observation dictionary containing current joint positions + original_action: The original action that may cause violations + + Returns: + Safe action with startup ramping applied if within ramp duration + """ + safe_action = original_action.copy() + + # Handle startup ramping for arm joints + if not self.startup_complete: + if self.initial_positions is None and "q" in obs: + # Store initial positions from first observation + self.initial_positions = obs["q"].copy() + + if ( + self.startup_counter < self.ramp_duration_steps + and self.initial_positions is not None + and "q" in safe_action + ): + # Ramp factor: 0.0 at start → 1.0 at end + ramp_factor = self.startup_counter / self.ramp_duration_steps + + # Apply ramping only to monitored arm joints + for joint_name in self.velocity_limits: # Only monitored arm joints + if joint_name in self.robot_model.joint_to_dof_index: + joint_idx = self.robot_model.joint_to_dof_index[joint_name] + if joint_idx < len(safe_action["q"]) and joint_idx < len( + self.initial_positions + ): + initial_pos = self.initial_positions[joint_idx] + target_pos = original_action["q"][joint_idx] + # Linear interpolation: initial + ramp_factor * (target - initial) + safe_action["q"][joint_idx] = initial_pos + ramp_factor * ( + target_pos - initial_pos + ) + + # Increment counter for next iteration + self.startup_counter += 1 + else: + # Ramping complete - use original actions + self.startup_complete = True + + return safe_action + + def get_violation_report(self, violations: Optional[List[Dict]] = None) -> str: + """Generate a formatted error report for violations. + + Args: + violations: List of violations to report (uses self.violations if None) + + Returns: + Formatted error message string + """ + if violations is None: + violations = self.violations + + if not violations: + return "No violations detected." + + timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3] + + # Check if these are critical violations or warnings + critical_violations = [v for v in violations if v.get("critical", True)] + warning_violations = [v for v in violations if not v.get("critical", True)] + + if critical_violations and warning_violations: + report = f"Joint safety bounds exceeded!\nTimestamp: {timestamp}\nViolations:\n" + elif critical_violations: + report = f"Joint safety bounds exceeded!\nTimestamp: {timestamp}\nViolations:\n" + else: + report = f"Joint position warnings!\nTimestamp: {timestamp}\nWarnings:\n" + + for violation in violations: + joint = violation["joint"] + vtype = violation["type"] + value = violation["value"] + exceeded = violation["exceeded_by"] + limit_min = violation["limit_min"] + limit_max = violation["limit_max"] + + if vtype == "velocity": + report += f" - {joint}: {vtype}={value:.3f} rad/s " + report += f"(limit: ±{limit_max:.3f} rad/s) - " + report += f"EXCEEDED by {exceeded:.1f}%\n" + elif vtype == "position": + report += f" - {joint}: {vtype}={value:.3f} rad " + report += f"(limits: [{limit_min:.3f}, {limit_max:.3f}] rad) - " + report += f"EXCEEDED by {exceeded:.1f}%\n" + + # Add appropriate action message + if critical_violations: + report += "Action: Safe mode engaged (kp=0, tau=0). System shutdown initiated.\n" + report += "Please restart Docker container to resume operation." + else: + report += "Action: Position warning only. Robot continues operation." + + return report + + def handle_violations(self, obs: Dict, action: Dict) -> Dict: + """Check safety and handle violations appropriately. + + Args: + obs: Observation dictionary + action: Action dictionary + + Returns: + Dict with keys: + - 'safe_to_continue': bool - whether robot should continue operation + - 'action': Dict - potentially modified safe action + - 'shutdown_required': bool - whether system shutdown is needed + """ + is_safe, violations = self.check_safety(obs, action) + + # Apply startup ramping (always, regardless of violations) + safe_action = self.get_safe_action(obs, action) + + # Visualize arm and hand joint positions and velocities if enabled + if self.enable_viz: + if ( + self.right_arm_indices is not None + and self.left_arm_indices is not None + and self.right_hand_indices is not None + and self.left_hand_indices is not None + and "q" in obs + and "dq" in obs + and self.viz is not None + ): + try: + right_arm_positions = obs["q"][self.right_arm_indices] + left_arm_positions = obs["q"][self.left_arm_indices] + right_arm_velocities = obs["dq"][self.right_arm_indices] + left_arm_velocities = obs["dq"][self.left_arm_indices] + right_hand_positions = obs["q"][self.right_hand_indices] + left_hand_positions = obs["q"][self.left_hand_indices] + right_hand_velocities = obs["dq"][self.right_hand_indices] + left_hand_velocities = obs["dq"][self.left_hand_indices] + tensor_dict = { + self.right_arm_pos_key: right_arm_positions, + self.left_arm_pos_key: left_arm_positions, + self.right_arm_vel_key: right_arm_velocities, + self.left_arm_vel_key: left_arm_velocities, + self.right_hand_pos_key: right_hand_positions, + self.left_hand_pos_key: left_hand_positions, + self.right_hand_vel_key: right_hand_velocities, + self.left_hand_vel_key: left_hand_velocities, + } + self.viz.plot_tensors(tensor_dict, time.time()) + except Exception: + pass + + if not violations: + return {"safe_to_continue": True, "action": safe_action, "shutdown_required": False} + + # Separate critical (velocity) and warning (position) violations + critical_violations = [v for v in violations if v.get("critical", True)] + # warning_violations = [v for v in violations if not v.get('critical', True)] + + # Print warnings for position violations + # if warning_violations: + # warning_msg = self.get_violation_report(warning_violations) + # print(f"[SAFETY WARNING] {warning_msg}") + + # Handle critical violations (velocity) - trigger shutdown + if not is_safe and critical_violations: + error_msg = self.get_violation_report(critical_violations) + if self.env_type == "real": + print(f"[SAFETY VIOLATION] {error_msg}") + self.trigger_system_shutdown() + + return {"safe_to_continue": False, "action": safe_action, "shutdown_required": True} + + # Only position violations - continue with safe action + return {"safe_to_continue": True, "action": safe_action, "shutdown_required": False} + + def trigger_system_shutdown(self): + """Trigger system shutdown after safety violation.""" + print("\n[SAFETY] Initiating system shutdown due to safety violation...") + sys.exit(1) + + +def main(): + """Test the joint safety monitor with joint groups.""" + print("Testing joint safety monitor with joint groups...") + + try: + from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model + + # Instantiate robot model + robot_model = instantiate_g1_robot_model() + print(f"Robot model created with {len(robot_model.joint_names)} joints") + + # Create safety monitor + safety_monitor = JointSafetyMonitor(robot_model) + print("Safety monitor created successfully!") + print(f"Monitoring {len(safety_monitor.velocity_limits)} joints") + + # Print monitored joints + print("\nVelocity limits:") + for joint_name, limits in safety_monitor.velocity_limits.items(): + print(f" - {joint_name}: ±{limits['max']:.2f} rad/s") + + print(f"\nPosition limits (arms only): {len(safety_monitor.position_limits)} joints") + for joint_name, limits in safety_monitor.position_limits.items(): + print(f" - {joint_name}: [{limits['min']:.3f}, {limits['max']:.3f}] rad") + + # Test safety checking with safe values + print("\n--- Testing Safety Checking ---") + + # Create mock observation with safe values + safe_obs = { + "q": np.zeros(robot_model.num_dofs), # All joints at zero position + "dq": np.zeros(robot_model.num_dofs), # All joints at zero velocity + } + safe_action = {"q": np.zeros(robot_model.num_dofs)} + + # Test handle_violations method + result = safety_monitor.handle_violations(safe_obs, safe_action) + print( + f"Safe values test: safe_to_continue={result['safe_to_continue']}, " + f"shutdown_required={result['shutdown_required']}" + ) + + # Test with unsafe velocity + unsafe_obs = safe_obs.copy() + unsafe_obs["dq"] = np.zeros(robot_model.num_dofs) + # Set left shoulder pitch velocity to exceed limit + left_shoulder_idx = robot_model.dof_index("left_shoulder_pitch_joint") + unsafe_obs["dq"][left_shoulder_idx] = 6.0 # Exceeds 5.0 rad/s limit + + print("\nUnsafe velocity test:") + result = safety_monitor.handle_violations(unsafe_obs, safe_action) + print( + f" safe_to_continue={result['safe_to_continue']}, shutdown_required={result['shutdown_required']}" + ) + + # Test with unsafe position only + unsafe_pos_obs = safe_obs.copy() + unsafe_pos_obs["q"] = np.zeros(robot_model.num_dofs) + # Set left shoulder pitch position to exceed limit + unsafe_pos_obs["q"][left_shoulder_idx] = -4.0 # Exceeds lower limit of -3.089 + + print("\nUnsafe position test:") + result = safety_monitor.handle_violations(unsafe_pos_obs, safe_action) + print( + f" safe_to_continue={result['safe_to_continue']}, shutdown_required={result['shutdown_required']}" + ) + + print("\nAll tests completed successfully!") + + except Exception as e: + print(f"Test failed with error: {e}") + import traceback + + traceback.print_exc() + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/envs/g1/utils/state_processor.py b/gr00t_wbc/control/envs/g1/utils/state_processor.py new file mode 100644 index 0000000..f8c0548 --- /dev/null +++ b/gr00t_wbc/control/envs/g1/utils/state_processor.py @@ -0,0 +1,143 @@ +import time + +import numpy as np +from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( + MotionSwitcherClient, +) +from unitree_sdk2py.core.channel import ChannelSubscriber +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as LowState_go +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import ( + HandState_, + IMUState_, + LowState_ as LowState_hg, + OdoState_, +) + + +class BodyStateProcessor: + def __init__(self, config): + self.config = config + + # Enter debug mode for real robot + if self.config["ENV_TYPE"] == "real": + msc = MotionSwitcherClient() + msc.SetTimeout(5.0) + msc.Init() + + status, result = msc.CheckMode() + print(status, result) + while result["name"]: + msc.ReleaseMode() + status, result = msc.CheckMode() + print(status, result) + time.sleep(1) + + if self.config["ROBOT_TYPE"] == "h1" or self.config["ROBOT_TYPE"] == "go2": + self.robot_lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_go) + self.robot_lowstate_subscriber.Init(None, 0) + self.robot_lowstate_subscriber.Init(None, 0) + elif ( + self.config["ROBOT_TYPE"] == "g1_29dof" + or self.config["ROBOT_TYPE"] == "h1-2_27dof" + or self.config["ROBOT_TYPE"] == "h1-2_21dof" + ): + self.robot_lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_hg) + self.robot_lowstate_subscriber.Init(None, 0) + + self.secondary_imu_subscriber = ChannelSubscriber("rt/secondary_imu", IMUState_) + self.secondary_imu_subscriber.Init(None, 0) + + # Subscribe to odo state (only available in simulation) + if self.config["ENV_TYPE"] == "sim": + self.odo_state_subscriber = ChannelSubscriber("rt/odostate", OdoState_) + self.odo_state_subscriber.Init(None, 0) + else: + raise NotImplementedError(f"Robot type {self.config['ROBOT_TYPE']} is not supported") + + self.num_dof = self.config["NUM_JOINTS"] + # 3 + 4 + 19 + self._init_q = np.zeros(3 + 4 + self.num_dof) + self.q = self._init_q + self.dq = np.zeros(3 + 3 + self.num_dof) + self.ddq = np.zeros(3 + 3 + self.num_dof) + self.tau_est = np.zeros(3 + 3 + self.num_dof) + self.torso_quat = np.zeros(4) + self.torso_ang_vel = np.zeros(3) + self.temp_first = np.zeros(self.num_dof) + self.temp_second = np.zeros(self.num_dof) + self.robot_low_state = None + self.secondary_imu_state = None + self.odo_state = None + + def _prepare_low_state(self) -> np.ndarray: + self.robot_low_state = self.robot_lowstate_subscriber.Read() + self.secondary_imu_state = self.secondary_imu_subscriber.Read() + + if not self.robot_low_state: + print("No low state received") + return + imu_state = self.robot_low_state.imu_state + + # Use odo_state for position and velocity if available, otherwise set to zero + if self.config["ENV_TYPE"] == "sim": + self.odo_state = self.odo_state_subscriber.Read() + self.q[0:3] = self.odo_state.position + self.dq[0:3] = self.odo_state.linear_velocity + else: + self.q[0:3] = [0.0, 0.0, 0.0] + self.dq[0:3] = [0.0, 0.0, 0.0] + + self.q[3:7] = imu_state.quaternion # w, x, y, z + self.dq[3:6] = imu_state.gyroscope + self.ddq[0:3] = imu_state.accelerometer + unitree_joint_state = self.robot_low_state.motor_state + self.torso_quat = self.secondary_imu_state.quaternion + self.torso_ang_vel = self.secondary_imu_state.gyroscope + + for i in range(self.num_dof): + self.q[7 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].q + self.dq[6 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].dq + self.tau_est[6 + i] = unitree_joint_state[self.config["JOINT2MOTOR"][i]].tau_est + + robot_state_data = np.concatenate( + [self.q, self.dq, self.tau_est, self.ddq, self.torso_quat, self.torso_ang_vel], axis=0 + ).reshape(1, -1) + # (7 + 29) + (6 + 29) + (6 + 29) + (6 + 29) = 141 dim + + return robot_state_data + + +class HandStateProcessor: + def __init__(self, is_left: bool = True): + self.is_left = is_left + if self.is_left: + self.state_sub = ChannelSubscriber("rt/dex3/left/state", HandState_) + else: + self.state_sub = ChannelSubscriber("rt/dex3/right/state", HandState_) + + self.state_sub.Init(None, 0) + self.state_sub.Init(None, 0) + self.state = None + self.num_dof = 7 # for single hand + + def _prepare_low_state(self) -> np.ndarray: + self.state = self.state_sub.Read() + + if not self.state: + print("No state received") + return + + state_data = ( + np.concatenate( + [ + [self.state.motor_state[i].q for i in range(self.num_dof)], + [self.state.motor_state[i].dq for i in range(self.num_dof)], + [self.state.motor_state[i].tau_est for i in range(self.num_dof)], + [self.state.motor_state[i].ddq for i in range(self.num_dof)], + ], + axis=0, + ) + .astype(np.float64) + .reshape(1, -1) + ) + return state_data diff --git a/gr00t_wbc/control/envs/robocasa/__init__.py b/gr00t_wbc/control/envs/robocasa/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/robocasa/async_env_server.py b/gr00t_wbc/control/envs/robocasa/async_env_server.py new file mode 100644 index 0000000..b2bd746 --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/async_env_server.py @@ -0,0 +1,303 @@ +from abc import abstractmethod +import threading +import time +from typing import Any, Dict, Tuple + +import mujoco +import numpy as np +import rclpy + +from gr00t_wbc.control.envs.g1.sim.image_publish_utils import ImagePublishProcess +from gr00t_wbc.control.envs.robocasa.utils.robocasa_env import Gr00tLocomanipRoboCasaEnv # noqa: F401 +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber + + +class RoboCasaEnvServer: + """ + This class is responsible for running the simulation environment loop in a separate thread. + It communicates with the main thread via the `publish_obs` and `get_action` methods through `channel_bridge`. + It will also handle the viewer sync when `onscreen` is True. + """ + + def __init__( + self, + env_name: str, + robot_name: str, + robot_model: RobotModel, + env_kwargs: Dict[str, Any], + **kwargs, + ): + # initialize environment + if env_kwargs.get("onscreen", False): + env_kwargs["onscreen"] = False + self.onscreen = True # onscreen render in the main thread + self.render_camera = env_kwargs.get("render_camera", None) + else: + self.onscreen = False + self.env_name = env_name + self.env = Gr00tLocomanipRoboCasaEnv(env_name, robot_name, robot_model, **env_kwargs) + self.init_caches() + self.cache_lock = threading.Lock() + + # initialize channel + self.init_channel() + + # initialize ROS2 node + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node("sim_robocasa") + self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) + self.thread.start() + else: + self.thread = None + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] # will only take the first node + + self.control_freq = env_kwargs.get("control_freq", 1 / 0.02) + self.sim_freq = kwargs.get("sim_freq", 1 / 0.005) + self.control_rate = self.node.create_rate(self.control_freq) + + self.running = False + self.sim_thread = None + self.sync_lock = threading.Lock() + + self.sync_mode = kwargs.get("sync_mode", False) + self.steps_per_action = kwargs.get("steps_per_action", 1) + + self.image_dt = kwargs.get("image_dt", 0.04) + self.image_publish_process = None + self.viewer_freq = kwargs.get("viewer_freq", 1 / 0.02) + self.viewer = None + + self.verbose = kwargs.get("verbose", True) + + # Initialize keyboard listener for env reset + self.keyboard_listener = KeyboardListenerSubscriber() + + self.reset() + + @property + def base_env(self): + return self.env.env + + def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555): + """Initialize image publishing subprocess if cameras are configured""" + if len(self.env.camera_names) == 0: + print( + "Warning: No camera configs provided, image publishing subprocess will not be started" + ) + return + + # Build camera configs from env camera settings + camera_configs = {} + for env_cam_name in self.env.camera_names: + camera_config = self.env.camera_key_mapper.get_camera_config(env_cam_name) + mapped_cam_name, cam_width, cam_height = camera_config + camera_configs[mapped_cam_name] = {"height": cam_height, "width": cam_width} + + self.image_publish_process = ImagePublishProcess( + camera_configs=camera_configs, + image_dt=self.image_dt, + zmq_port=camera_port, + start_method=start_method, + verbose=self.verbose, + ) + + self.image_publish_process.start_process() + + def update_render_caches(self, obs: Dict[str, Any]): + """Update render cache and shared memory for subprocess""" + if self.image_publish_process is None: + return + + # Extract image observations from obs dict + render_caches = { + k: v for k, v in obs.items() if k.endswith("_image") and isinstance(v, np.ndarray) + } + + # Update shared memory if image publishing process is available + if render_caches: + self.image_publish_process.update_shared_memory(render_caches) + + def init_caches(self): + self.caches = { + "obs": None, + "reward": None, + "terminated": None, + "truncated": None, + "info": None, + } + + def reset(self, **kwargs): + if self.viewer is not None: + self.viewer.close() + + obs, info = self.env.reset(**kwargs) + self.caches["obs"] = obs + self.caches["reward"] = 0 + self.caches["terminated"] = False + self.caches["truncated"] = False + self.caches["info"] = info + + # initialize viewer + if self.onscreen: + self.viewer = mujoco.viewer.launch_passive( + self.base_env.sim.model._model, + self.base_env.sim.data._data, + show_left_ui=False, + show_right_ui=False, + ) + self.viewer.opt.geomgroup[0] = 0 # disable collision visualization + if self.render_camera is not None: + self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED + self.viewer.cam.fixedcamid = self.base_env.sim.model._model.cam( + self.render_camera + ).id + + # self.episode_state.reset_state() + return obs, info + + @abstractmethod + def init_channel(self): + raise NotImplementedError("init_channel must be implemented by the subclass") + + @abstractmethod + def publish_obs(self): + raise NotImplementedError("publish_obs must be implemented by the subclass") + + @abstractmethod + def get_action(self) -> Tuple[Dict[str, Any], bool, bool]: + raise NotImplementedError("get_action must be implemented by the subclass") + + def start_as_thread(self): + """Start the simulation thread""" + if self.sim_thread is not None and self.sim_thread.is_alive(): + return + + self.sim_thread = threading.Thread(target=self.start) + self.sim_thread.daemon = True + self.sim_thread.start() + + def set_sync_mode(self, sync_mode: bool, steps_per_action: int = 4): + """Set the sync mode of the environment server""" + with self.sync_lock: + self.sync_mode = sync_mode + self.steps_per_action = steps_per_action + + def _check_keyboard_input(self): + """Check for keyboard input and handle state transitions""" + key = self.keyboard_listener.read_msg() + if key == "k": + print("\033[1;32m[Sim env]\033[0m Resetting sim environment") + self.reset() + + def start(self): + """Function executed by the simulation thread""" + iter_idx = 0 + steps_per_cur_action = 0 + t_start = time.monotonic() + + self.running = True + + while self.running: + # Check keyboard input for state transitions + self._check_keyboard_input() + + # Publish observations and get new action + self.publish_obs() + action, ready, is_new_action = self.get_action() + # ready is True if the action is received from the control loop + # is_new_action is True if the action is new (not the same as the previous action) + with self.sync_lock: + sync_mode = self.sync_mode + max_steps_per_action = self.steps_per_action + + # Process action if ready and within step limits + action_should_apply = ready and ( + (not sync_mode) or steps_per_cur_action < max_steps_per_action + ) + if action_should_apply: + obs, reward, terminated, truncated, info = self.env.step(action) + with self.cache_lock: + self.caches["obs"] = obs + self.caches["reward"] = reward + self.caches["terminated"] = terminated + self.caches["truncated"] = truncated + self.caches["info"] = info + + if reward == 1.0 and iter_idx % 50 == 0: + print("\033[92mTask successful. Can save data now.\033[0m") + + iter_idx += 1 + steps_per_cur_action += 1 + if self.verbose and sync_mode: + print("steps_per_cur_action: ", steps_per_cur_action) + + # Update render caches at image publishing rate + if action_should_apply and iter_idx % int(self.image_dt * self.control_freq) == 0: + with self.cache_lock: + obs_copy = self.caches["obs"].copy() + self.update_render_caches(obs_copy) + + # Reset step counter for new actions + if is_new_action: + steps_per_cur_action = 0 + + # Update viewer at specified frequency + if self.onscreen and iter_idx % (self.control_freq / self.viewer_freq) == 0: + self.viewer.sync() + + # Check if we're meeting the desired control frequency + if iter_idx % 100 == 0: + end_time = time.monotonic() + if self.verbose: + print( + f"sim FPS: {100.0 / (end_time - t_start) * (self.sim_freq / self.control_freq)}" + ) + if (end_time - t_start) > ((110.0 / self.control_freq)): # for tolerance + print( + f"Warning: Sim runs at " + "{100.0/(end_time - t_start) * (self.sim_freq / self.control_freq):.1f}Hz, " + f"but should run at {self.sim_freq:.1f}Hz" + ) + t_start = end_time + + # reset obj pos every 200 steps + if iter_idx % 200 == 0: + if hasattr(self.base_env, "reset_obj_pos"): + self.base_env.reset_obj_pos() + + self.control_rate.sleep() + + def get_privileged_obs(self): + """Get privileged observation. Should be implemented by subclasses.""" + obs = {} + with self.cache_lock: + if hasattr(self.base_env, "get_privileged_obs_keys"): + for key in self.base_env.get_privileged_obs_keys(): + obs[key] = self.caches["obs"][key] + + for key in self.caches["obs"].keys(): + if key.endswith("_image"): + obs[key] = self.caches["obs"][key] + + return obs + + def stop(self): + """Stop the simulation thread""" + self.running = False + if self.sim_thread is not None: + self.sim_thread.join(timeout=1.0) # Wait for thread to finish with timeout + self.sim_thread = None + + def close(self): + self.stop() + if self.image_publish_process is not None: + self.image_publish_process.stop() + if self.onscreen: + self.viewer.close() + self.env.close() + + def get_reward(self): + return self.base_env.reward() diff --git a/gr00t_wbc/control/envs/robocasa/sync_env.py b/gr00t_wbc/control/envs/robocasa/sync_env.py new file mode 100644 index 0000000..bdfe7a2 --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/sync_env.py @@ -0,0 +1,584 @@ +import sys +from typing import Any, Dict, Tuple + +import gymnasium as gym +from gymnasium.envs.registration import register +import numpy as np +from robocasa.environments.locomanipulation import REGISTERED_LOCOMANIPULATION_ENVS +from robocasa.models.robots import GR00T_LOCOMANIP_ENVS_ROBOTS +from robosuite.environments.robot_env import RobotEnv +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.envs.g1.utils.joint_safety import JointSafetyMonitor +from gr00t_wbc.control.envs.robocasa.utils.controller_utils import update_robosuite_controller_configs +from gr00t_wbc.control.envs.robocasa.utils.robocasa_env import ( # noqa: F401 + ALLOWED_LANGUAGE_CHARSET, + Gr00tLocomanipRoboCasaEnv, +) +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model +from gr00t_wbc.control.utils.n1_utils import ( + prepare_gym_space_for_eval, + prepare_observation_for_eval, +) +from gr00t_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + + +class SyncEnv(gym.Env): + MAX_MUJOCO_STATE_LEN = 800 + + def __init__(self, env_name, robot_name, **kwargs): + self.env_name = env_name + self.robot_name = robot_name + self.onscreen = kwargs.get("onscreen", True) + self.enable_gravity_compensation = kwargs.pop("enable_gravity_compensation", False) + self.gravity_compensation_joints = kwargs.pop("gravity_compensation_joints", ["arms"]) + _, self.robot_model = get_robot_type_and_model( + robot_name, enable_waist_ik=kwargs.pop("enable_waist", False) + ) + + env_kwargs = { + "onscreen": kwargs.get("onscreen", True), + "offscreen": kwargs.get("offscreen", False), + "renderer": kwargs.get("renderer", "mjviewer"), + "render_camera": kwargs.get("render_camera", "frontview"), + "camera_names": kwargs.get("camera_names", ["frontview"]), + "camera_heights": kwargs.get("camera_heights", None), + "camera_widths": kwargs.get("camera_widths", None), + "controller_configs": kwargs["controller_configs"], + "control_freq": kwargs.get("control_freq", 50), + "translucent_robot": kwargs.get("translucent_robot", True), + "ik_indicator": kwargs.get("ik_indicator", False), + "randomize_cameras": kwargs.get("randomize_cameras", True), + } + self.env = Gr00tLocomanipRoboCasaEnv( + env_name, robot_name, robot_model=self.robot_model, **env_kwargs + ) + self.init_cache() + + self.reset() + + @property + def base_env(self) -> RobotEnv: + return self.env.env + + def overwrite_floating_base_action(self, navigate_cmd): + if self.base_env.robots[0].robot_model.default_base == "FloatingLeggedBase": + self.env.unwrapped.overridden_floating_base_action = navigate_cmd + + def get_mujoco_state_info(self): + mujoco_state = self.base_env.sim.get_state().flatten() + assert len(mujoco_state) < SyncEnv.MAX_MUJOCO_STATE_LEN + padding_width = SyncEnv.MAX_MUJOCO_STATE_LEN - len(mujoco_state) + padded_mujoco_state = np.pad( + mujoco_state, (0, padding_width), mode="constant", constant_values=0 + ) + max_mujoco_state_len = SyncEnv.MAX_MUJOCO_STATE_LEN + mujoco_state_len = len(mujoco_state) + mujoco_state = padded_mujoco_state.copy() + return max_mujoco_state_len, mujoco_state_len, mujoco_state + + def reset_to(self, state: Dict[str, Any]) -> Dict[str, Any] | None: + if hasattr(self.base_env, "reset_to"): + self.base_env.reset_to(state) + else: + # todo: maybe update robosuite to have reset_to() + env = self.base_env + if "model_file" in state: + xml = env.edit_model_xml(state["model_file"]) + env.reset_from_xml_string(xml) + env.sim.reset() + if "states" in state: + env.sim.set_state_from_flattened(state["states"]) + env.sim.forward() + + obs = self.env.force_update_observation(timestep=0) + self.cache["obs"] = obs + return + + def get_state(self) -> Dict[str, Any]: + return self.base_env.get_state() + + def is_success(self): + """ + Check if the task condition(s) is reached. Should return a dictionary + { str: bool } with at least a "task" key for the overall task success, + and additional optional keys corresponding to other task criteria. + """ + # First, try to use the base environment's is_success method if it exists + if hasattr(self.base_env, "is_success"): + return self.base_env.is_success() + + # Fall back to using _check_success if available + elif hasattr(self.base_env, "_check_success"): + succ = self.base_env._check_success() + if isinstance(succ, dict): + assert "task" in succ + return succ + return {"task": succ} + + # If neither method exists, return failure + else: + return {"task": False} + + def init_cache(self): + self.cache = { + "obs": None, + "reward": None, + "terminated": None, + "truncated": None, + "info": None, + } + + def reset(self, seed=None, options=None) -> Tuple[Dict[str, any], Dict[str, any]]: + self.init_cache() + obs, info = self.env.reset(seed=seed, options=options) + self.cache["obs"] = obs + self.cache["reward"] = 0 + self.cache["terminated"] = False + self.cache["truncated"] = False + self.cache["info"] = info + return self.observe(), info + + def observe(self) -> Dict[str, any]: + # Get observations from body and hands + assert ( + self.cache["obs"] is not None + ), "Observation cache is not initialized, please reset the environment first" + raw_obs = self.cache["obs"] + + # Body and hand joint measurements come in actuator order, so we need to convert them to joint order + whole_q = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=raw_obs["body_q"], + left_hand_actuated_joint_values=raw_obs["left_hand_q"], + right_hand_actuated_joint_values=raw_obs["right_hand_q"], + ) + whole_dq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=raw_obs["body_dq"], + left_hand_actuated_joint_values=raw_obs["left_hand_dq"], + right_hand_actuated_joint_values=raw_obs["right_hand_dq"], + ) + whole_ddq = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=raw_obs["body_ddq"], + left_hand_actuated_joint_values=raw_obs["left_hand_ddq"], + right_hand_actuated_joint_values=raw_obs["right_hand_ddq"], + ) + whole_tau_est = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=raw_obs["body_tau_est"], + left_hand_actuated_joint_values=raw_obs["left_hand_tau_est"], + right_hand_actuated_joint_values=raw_obs["right_hand_tau_est"], + ) + eef_obs = self.get_eef_obs(whole_q) + + obs = { + "q": whole_q, + "dq": whole_dq, + "ddq": whole_ddq, + "tau_est": whole_tau_est, + "floating_base_pose": raw_obs["floating_base_pose"], + "floating_base_vel": raw_obs["floating_base_vel"], + "floating_base_acc": raw_obs["floating_base_acc"], + "wrist_pose": np.concatenate([eef_obs["left_wrist_pose"], eef_obs["right_wrist_pose"]]), + } + + # Add state keys for model input + obs = prepare_observation_for_eval(self.robot_model, obs) + + obs["language.language_instruction"] = raw_obs["language.language_instruction"] + + if hasattr(self.base_env, "get_privileged_obs_keys"): + for key in self.base_env.get_privileged_obs_keys(): + obs[key] = raw_obs[key] + + for key in raw_obs.keys(): + if key.endswith("_image"): + obs[key] = raw_obs[key] + # TODO: add video.key without _image suffix for evaluation, convert to uint8, remove later + obs[f"video.{key.replace('_image', '')}"] = raw_obs[key] + return obs + + def step( + self, action: Dict[str, any] + ) -> Tuple[Dict[str, any], float, bool, bool, Dict[str, any]]: + self.queue_action(action) + return self.get_step_info() + + def get_observation(self): + return self.base_env._get_observations() # assumes base env is robosuite + + def get_step_info(self) -> Dict[str, any]: + return ( + self.observe(), + self.cache["reward"], + self.cache["terminated"], + self.cache["truncated"], + self.cache["info"], + ) + + def convert_q_to_actuated_joint_order(self, q: np.ndarray) -> np.ndarray: + body_q = self.robot_model.get_body_actuated_joints(q) + left_hand_q = self.robot_model.get_hand_actuated_joints(q, side="left") + right_hand_q = self.robot_model.get_hand_actuated_joints(q, side="right") + whole_q = np.zeros_like(q) + whole_q[self.robot_model.get_joint_group_indices("body")] = body_q + whole_q[self.robot_model.get_joint_group_indices("left_hand")] = left_hand_q + whole_q[self.robot_model.get_joint_group_indices("right_hand")] = right_hand_q + + return whole_q + + def set_ik_indicator(self, teleop_cmd): + """Set the IK indicators for the simulator""" + if "left_wrist" in teleop_cmd and "right_wrist" in teleop_cmd: + left_wrist_input_pose = teleop_cmd["left_wrist"] + right_wrist_input_pose = teleop_cmd["right_wrist"] + ik_wrapper = self.base_env + ik_wrapper.set_target_poses_outside_env([left_wrist_input_pose, right_wrist_input_pose]) + + def render(self): + if self.base_env.viewer is not None: + self.base_env.viewer.update() + if self.onscreen: + self.base_env.render() + + def queue_action(self, action: Dict[str, any]): + # action is in pinocchio joint order, we need to convert it to actuator order + action_q = self.convert_q_to_actuated_joint_order(action["q"]) + + # Compute gravity compensation torques if enabled + tau_q = np.zeros_like(action_q) + if self.enable_gravity_compensation and self.robot_model is not None: + try: + # Get current robot configuration from cache (more efficient than observe()) + raw_obs = self.cache["obs"] + + # Convert from actuated joint order to joint order for Pinocchio + current_q_joint_order = self.robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=raw_obs["body_q"], + left_hand_actuated_joint_values=raw_obs["left_hand_q"], + right_hand_actuated_joint_values=raw_obs["right_hand_q"], + ) + + # Compute gravity compensation in joint order using current robot configuration + gravity_torques_joint_order = self.robot_model.compute_gravity_compensation_torques( + current_q_joint_order, joint_groups=self.gravity_compensation_joints + ) + + # Convert gravity torques to actuated joint order + gravity_torques_actuated = self.convert_q_to_actuated_joint_order( + gravity_torques_joint_order + ) + + # Add gravity compensation to torques + tau_q += gravity_torques_actuated + + except Exception as e: + print(f"Error applying gravity compensation in sync_env: {e}") + + obs, reward, terminated, truncated, info = self.env.step({"q": action_q, "tau": tau_q}) + self.cache["obs"] = obs + self.cache["reward"] = reward + self.cache["terminated"] = terminated + self.cache["truncated"] = truncated + self.cache["info"] = info + + def queue_state(self, state: Dict[str, any]): + # This function is for debugging or cross-playback between sim and real only. + state_q = self.convert_q_to_actuated_joint_order(state["q"]) + obs, reward, terminated, truncated, info = self.env.unwrapped.step_only_kinematics( + {"q": state_q} + ) + self.cache["obs"] = obs + self.cache["reward"] = reward + self.cache["terminated"] = terminated + self.cache["truncated"] = truncated + self.cache["info"] = info + + @property + def observation_space(self) -> gym.Space: + # @todo: check if the low and high bounds are correct for body_obs. + q_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + dq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + ddq_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + tau_est_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.robot_model.num_dofs,)) + floating_base_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7,)) + floating_base_vel_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)) + floating_base_acc_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(6,)) + wrist_pose_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(7 + 7,)) + + obs_space = gym.spaces.Dict( + { + "floating_base_pose": floating_base_pose_space, + "floating_base_vel": floating_base_vel_space, + "floating_base_acc": floating_base_acc_space, + "q": q_space, + "dq": dq_space, + "ddq": ddq_space, + "tau_est": tau_est_space, + "wrist_pose": wrist_pose_space, + } + ) + + obs_space = prepare_gym_space_for_eval(self.robot_model, obs_space) + + obs_space["language.language_instruction"] = gym.spaces.Text( + max_length=256, charset=ALLOWED_LANGUAGE_CHARSET + ) + + if hasattr(self.base_env, "get_privileged_obs_keys"): + for key, shape in self.base_env.get_privileged_obs_keys().items(): + space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=shape) + obs_space[key] = space + + robocasa_obs_space = self.env.observation_space + for key in robocasa_obs_space.keys(): + if key.endswith("_image"): + space = gym.spaces.Box( + low=-np.inf, high=np.inf, shape=robocasa_obs_space[key].shape + ) + obs_space[key] = space + # TODO: add video.key without _image suffix for evaluation, remove later + space_uint = gym.spaces.Box(low=0, high=255, shape=space.shape, dtype=np.uint8) + obs_space[f"video.{key.replace('_image', '')}"] = space_uint + + return obs_space + + def reset_obj_pos(self): + # For Tairan's goal-reaching task, a hacky way to reset the object position is needed. + if hasattr(self.base_env, "reset_obj_pos"): + self.base_env.reset_obj_pos() + + @property + def action_space(self) -> gym.Space: + return self.env.action_space + + def close(self): + self.env.close() + + def __repr__(self): + return ( + f"SyncEnv(env_name={self.env_name}, \n" + f" observation_space={self.observation_space}, \n" + f" action_space={self.action_space})" + ) + + def get_joint_gains(self): + controller = self.base_env.robots[0].composite_controller + + gains = {} + key_mapping = { + "left": "left_arm", + "right": "right_arm", + "legs": "legs", + "torso": "waist", + "head": "neck", + } + for k in controller.part_controllers.keys(): + if hasattr(controller.part_controllers[k], "kp"): + if k in key_mapping: + gains[key_mapping[k]] = controller.part_controllers[k].kp + else: + gains[k] = controller.part_controllers[k].kp + gains.update( + { + "left_hand": self.base_env.sim.model.actuator_gainprm[ + self.base_env.robots[0]._ref_actuators_indexes_dict["left_gripper"], 0 + ], + "right_hand": self.base_env.sim.model.actuator_gainprm[ + self.base_env.robots[0]._ref_actuators_indexes_dict["right_gripper"], 0 + ], + } + ) + joint_gains = np.zeros(self.robot_model.num_dofs) + for k in gains.keys(): + joint_gains[self.robot_model.get_joint_group_indices(k)] = gains[k] + return joint_gains + + def get_joint_damping(self): + controller = self.base_env.robots[0].composite_controller + damping = {} + key_mapping = { + "left": "left_arm", + "right": "right_arm", + "legs": "legs", + "torso": "waist", + "head": "neck", + } + for k in controller.part_controllers.keys(): + if hasattr(controller.part_controllers[k], "kd"): + if k in key_mapping: + damping[key_mapping[k]] = controller.part_controllers[k].kd + else: + damping[k] = controller.part_controllers[k].kd + damping.update( + { + "left_hand": -self.base_env.sim.model.actuator_biasprm[ + self.base_env.robots[0]._ref_actuators_indexes_dict["left_gripper"], 2 + ], + "right_hand": -self.base_env.sim.model.actuator_biasprm[ + self.base_env.robots[0]._ref_actuators_indexes_dict["right_gripper"], 2 + ], + } + ) + joint_damping = np.zeros(self.robot_model.num_dofs) + for k in damping.keys(): + joint_damping[self.robot_model.get_joint_group_indices(k)] = damping[k] + return joint_damping + + def get_eef_obs(self, q: np.ndarray) -> Dict[str, np.ndarray]: + self.robot_model.cache_forward_kinematics(q) + eef_obs = {} + for side in ["left", "right"]: + wrist_placement = self.robot_model.frame_placement( + self.robot_model.supplemental_info.hand_frame_names[side] + ) + wrist_pos, wrist_quat = wrist_placement.translation[:3], R.from_matrix( + wrist_placement.rotation + ).as_quat(scalar_first=True) + eef_obs[f"{side}_wrist_pose"] = np.concatenate([wrist_pos, wrist_quat]) + + return eef_obs + + +class G1SyncEnv(SyncEnv): + def __init__( + self, + env_name, + robot_name, + **kwargs, + ): + renderer = kwargs.get("renderer", "mjviewer") + if renderer == "mjviewer": + default_render_camera = ["robot0_oak_egoview"] + elif renderer in ["mujoco", "rerun"]: + default_render_camera = [ + "robot0_oak_egoview", + "robot0_oak_left_monoview", + "robot0_oak_right_monoview", + ] + else: + raise NotImplementedError + default_camera_names = [ + "robot0_oak_egoview", + "robot0_oak_left_monoview", + "robot0_oak_right_monoview", + ] + default_camera_heights = [ + RS_VIEW_CAMERA_HEIGHT, + RS_VIEW_CAMERA_HEIGHT, + RS_VIEW_CAMERA_HEIGHT, + ] + default_camera_widths = [ + RS_VIEW_CAMERA_WIDTH, + RS_VIEW_CAMERA_WIDTH, + RS_VIEW_CAMERA_WIDTH, + ] + + kwargs.update( + { + "onscreen": kwargs.get("onscreen", True), + "offscreen": kwargs.get("offscreen", False), + "render_camera": kwargs.get("render_camera", default_render_camera), + "camera_names": kwargs.get("camera_names", default_camera_names), + "camera_heights": kwargs.get("camera_heights", default_camera_heights), + "camera_widths": kwargs.get("camera_widths", default_camera_widths), + "translucent_robot": kwargs.get("translucent_robot", False), + } + ) + super().__init__(env_name=env_name, robot_name=robot_name, **kwargs) + + # Initialize safety monitor (visualization disabled) - G1 specific + self.safety_monitor = JointSafetyMonitor( + self.robot_model, + enable_viz=False, + env_type="sim", # G1SyncEnv is always simulation + ) + self.safety_monitor.ramp_duration_steps = 0 + self.safety_monitor.startup_complete = True + self.safety_monitor.LOWER_BODY_VELOCITY_LIMIT = ( + 1e10 # disable lower body velocity limits since it impacts WBC + ) + self.last_safety_ok = True # Track last safety status from queue_action + + @property + def observation_space(self): + obs_space = super().observation_space + obs_space["torso_quat"] = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(4,)) + obs_space["torso_ang_vel"] = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(3,)) + return obs_space + + def observe(self): + obs = super().observe() + obs["torso_quat"] = self.cache["obs"]["secondary_imu_quat"] + obs["torso_ang_vel"] = self.cache["obs"]["secondary_imu_vel"][3:6] + return obs + + def queue_action(self, action: Dict[str, any]): + # Safety check before queuing action + obs = self.observe() + safety_result = self.safety_monitor.handle_violations(obs, action) + action = safety_result["action"] + # Save safety status for efficient access + self.last_safety_ok = not safety_result.get("shutdown_required", False) + # Check if shutdown is required + if safety_result["shutdown_required"]: + self.safety_monitor.trigger_system_shutdown() + + # Call parent queue_action with potentially modified action + super().queue_action(action) + + def get_joint_safety_status(self) -> bool: + """Get current joint safety status from the last queue_action safety check. + + Returns: + bool: True if joints are safe (no shutdown required), False if unsafe + """ + return self.last_safety_ok + + +def create_gym_sync_env_class(env, robot, robot_alias, wbc_version): + class_name = f"{env}_{robot}_{wbc_version}" + id_name = f"gr00tlocomanip_{robot_alias}/{class_name}" + + if robot_alias.startswith("g1"): + env_class_type = G1SyncEnv + elif robot_alias.startswith("gr1"): + env_class_type = globals().get("GR1SyncEnv", SyncEnv) + else: + env_class_type = SyncEnv + + controller_configs = update_robosuite_controller_configs( + robot=robot, + wbc_version=wbc_version, + ) + + env_class_type = type( + class_name, + (env_class_type,), + { + "__init__": lambda self, **kwargs: super(self.__class__, self).__init__( + env_name=env, + robot_name=robot, + controller_configs=controller_configs, + **kwargs, + ) + }, + ) + + current_module = sys.modules["gr00t_wbc.control.envs.robocasa.sync_env"] + setattr(current_module, class_name, env_class_type) + register( + id=id_name, # Unique ID for the environment + entry_point=f"gr00t_wbc.control.envs.robocasa.sync_env:{class_name}", + ) + + +WBC_VERSION = "gear_wbc" + +for ENV in REGISTERED_LOCOMANIPULATION_ENVS: + for ROBOT, ROBOT_ALIAS in GR00T_LOCOMANIP_ENVS_ROBOTS.items(): + create_gym_sync_env_class(ENV, ROBOT, ROBOT_ALIAS, WBC_VERSION) + + +if __name__ == "__main__": + + env = gym.make("gr00tlocomanip_g1_sim/PnPBottle_g1_gear_wbc") + print(env.observation_space) diff --git a/gr00t_wbc/control/envs/robocasa/utils/__init__.py b/gr00t_wbc/control/envs/robocasa/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/envs/robocasa/utils/cam_key_converter.py b/gr00t_wbc/control/envs/robocasa/utils/cam_key_converter.py new file mode 100644 index 0000000..1c1aeaa --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/utils/cam_key_converter.py @@ -0,0 +1,73 @@ +from dataclasses import dataclass +from typing import Dict, Optional, Tuple + +from gr00t_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + + +@dataclass +class CameraConfig: + width: int + height: int + mapped_key: str + + +class CameraKeyMapper: + def __init__(self): + # Default camera dimensions + self.default_width = RS_VIEW_CAMERA_WIDTH + self.default_height = RS_VIEW_CAMERA_HEIGHT + + # Camera key mapping with custom dimensions + self.camera_configs: Dict[str, CameraConfig] = { + # GR1 + "egoview": CameraConfig(self.default_width, self.default_height, "ego_view"), + "frontview": CameraConfig(self.default_width, self.default_height, "front_view"), + # G1 + "robot0_rs_egoview": CameraConfig(self.default_width, self.default_height, "ego_view"), + "robot0_rs_tppview": CameraConfig(self.default_width, self.default_height, "tpp_view"), + "robot0_oak_egoview": CameraConfig(self.default_width, self.default_height, "ego_view"), + "robot0_oak_left_monoview": CameraConfig( + self.default_width, self.default_height, "ego_view_left_mono" + ), + "robot0_oak_right_monoview": CameraConfig( + self.default_width, self.default_height, "ego_view_right_mono" + ), + } + + def get_camera_config(self, key: str) -> Optional[Tuple[str, int, int]]: + """ + Get the mapped camera key and dimensions for a given camera key. + + Args: + key: The input camera key + + Returns: + Tuple of (mapped_key, width, height) if key exists, None otherwise + """ + config = self.camera_configs.get(key.lower()) + if config is None: + return None + return config.mapped_key, config.width, config.height + + def add_camera_config( + self, key: str, mapped_key: str, width: int = 256, height: int = 256 + ) -> None: + """ + Add a new camera configuration or update an existing one. + + Args: + key: The camera key to add/update + mapped_key: The actual camera key to map to + width: Camera width in pixels + height: Camera height in pixels + """ + self.camera_configs[key.lower()] = CameraConfig(width, height, mapped_key) + + def get_all_camera_keys(self) -> list: + """ + Get all available camera keys. + + Returns: + List of all camera keys + """ + return list(self.camera_configs.keys()) diff --git a/gr00t_wbc/control/envs/robocasa/utils/controller_utils.py b/gr00t_wbc/control/envs/robocasa/utils/controller_utils.py new file mode 100644 index 0000000..a8627b6 --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/utils/controller_utils.py @@ -0,0 +1,54 @@ +def get_body_ik_solver_settings_type(robot: str): + robot2body_ik_solver_settings_type = { + "G1FixedLowerBody": "sim_optimized", + "G1FixedBase": "sim_optimized", + "G1FloatingBody": "sim_optimized", + "G1ArmsOnly": "sim_optimized", + "G1ArmsOnlyFloating": "sim_optimized", + "G1": "default", + "GR1ArmsAndWaistFourierHands": "default", + "GR1FixedLowerBody": "default", + "GR1ArmsOnlyFourierHands": "default", + "GR1ArmsOnly": "default", + } + return robot2body_ik_solver_settings_type[robot] + + +def update_robosuite_controller_configs( + robot: str, + wbc_version: str = None, + enable_gravity_compensation: bool = False, +): + """ + Update the robosuite controller configs based on the robot type and wbc version. + """ + body_ik_solver_settings_type = get_body_ik_solver_settings_type(robot) + if robot.startswith("G1"): + if wbc_version == "gear_wbc": + if enable_gravity_compensation: + robosuite_controller_configs = ( + "robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc_gc.json" + ) + else: + robosuite_controller_configs = ( + "robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc.json" + ) + else: + if body_ik_solver_settings_type == "default": + robosuite_controller_configs = ( + "robocasa/examples/third_party_controller/default_mink_ik_g1_wbc.json" + ) + elif body_ik_solver_settings_type == "sim_optimized": + robosuite_controller_configs = ( + "robocasa/examples/third_party_controller/" + "default_mink_ik_g1_wbc_sim_optimized.json" + ) + else: + raise ValueError( + f"Invalid body_ik_solver_settings_type: {body_ik_solver_settings_type}" + ) + elif robot.startswith("GR1"): + return "robocasa/examples/third_party_controller/default_mink_ik_gr1_smallkd.json" + else: + raise ValueError(f"Invalid robot: {robot}") + return robosuite_controller_configs diff --git a/gr00t_wbc/control/envs/robocasa/utils/robocasa_env.py b/gr00t_wbc/control/envs/robocasa/utils/robocasa_env.py new file mode 100644 index 0000000..a3b5dfc --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/utils/robocasa_env.py @@ -0,0 +1,443 @@ +import os +from typing import Any, Dict, List, Tuple + +from gymnasium import spaces +import mujoco +import numpy as np +import robocasa +from robocasa.utils.gym_utils.gymnasium_basic import ( + RoboCasaEnv, + create_env_robosuite, +) +from robocasa.wrappers.ik_wrapper import IKWrapper +from robosuite.controllers import load_composite_controller_config +from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER + +from gr00t_wbc.control.envs.robocasa.utils.cam_key_converter import CameraKeyMapper +from gr00t_wbc.control.envs.robocasa.utils.robot_key_converter import Gr00tObsActionConverter +from gr00t_wbc.control.robot_model.robot_model import RobotModel + +ALLOWED_LANGUAGE_CHARSET = ( + "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789 ,.\n\t[]{}()!?'_:" +) + + +class Gr00tLocomanipRoboCasaEnv(RoboCasaEnv): + def __init__( + self, + env_name: str, + robots_name: str, + robot_model: RobotModel, # gr00t robot model + input_space: str = "JOINT_SPACE", # either "JOINT_SPACE" or "EEF_SPACE" + camera_names: List[str] = ["egoview"], + camera_heights: List[int] | None = None, + camera_widths: List[int] | None = None, + onscreen: bool = False, + offscreen: bool = False, + dump_rollout_dataset_dir: str | None = None, + rollout_hdf5: str | None = None, + rollout_trainset: int | None = None, + controller_configs: str | None = None, + ik_indicator: bool = False, + **kwargs, + ): + # ========= Create env ========= + if controller_configs is None: + if "G1" in robots_name: + controller_configs = ( + "robocasa/examples/third_party_controller/default_mink_ik_g1_wbc.json" + ) + elif "GR1" in robots_name: + controller_configs = ( + "robocasa/examples/third_party_controller/default_mink_ik_gr1_smallkd.json" + ) + else: + assert False, f"Unsupported robot name: {robots_name}" + controller_configs = os.path.join( + os.path.dirname(robocasa.__file__), + "../", + controller_configs, + ) + controller_configs = load_composite_controller_config( + controller=controller_configs, + robot=robots_name.split("_")[0], + ) + if input_space == "JOINT_SPACE": + controller_configs["type"] = "BASIC" + controller_configs["composite_controller_specific_configs"] = {} + controller_configs["control_delta"] = False + + self.camera_key_mapper = CameraKeyMapper() + self.camera_names = camera_names + + if camera_widths is None: + self.camera_widths = [ + self.camera_key_mapper.get_camera_config(name)[1] for name in camera_names + ] + else: + self.camera_widths = camera_widths + if camera_heights is None: + self.camera_heights = [ + self.camera_key_mapper.get_camera_config(name)[2] for name in camera_names + ] + else: + self.camera_heights = camera_heights + + self.env, self.env_kwargs = create_env_robosuite( + env_name=env_name, + robots=robots_name.split("_"), + controller_configs=controller_configs, + camera_names=camera_names, + camera_widths=self.camera_widths, + camera_heights=self.camera_heights, + enable_render=offscreen, + onscreen=onscreen, + **kwargs, # Forward kwargs to create_env_robosuite + ) + + if ik_indicator: + self.env = IKWrapper(self.env, ik_indicator=True) + + # ========= create converters first to get total DOFs ========= + # For now, assume single robot (multi-robot support can be added later) + self.obs_action_converter: List[Gr00tObsActionConverter] = [ + Gr00tObsActionConverter( + robot_model=robot_model, + robosuite_robot_model=self.env.robots[i], + ) + for i in range(len(self.env.robots)) + ] + + self.body_dofs = sum(converter.body_dof for converter in self.obs_action_converter) + self.gripper_dofs = sum(converter.gripper_dof for converter in self.obs_action_converter) + self.total_dofs = self.body_dofs + self.gripper_dofs + self.body_nu = sum(converter.body_nu for converter in self.obs_action_converter) + self.gripper_nu = sum(converter.gripper_nu for converter in self.obs_action_converter) + self.total_nu = self.body_nu + self.gripper_nu + + # ========= create spaces to match total DOFs ========= + self.get_observation_space() + self.get_action_space() + + self.enable_render = offscreen + self.render_obs_key = f"{camera_names[0]}_image" + self.render_cache = None + + self.dump_rollout_dataset_dir = dump_rollout_dataset_dir + self.gr00t_exporter = None + self.np_exporter = None + + self.rollout_hdf5 = rollout_hdf5 + self.rollout_trainset = rollout_trainset + self.rollout_initial_state = {} + + self.verbose = False + for k, v in self.observation_space.items(): + self.verbose and print("{OBS}", k, v) + for k, v in self.action_space.items(): + self.verbose and print("{ACTION}", k, v) + + self.overridden_floating_base_action = None + + def get_observation_space(self): + self.observation_space = spaces.Dict({}) + + # Add all the observation spaces + self.observation_space["time"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(1,), dtype=np.float32 + ) + self.observation_space["floating_base_pose"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(7,), dtype=np.float32 + ) + self.observation_space["floating_base_vel"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32 + ) + self.observation_space["floating_base_acc"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32 + ) + self.observation_space["body_q"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32 + ) + self.observation_space["body_dq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32 + ) + self.observation_space["body_ddq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.body_dofs,), dtype=np.float32 + ) + self.observation_space["body_tau_est"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.body_nu,), dtype=np.float32 + ) + self.observation_space["left_hand_q"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["left_hand_dq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["left_hand_ddq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["left_hand_tau_est"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_nu // 2,), dtype=np.float32 + ) + self.observation_space["right_hand_q"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["right_hand_dq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["right_hand_ddq"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_dofs // 2,), dtype=np.float32 + ) + self.observation_space["right_hand_tau_est"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(self.gripper_nu // 2,), dtype=np.float32 + ) + + self.observation_space["language.language_instruction"] = spaces.Text( + max_length=256, charset=ALLOWED_LANGUAGE_CHARSET + ) + + # Add camera observation spaces + for camera_name, w, h in zip(self.camera_names, self.camera_widths, self.camera_heights): + k = self.camera_key_mapper.get_camera_config(camera_name)[0] + self.observation_space[f"{k}_image"] = spaces.Box( + low=0, high=255, shape=(h, w, 3), dtype=np.uint8 + ) + + # Add extra privileged observation spaces + if hasattr(self.env, "get_privileged_obs_keys"): + for key, shape in self.env.get_privileged_obs_keys().items(): + self.observation_space[key] = spaces.Box( + low=-np.inf, high=np.inf, shape=shape, dtype=np.float32 + ) + + # Add robot-specific observation spaces + if hasattr(self.env.robots[0].robot_model, "torso_body"): + self.observation_space["secondary_imu_quat"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 + ) + self.observation_space["secondary_imu_vel"] = spaces.Box( + low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32 + ) + + def get_action_space(self): + self.action_space = spaces.Dict( + {"q": spaces.Box(low=-np.inf, high=np.inf, shape=(self.total_dofs,), dtype=np.float32)} + ) + + def reset(self, seed=None, options=None): + raw_obs, info = super().reset(seed=seed, options=options) + obs = self.get_gr00t_observation(raw_obs) + + lang = self.env.get_ep_meta().get("lang", "") + ROBOSUITE_DEFAULT_LOGGER.info(f"Instruction: {lang}") + + return obs, info + + def step( + self, action: Dict[str, Any] + ) -> Tuple[Dict[str, Any], float, bool, bool, Dict[str, Any]]: + # action={"q": xxx, "tau": xxx} + for k, v in action.items(): + self.verbose and print("", k, v) + + joint_actoin_vec = action["q"] + action_dict = {} + for ii, robot in enumerate(self.env.robots): + pf = robot.robot_model.naming_prefix + _action_dict = self.obs_action_converter[ii].gr00t_to_robocasa_action_dict( + joint_actoin_vec + ) + action_dict.update({f"{pf}{k}": v for k, v in _action_dict.items()}) + if action.get("tau", None) is not None: + _torque_dict = self.obs_action_converter[ii].gr00t_to_robocasa_action_dict( + action["tau"] + ) + action_dict.update({f"{pf}{k}_tau": v for k, v in _torque_dict.items()}) + if self.overridden_floating_base_action is not None: + action_dict["robot0_base"] = self.overridden_floating_base_action + raw_obs, reward, terminated, truncated, info = super().step(action_dict) + obs = self.get_gr00t_observation(raw_obs) + + for k, v in obs.items(): + self.verbose and print("", k, v.shape if k.startswith("video.") else v) + self.verbose = False + + return obs, reward, terminated, truncated, info + + def step_only_kinematics( + self, action: Dict[str, Any] + ) -> Tuple[Dict[str, Any], float, bool, bool, Dict[str, Any]]: + joint_actoin_vec = action["q"] + for ii, robot in enumerate(self.env.robots): + joint_names = np.array(self.env.sim.model.joint_names)[robot._ref_joint_indexes] + body_q = self.obs_action_converter[ii].gr00t_to_robocasa_joint_order( + joint_names, joint_actoin_vec + ) + self.env.sim.data.qpos[robot._ref_joint_pos_indexes] = body_q + + for side in ["left", "right"]: + joint_names = np.array(self.env.sim.model.joint_names)[ + robot._ref_joints_indexes_dict[side + "_gripper"] + ] + gripper_q = self.obs_action_converter[ii].gr00t_to_robocasa_joint_order( + joint_names, joint_actoin_vec + ) + self.env.sim.data.qpos[robot._ref_gripper_joint_pos_indexes[side]] = gripper_q + + mujoco.mj_forward(self.env.sim.model._model, self.env.sim.data._data) + + obs = self.force_update_observation() + return obs, 0, False, False, {"success": False} + + def force_update_observation(self, timestep=0): + raw_obs = self.env._get_observations(force_update=True, timestep=timestep) + obs = self.get_basic_observation(raw_obs) + obs = self.get_gr00t_observation(obs) + return obs + + def get_basic_observation(self, raw_obs): + # this function takes a lot of time, so we disable it for now + # raw_obs.update(gather_robot_observations(self.env, format_gripper_space=False)) + + # Image are in (H, W, C), flip it upside down + def process_img(img): + return np.copy(img[::-1, :, :]) + + for obs_name, obs_value in raw_obs.items(): + if obs_name.endswith("_image"): + # image observations + raw_obs[obs_name] = process_img(obs_value) + else: + # non-image observations + raw_obs[obs_name] = obs_value.astype(np.float32) + + # Return black image if rendering is disabled + if not self.enable_render: + for ii, name in enumerate(self.camera_names): + raw_obs[f"{name}_image"] = np.zeros( + (self.camera_heights[ii], self.camera_widths[ii], 3), dtype=np.uint8 + ) + + self.render_cache = raw_obs[self.render_obs_key] + raw_obs["language"] = self.env.get_ep_meta().get("lang", "") + + return raw_obs + + def convert_body_q(self, q: np.ndarray) -> np.ndarray: + # q is in the order of the joints + robot = self.env.robots[0] + joint_names = np.array(self.env.sim.model.joint_names)[robot._ref_joint_indexes] + # this joint names are in the order of the obs_vec + actuated_q = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order( + joint_names, q, "body" + ) + return actuated_q + + def convert_gripper_q(self, q: np.ndarray, side: str = "left") -> np.ndarray: + # q is in the order of the joints + robot = self.env.robots[0] + joint_names = np.array(self.env.sim.model.joint_names)[ + robot._ref_joints_indexes_dict[side + "_gripper"] + ] + actuated_q = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order( + joint_names, q, side + "_gripper" + ) + return actuated_q + + def convert_gripper_tau(self, tau: np.ndarray, side: str = "left") -> np.ndarray: + # tau is in the order of the actuators + robot = self.env.robots[0] + actuator_idx = robot._ref_actuators_indexes_dict[side + "_gripper"] + actuated_joint_names = [ + self.env.sim.model.joint_id2name(self.env.sim.model.actuator_trnid[i][0]) + for i in actuator_idx + ] + actuated_tau = self.obs_action_converter[0].robocasa_to_gr00t_actuated_order( + actuated_joint_names, tau, side + "_gripper" + ) + return actuated_tau + + def get_gr00t_observation(self, raw_obs: Dict[str, Any]) -> Dict[str, Any]: + obs = {} + + if self.env.sim.model.jnt_type[0] == mujoco.mjtJoint.mjJNT_FREE: + # If the first joint is a free joint, use this way to get the floating base data + obs["floating_base_pose"] = self.env.sim.data.qpos[:7] + obs["floating_base_vel"] = self.env.sim.data.qvel[:6] + obs["floating_base_acc"] = self.env.sim.data.qacc[:6] + else: + # Otherwise, use self.env.sim.model to fetch the floating base pose + root_body_id = self.env.sim.model.body_name2id("robot0_base") + + # Get position and orientation from body state + root_pos = self.env.sim.data.body_xpos[root_body_id] + root_quat = self.env.sim.data.body_xquat[root_body_id] # quaternion in wxyz format + + # Combine position and quaternion to form 7-DOF pose + obs["floating_base_pose"] = np.concatenate([root_pos, root_quat]) + # set vel and acc to 0 + obs["floating_base_vel"] = np.zeros(6) + obs["floating_base_acc"] = np.zeros(6) + + obs["body_q"] = self.convert_body_q(raw_obs["robot0_joint_pos"]) + obs["body_dq"] = self.convert_body_q(raw_obs["robot0_joint_vel"]) + obs["body_ddq"] = self.convert_body_q(raw_obs["robot0_joint_acc"]) + + obs["left_hand_q"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qpos"], "left") + obs["left_hand_dq"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qvel"], "left") + obs["left_hand_ddq"] = self.convert_gripper_q(raw_obs["robot0_left_gripper_qacc"], "left") + obs["right_hand_q"] = self.convert_gripper_q(raw_obs["robot0_right_gripper_qpos"], "right") + obs["right_hand_dq"] = self.convert_gripper_q(raw_obs["robot0_right_gripper_qvel"], "right") + obs["right_hand_ddq"] = self.convert_gripper_q( + raw_obs["robot0_right_gripper_qacc"], "right" + ) + + robot = self.env.robots[0] + body_tau_idx_list = [] + left_gripper_tau_idx_list = [] + right_gripper_tau_idx_list = [] + for part_name, actuator_idx in robot._ref_actuators_indexes_dict.items(): + if "left_gripper" in part_name: + left_gripper_tau_idx_list.extend(actuator_idx) + elif "right_gripper" in part_name: + right_gripper_tau_idx_list.extend(actuator_idx) + elif "base" in part_name: + assert ( + len(actuator_idx) == 0 or robot.robot_model.default_base == "FloatingLeggedBase" + ) + else: + body_tau_idx_list.extend(actuator_idx) + + body_tau_idx_list = sorted(body_tau_idx_list) + left_gripper_tau_idx_list = sorted(left_gripper_tau_idx_list) + right_gripper_tau_idx_list = sorted(right_gripper_tau_idx_list) + obs["body_tau_est"] = self.convert_body_q( + self.env.sim.data.actuator_force[body_tau_idx_list] + ) + obs["right_hand_tau_est"] = self.convert_gripper_tau( + self.env.sim.data.actuator_force[right_gripper_tau_idx_list], "right" + ) + obs["left_hand_tau_est"] = self.convert_gripper_tau( + self.env.sim.data.actuator_force[left_gripper_tau_idx_list], "left" + ) + + obs["time"] = self.env.sim.data.time + + # Add camera images + for ii, camera_name in enumerate(self.camera_names): + mapped_camera_name = self.camera_key_mapper.get_camera_config(camera_name)[0] + obs[f"{mapped_camera_name}_image"] = raw_obs[f"{camera_name}_image"] + + # Add privileged observations + if hasattr(self.env, "get_privileged_obs_keys"): + for key in self.env.get_privileged_obs_keys(): + obs[key] = raw_obs[key] + + # Add robot-specific observations + if hasattr(self.env.robots[0].robot_model, "torso_body"): + obs["secondary_imu_quat"] = raw_obs["robot0_torso_link_imu_quat"] + obs["secondary_imu_vel"] = raw_obs["robot0_torso_link_imu_vel"] + + obs["language.language_instruction"] = raw_obs["language"] + + return obs diff --git a/gr00t_wbc/control/envs/robocasa/utils/robot_key_converter.py b/gr00t_wbc/control/envs/robocasa/utils/robot_key_converter.py new file mode 100644 index 0000000..617bfb5 --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/utils/robot_key_converter.py @@ -0,0 +1,301 @@ +from dataclasses import dataclass +from typing import Any, Dict, List, Tuple + +import numpy as np +from robocasa.models.robots import remove_mimic_joints +from robosuite.models.robots import RobotModel as RobosuiteRobotModel + +from gr00t_wbc.control.robot_model import RobotModel + + +class Gr00tJointInfo: + """ + Mapping from gr00t_wbc actuated joint names to robocasa joint names. + """ + + def __init__(self, robot_model: RobosuiteRobotModel): + self.robocasa_body_prefix = "robot0_" + self.robocasa_gripper_prefix = "gripper0_" + + self.robot_model: RobotModel = robot_model + self.body_actuated_joint_names: List[str] = ( + self.robot_model.supplemental_info.body_actuated_joints + ) + self.left_hand_actuated_joint_names: List[str] = ( + self.robot_model.supplemental_info.left_hand_actuated_joints + ) + self.right_hand_actuated_joint_names: List[str] = ( + self.robot_model.supplemental_info.right_hand_actuated_joints + ) + + self.actuated_joint_names: List[str] = self._get_gr00t_actuated_joint_names() + self.body_actuated_joint_to_index: Dict[str, int] = ( + self._get_gr00t_body_actuated_joint_name_to_index() + ) + self.gripper_actuated_joint_to_index: Tuple[Dict[str, int], Dict[str, int]] = ( + self._get_gr00t_gripper_actuated_joint_name_to_index() + ) + self.actuated_joint_name_to_index: Dict[str, int] = ( + self._get_gr00t_actuated_joint_name_to_index() + ) + + def _get_gr00t_actuated_joint_names(self) -> List[str]: + """Get list of gr00t actuated joint names ordered by their indices.""" + if self.robot_model.supplemental_info is None: + raise ValueError("Robot model must have supplemental_info") + + # Get joint names and indices + body_names = self.robot_model.supplemental_info.body_actuated_joints + left_hand_names = self.robot_model.supplemental_info.left_hand_actuated_joints + right_hand_names = self.robot_model.supplemental_info.right_hand_actuated_joints + + body_indices = self.robot_model.get_joint_group_indices("body") + left_hand_indices = self.robot_model.get_joint_group_indices("left_hand") + right_hand_indices = self.robot_model.get_joint_group_indices("right_hand") + + # Create a dictionary mapping index to name + index_to_name = {} + for name, idx in zip(body_names, body_indices): + index_to_name[idx] = self.robocasa_body_prefix + name + for name, idx in zip(left_hand_names, left_hand_indices): + index_to_name[idx] = self.robocasa_gripper_prefix + "left_" + name + for name, idx in zip(right_hand_names, right_hand_indices): + index_to_name[idx] = self.robocasa_gripper_prefix + "right_" + name + sorted_indices = sorted(index_to_name.keys()) + all_actuated_joint_names = [index_to_name[idx] for idx in sorted_indices] + return all_actuated_joint_names + + def _get_gr00t_body_actuated_joint_name_to_index(self) -> Dict[str, int]: + """Get dictionary mapping gr00t actuated joint names to indices.""" + if self.robot_model.supplemental_info is None: + raise ValueError("Robot model must have supplemental_info") + body_names = self.robot_model.supplemental_info.body_actuated_joints + body_indices = self.robot_model.get_joint_group_indices("body") + sorted_indices = np.argsort(body_indices) + sorted_names = [body_names[i] for i in sorted_indices] + return {self.robocasa_body_prefix + name: ii for ii, name in enumerate(sorted_names)} + + def _get_gr00t_gripper_actuated_joint_name_to_index( + self, + ) -> Tuple[Dict[str, int], Dict[str, int]]: + """Get dictionary mapping gr00t actuated joint names to indices.""" + if self.robot_model.supplemental_info is None: + raise ValueError("Robot model must have supplemental_info") + left_hand_names = self.robot_model.supplemental_info.left_hand_actuated_joints + right_hand_names = self.robot_model.supplemental_info.right_hand_actuated_joints + left_hand_indices = self.robot_model.get_joint_group_indices("left_hand") + right_hand_indices = self.robot_model.get_joint_group_indices("right_hand") + sorted_left_hand_indices = np.argsort(left_hand_indices) + sorted_right_hand_indices = np.argsort(right_hand_indices) + sorted_left_hand_names = [left_hand_names[i] for i in sorted_left_hand_indices] + sorted_right_hand_names = [right_hand_names[i] for i in sorted_right_hand_indices] + return ( + { + self.robocasa_gripper_prefix + "left_" + name: ii + for ii, name in enumerate(sorted_left_hand_names) + }, + { + self.robocasa_gripper_prefix + "right_" + name: ii + for ii, name in enumerate(sorted_right_hand_names) + }, + ) + + def _get_gr00t_actuated_joint_name_to_index(self) -> Dict[str, int]: + """Get dictionary mapping gr00t actuated joint names to indices.""" + return {name: ii for ii, name in enumerate(self.actuated_joint_names)} + + +@dataclass +class Gr00tObsActionConverter: + """ + Converter to align simulation environment joint action space with real environment joint action space. + Handles joint order and range conversion. + """ + + robot_model: RobotModel + robosuite_robot_model: RobosuiteRobotModel + robocasa_body_prefix: str = "robot0_" + robocasa_gripper_prefix: str = "gripper0_" + + def __post_init__(self): + """Initialize converter with robot configuration.""" + + self.robot_key = self.robot_model.supplemental_info.name + self.gr00t_joint_info = Gr00tJointInfo(self.robot_model) + self.robocasa_joint_names_for_each_part: Dict[str, List[str]] = ( + self._get_robocasa_joint_names_for_each_part() + ) + self.robocasa_actuator_names_for_each_part: Dict[str, List[str]] = ( + self._get_robotcasa_actuator_names_for_each_part() + ) + + # Store mappings directly as class attributes + self.gr00t_joint_name_to_index = self.gr00t_joint_info.actuated_joint_name_to_index + self.gr00t_body_joint_name_to_index = self.gr00t_joint_info.body_actuated_joint_to_index + self.gr00t_gripper_joint_name_to_index = { + "left": self.gr00t_joint_info.gripper_actuated_joint_to_index[0], + "right": self.gr00t_joint_info.gripper_actuated_joint_to_index[1], + } + self.gr00t_to_robocasa_actuator_indices = self._get_actuator_mapping() + + if self.robot_key == "GR1_Fourier": + self.joint_multiplier = ( + lambda x: np.array([-1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1]) * x + ) + self.actuator_multiplier = ( + lambda x: np.array([-1, -1, -1, -1, -1, -1, -1, -1, 1, 1, -1]) * x + ) + else: + self.joint_multiplier = lambda x: x + self.actuator_multiplier = lambda x: x + + # Store DOF counts directly + self.body_dof = len(self.gr00t_joint_info.body_actuated_joint_names) + self.gripper_dof = len(self.gr00t_joint_info.left_hand_actuated_joint_names) + len( + self.gr00t_joint_info.right_hand_actuated_joint_names + ) + self.whole_dof = self.body_dof + self.gripper_dof + self.body_nu = len(self.gr00t_joint_info.body_actuated_joint_names) + self.gripper_nu = len(self.gr00t_joint_info.left_hand_actuated_joint_names) + len( + self.gr00t_joint_info.right_hand_actuated_joint_names + ) + self.whole_nu = self.body_nu + self.gripper_nu + + def _get_robocasa_joint_names_for_each_part(self) -> Dict[str, List[str]]: + part_names = self.robosuite_robot_model._ref_joints_indexes_dict.keys() + robocasa_joint_names_for_each_part = {} + for part_name in part_names: + joint_indices = self.robosuite_robot_model._ref_joints_indexes_dict[part_name] + joint_names = [ + self.robosuite_robot_model.sim.model.joint_id2name(j) for j in joint_indices + ] + robocasa_joint_names_for_each_part[part_name] = joint_names + return robocasa_joint_names_for_each_part + + def _get_robotcasa_actuator_names_for_each_part(self) -> Dict[str, List[str]]: + part_names = self.robosuite_robot_model._ref_actuators_indexes_dict.keys() + robocasa_actuator_names_for_each_part = {} + for part_name in part_names: + if part_name == "base": + continue + actuator_indices = self.robosuite_robot_model._ref_actuators_indexes_dict[part_name] + actuator_names = [ + self.robosuite_robot_model.sim.model.actuator_id2name(j) for j in actuator_indices + ] + robocasa_actuator_names_for_each_part[part_name] = actuator_names + return robocasa_actuator_names_for_each_part + + def _get_actuator_mapping(self) -> Dict[str, List[int]]: + """Get mapping from gr00t_wbc actuatored joint order to robocasa actuatored joint order for whole body.""" + return { + part_name: [ + self.gr00t_joint_info.actuated_joint_name_to_index[j] + for j in self.robocasa_actuator_names_for_each_part[part_name] + ] + for part_name in self.robocasa_actuator_names_for_each_part.keys() + } + + def check_action_dim_match(self, vec_dim: int) -> bool: + """ + Check if input vector dimension matches expected dimension. + + Args: + vec_dim: Dimension of input vector + + Returns: + bool: True if dimensions match + """ + return vec_dim == self.whole_dof + + def gr00t_to_robocasa_action_dict(self, action_vec: np.ndarray) -> Dict[str, Any]: + """ + Convert gr00t flat action vector to robocasa dictionary mapping part names to actions. + + Args: + robot: Robocasa robot model instance + action_vec: Full action vector array in gr00t actuated joint order + + Returns: + dict: Mapping from part names to action vectors for robocasa + """ + if not self.check_action_dim_match(len(action_vec)): + raise ValueError( + f"Action vector dimension mismatch: {len(action_vec)} != {self.whole_dof}" + ) + + action_dict = {} + cc = self.robosuite_robot_model.composite_controller + + for part_name, controller in cc.part_controllers.items(): + if "gripper" in part_name: + robocasa_action = action_vec[self.gr00t_to_robocasa_actuator_indices[part_name]] + if self.actuator_multiplier is not None: + robocasa_action = self.actuator_multiplier(robocasa_action) + action_dict[part_name] = remove_mimic_joints( + cc.grippers[part_name], robocasa_action + ) + elif "base" in part_name: + assert ( + len(self.gr00t_to_robocasa_actuator_indices.get(part_name, [])) == 0 + or self.robosuite_robot_model.default_base == "FloatingLeggedBase" + ) + else: + action_dict[part_name] = action_vec[ + self.gr00t_to_robocasa_actuator_indices[part_name] + ] + + return action_dict + + def robocasa_to_gr00t_actuated_order( + self, joint_names: List[str], q: np.ndarray, obs_type: str = "body" + ) -> np.ndarray: + """ + Convert observation from robocasa joint order to gr00t actuated joint order. + + Args: + joint_names: List of joint names in robocasa order (with prefixes) + q: Joint positions corresponding to joint_names + obs_type: Type of observation ("body", "left_gripper", "right_gripper", or "whole") + + Returns: + Joint positions in gr00t actuated joint order + """ + assert len(joint_names) == len(q), "Joint names and q must have the same length" + + if obs_type == "body": + actuated_q = np.zeros(self.body_dof) + for i, jn in enumerate(joint_names): + actuated_q[self.gr00t_body_joint_name_to_index[jn]] = q[i] + elif obs_type == "left_gripper": + actuated_q = np.zeros(self.gripper_dof // 2) + for i, jn in enumerate(joint_names): + actuated_q[self.gr00t_gripper_joint_name_to_index["left"][jn]] = q[i] + elif obs_type == "right_gripper": + actuated_q = np.zeros(self.gripper_dof // 2) + for i, jn in enumerate(joint_names): + actuated_q[self.gr00t_gripper_joint_name_to_index["right"][jn]] = q[i] + elif obs_type == "whole": + actuated_q = np.zeros(self.whole_dof) + for i, jn in enumerate(joint_names): + actuated_q[self.gr00t_joint_name_to_index[jn]] = q[i] + else: + raise ValueError(f"Unknown observation type: {obs_type}") + return actuated_q + + def gr00t_to_robocasa_joint_order( + self, joint_names: List[str], q_in_actuated_order: np.ndarray + ) -> np.ndarray: + """ + Convert gr00t actuated joint order to robocasa joint order. + + Args: + joint_names: List of joint names in robocasa order (with prefixes) + q_in_actuated_order: Joint positions corresponding to joint_names in gr00t actuated joint order + + Returns: + Joint positions in robocasa joint order + """ + q = np.zeros(len(joint_names)) + for i, jn in enumerate(joint_names): + q[i] = q_in_actuated_order[self.gr00t_joint_name_to_index[jn]] + return q diff --git a/gr00t_wbc/control/envs/robocasa/utils/sim_utils.py b/gr00t_wbc/control/envs/robocasa/utils/sim_utils.py new file mode 100644 index 0000000..5250f25 --- /dev/null +++ b/gr00t_wbc/control/envs/robocasa/utils/sim_utils.py @@ -0,0 +1,8 @@ +try: + import robosuite.macros_private as macros +except ImportError: + import robosuite.macros as macros + + +def change_simulation_timestep(timestep: float): + macros.SIMULATION_TIMESTEP = timestep diff --git a/gr00t_wbc/control/main/__init__.py b/gr00t_wbc/control/main/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/main/config_template.py b/gr00t_wbc/control/main/config_template.py new file mode 100644 index 0000000..8da84d5 --- /dev/null +++ b/gr00t_wbc/control/main/config_template.py @@ -0,0 +1,45 @@ +from dataclasses import asdict, dataclass +from typing import Any + + +@dataclass +class ArgsConfig: + """Args Config for running the data collection loop.""" + + def update( + self, + config_dict: dict, + strict: bool = False, + skip_keys: list[str] = [], + allowed_keys: list[str] | None = None, + ): + for k, v in config_dict.items(): + if k in skip_keys: + continue + if allowed_keys is not None and k not in allowed_keys: + continue + if strict and not hasattr(self, k): + raise ValueError(f"Config {k} not found in {self.__class__.__name__}") + if not strict and not hasattr(self, k): + continue + setattr(self, k, v) + + @classmethod + def from_dict( + cls, + config_dict: dict, + strict: bool = False, + skip_keys: list[str] = [], + allowed_keys: list[str] | None = None, + ): + instance = cls() + instance.update( + config_dict=config_dict, strict=strict, skip_keys=skip_keys, allowed_keys=allowed_keys + ) + return instance + + def to_dict(self): + return asdict(self) + + def get(self, key: str, default: Any = None): + return getattr(self, key) if hasattr(self, key) else default diff --git a/gr00t_wbc/control/main/constants.py b/gr00t_wbc/control/main/constants.py new file mode 100644 index 0000000..8d54f25 --- /dev/null +++ b/gr00t_wbc/control/main/constants.py @@ -0,0 +1,16 @@ +IMAGE_TOPIC_NAME = "realsense/color/image_raw" +STATE_TOPIC_NAME = "G1Env/env_state_act" +CONTROL_GOAL_TOPIC = "ControlPolicy/upper_body_pose" +ROBOT_CONFIG_TOPIC = "WBCPolicy/robot_config" +KEYBOARD_INPUT_TOPIC = "/keyboard_input" +LOCO_MANIP_TASK_STATUS_TOPIC = "LocoManipPolicy/task_status" +LOCO_NAV_TASK_STATUS_TOPIC = "NavigationPolicy/task_status" +LOWER_BODY_POLICY_STATUS_TOPIC = "ControlPolicy/lower_body_policy_status" +JOINT_SAFETY_STATUS_TOPIC = "ControlPolicy/joint_safety_status" + + +DEFAULT_NAV_CMD = [0.0, 0.0, 0.0] +DEFAULT_BASE_HEIGHT = 0.74 +DEFAULT_WRIST_POSE = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] * 2 # x, y, z + w, x, y, z + +DEFAULT_MODEL_SERVER_PORT = 5555 # port used to host the model server diff --git a/gr00t_wbc/control/main/teleop/__init__.py b/gr00t_wbc/control/main/teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/main/teleop/configs/configs.py b/gr00t_wbc/control/main/teleop/configs/configs.py new file mode 100644 index 0000000..b1f6251 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/configs/configs.py @@ -0,0 +1,484 @@ +from dataclasses import dataclass +import os +from pathlib import Path +from typing import Literal, Optional + +import yaml + +import gr00t_wbc +from gr00t_wbc.control.main.config_template import ArgsConfig as ArgsConfigTemplate +from gr00t_wbc.control.policy.wbc_policy_factory import WBC_VERSIONS +from gr00t_wbc.control.utils.network_utils import resolve_interface + + +def override_wbc_config( + wbc_config: dict, config: "BaseConfig", missed_keys_only: bool = False +) -> dict: + """Override WBC YAML values with dataclass values. + + Args: + wbc_config: The loaded WBC YAML configuration dictionary + config: The BaseConfig dataclass instance with override values + missed_keys_only: If True, only add keys that don't exist in wbc_config. + If False, validate all keys exist and override all. + + Returns: + Updated wbc_config dictionary with overridden values + + Raises: + KeyError: If any required keys are missing from the WBC YAML configuration + (only when missed_keys_only=False) + """ + # Override yaml values with dataclass values + key_to_value = { + "INTERFACE": config.interface, + "ENV_TYPE": config.env_type, + "VERSION": config.wbc_version, + "SIMULATOR": config.simulator, + "SIMULATE_DT": 1 / float(config.sim_frequency), + "ENABLE_OFFSCREEN": config.enable_offscreen, + "ENABLE_ONSCREEN": config.enable_onscreen, + "model_path": config.wbc_model_path, + "enable_waist": config.enable_waist, + "with_hands": config.with_hands, + "verbose": config.verbose, + "verbose_timing": config.verbose_timing, + "upper_body_max_joint_speed": config.upper_body_joint_speed, + "keyboard_dispatcher_type": config.keyboard_dispatcher_type, + "enable_gravity_compensation": config.enable_gravity_compensation, + "gravity_compensation_joints": config.gravity_compensation_joints, + "high_elbow_pose": config.high_elbow_pose, + } + + if missed_keys_only: + # Only add keys that don't exist in wbc_config + for key in key_to_value: + if key not in wbc_config: + wbc_config[key] = key_to_value[key] + else: + # Set all keys (overwrite existing) + for key in key_to_value: + wbc_config[key] = key_to_value[key] + + # g1 kp, kd, sim2real gap + if config.env_type == "real": + # update waist pitch damping, index 14 + wbc_config["MOTOR_KD"][14] = wbc_config["MOTOR_KD"][14] - 10 + + return wbc_config + + +@dataclass +class BaseConfig(ArgsConfigTemplate): + """Base config inherited by all G1 control loops""" + + # WBC Configuration + wbc_version: Literal[tuple(WBC_VERSIONS)] = "gear_wbc" + """Version of the whole body controller.""" + + wbc_model_path: str = ( + "policy/GR00T-WholeBodyControl-Balance.onnx," + "policy/GR00T-WholeBodyControl-Walk.onnx" + ) + """Path to WBC model file (relative to gr00t_wbc/sim2mujoco/resources/robots/g1)""" + """gear_wbc model path: policy/GR00T-WholeBodyControl-Balance.onnx,policy/GR00T-WholeBodyControl-Walk.onnx""" + + wbc_policy_class: str = "G1DecoupledWholeBodyPolicy" + """Whole body policy class.""" + + # System Configuration + interface: str = "sim" + """Interface to use for the control loop. [sim, real, lo, enxe8ea6a9c4e09]""" + + simulator: str = "mujoco" + """Simulator to use.""" + + sim_sync_mode: bool = False + """Whether to run the control loop in sync mode.""" + + control_frequency: int = 50 + """Frequency of the control loop.""" + + sim_frequency: int = 200 + """Frequency of the simulation loop.""" + + # Robot Configuration + enable_waist: bool = False + """Whether to include waist joints in IK.""" + + with_hands: bool = True + """Enable hand functionality. When False, robot operates without hands.""" + + high_elbow_pose: bool = False + """Enable high elbow pose configuration for default joint positions.""" + + verbose: bool = True + """Whether to print verbose output.""" + + # Additional common fields + enable_offscreen: bool = False + """Whether to enable offscreen rendering.""" + + enable_onscreen: bool = True + """Whether to enable onscreen rendering.""" + + upper_body_joint_speed: float = 1000 + """Upper body joint speed.""" + + env_name: str = "default" + """Environment name.""" + + ik_indicator: bool = False + """Whether to draw IK indicators.""" + + verbose_timing: bool = False + """Enable verbose timing output every iteration.""" + + keyboard_dispatcher_type: str = "raw" + """Keyboard dispatcher to use. [raw, ros]""" + + # Gravity Compensation Configuration + enable_gravity_compensation: bool = False + """Enable gravity compensation using pinocchio dynamics.""" + + gravity_compensation_joints: Optional[list[str]] = None + """Joint groups to apply gravity compensation to (e.g., ['arms', 'left_arm', 'right_arm']).""" + # Teleop/Device Configuration + body_control_device: str = "dummy" + """Device to use for body control. Options: dummy, vive, iphone, leapmotion, joycon.""" + + hand_control_device: Optional[str] = "dummy" + """Device to use for hand control. Options: None, manus, joycon, iphone.""" + + body_streamer_ip: str = "10.112.210.229" + """IP address for body streamer (vive only).""" + + body_streamer_keyword: str = "knee" + """Body streamer keyword (vive only).""" + + enable_visualization: bool = False + """Whether to enable visualization.""" + + enable_real_device: bool = True + """Whether to enable real device.""" + + teleop_frequency: int = 20 + """Teleoperation frequency (Hz).""" + + teleop_replay_path: Optional[str] = None + """Path to teleop replay data.""" + + # Deployment/Camera Configuration + robot_ip: str = "192.168.123.164" + """Robot IP address""" + # Data collection settings + data_collection: bool = True + """Enable data collection""" + + data_collection_frequency: int = 20 + """Data collection frequency (Hz)""" + + root_output_dir: str = "outputs" + """Root output directory""" + + # Policy settings + enable_upper_body_operation: bool = True + """Enable upper body operation""" + + upper_body_operation_mode: Literal["teleop", "inference"] = "teleop" + """Upper body operation mode""" + + def __post_init__(self): + # Resolve interface (handles sim/real shortcuts, platform differences, and error handling) + self.interface, self.env_type = resolve_interface(self.interface) + + def load_wbc_yaml(self) -> dict: + """Load and merge wbc yaml with dataclass overrides""" + # Get the base path to gr00t_wbc and convert to Path object + package_path = Path(os.path.dirname(gr00t_wbc.__file__)) + + if self.wbc_version == "gear_wbc": + config_path = str(package_path / "control/main/teleop/configs/g1_29dof_gear_wbc.yaml") + else: + raise ValueError( + f"Invalid wbc_version: {self.wbc_version}, please use one of: " f"gear_wbc" + ) + + with open(config_path) as file: + wbc_config = yaml.load(file, Loader=yaml.FullLoader) + + # Override yaml values with dataclass values + wbc_config = override_wbc_config(wbc_config, self) + + return wbc_config + + +@dataclass +class ControlLoopConfig(BaseConfig): + """Config for running the G1 control loop.""" + + pass + + +@dataclass +class TeleopConfig(BaseConfig): + """Config for running the G1 teleop policy loop.""" + + robot: Literal["g1"] = "g1" + """Name of the robot to use, e.g., 'g1'.""" + + lerobot_replay_path: Optional[str] = None + """Path to lerobot replay data.""" + + # Override defaults for teleop-specific values + body_streamer_ip: str = "10.110.67.24" + """IP address for body streamer (vive only).""" + + body_streamer_keyword: str = "foot" + """Keyword for body streamer (vive only).""" + + teleop_frequency: float = 20 # Override to be float instead of int + """Frequency of the teleop loop.""" + + binary_hand_ik: bool = True + """Whether to use binary IK.""" + + +@dataclass +class ComposedCameraClientConfig: + """Config for running the composed camera client.""" + + camera_port: int = 5555 + """Port number""" + + camera_host: str = "localhost" + """Host IP address""" + + fps: float = 20.0 + """FPS of the camera viewer""" + + +@dataclass +class DataExporterConfig(BaseConfig, ComposedCameraClientConfig): + """Config for running the G1 data exporter.""" + + dataset_name: Optional[str] = None + """Name of the dataset to save the data to. If the dataset already exists, + the new episodes will be appended to existing dataset. If the dataset does not exist, + episodes will be saved under root_output_dir/dataset_name. + """ + + task_prompt: str = "demo" + """Language Task prompt for the dataset.""" + + state_dim: int = 43 + """Size of the state.""" + + action_dim: int = 43 + """Size of the action.""" + + teleoperator_username: Optional[str] = None + """Teleoperator username.""" + + support_operator_username: Optional[str] = None + """Support operator username.""" + + robot_id: Optional[str] = None + """Robot ID.""" + + lower_body_policy: Optional[str] = None + """Lower body policy.""" + + img_stream_viewer: bool = False + """Whether to open a matplot lib window to view the camera images.""" + + text_to_speech: bool = True + """Whether to use text-to-speech for voice feedback.""" + + add_stereo_camera: bool = True + """Whether to add stereo camera for data collection. If False, only use a signle ego view camera.""" + + +@dataclass +class SyncSimDataCollectionConfig(ControlLoopConfig, TeleopConfig): + """Args Config for running the data collection loop.""" + + robot: str = "G1" + """Name of the robot to collect data for (e.g., G1 variants).""" + + task_name: str = "GroundOnly" + """Name of the task to collect data for. [PnPBottle, GroundOnly, ...]""" + + body_control_device: str = "dummy" + """Device to use for body control. Options: dummy, vive, iphone, leapmotion, joycon.""" + + hand_control_device: Optional[str] = "dummy" + """Device to use for hand control. Options: None, manus, joycon, iphone.""" + + remove_existing_dir: bool = False + """Whether to remove existing output directory if it exists.""" + + hardcode_teleop_cmd: bool = False + """Whether to hardcode the teleop command for testing purposes.""" + + ik_indicator: bool = False + """Whether to draw IK indicators.""" + + enable_onscreen: bool = True + """Whether to show the onscreen rendering.""" + + save_img_obs: bool = False + """Whether to save image observations.""" + + success_hold_steps: int = 50 + """Number of steps to collect after task completion before saving.""" + + renderer: Literal["mjviewer", "mujoco", "rerun"] = "mjviewer" + """Renderer to use for the environment. """ + + replay_data_path: str | None = None + """Path to the data (.pkl) to replay. If None, will not replay. Used for CI/CD.""" + + replay_speed: float = 2.5 + """Speed multiplier for replay data. Higher values make replay slower (e.g., 2.5 for sync sim tests).""" + + ci_test: bool = False + """Whether to run the CI test.""" + + ci_test_mode: Literal["unit", "pre_merge"] = "pre_merge" + """'unit' for fast 50-step tests, 'pre_merge' for 500-step test with tracking checks.""" + + manual_control: bool = False + """Enable manual control of data collection start/save. When True, use toggle_data_collection + to manually control episode states (idle -> recording -> need_to_save -> idle). + When False (default), automatically starts and stops data collection based on task completion.""" + + +@dataclass +class SyncSimPlaybackConfig(SyncSimDataCollectionConfig): + """Configuration class for playback script arguments.""" + + enable_real_device: bool = False + """Whether to enable real device""" + + dataset: str | None = None + """Path to the demonstration dataset, either an HDF5 file or a LeRobot folder path.""" + + use_actions: bool = False + """Whether to use actions for playback""" + + use_wbc_goals: bool = False + """Whether to use WBC goals for control""" + + use_teleop_cmd: bool = False + """Whether to use teleop IK for action generation""" + + # Video recording arguments. + # Warning: enabling this key will leads to divergence between playback and recording. + save_video: bool = False + """Whether to save video of the playback""" + + # Saving to LeRobot dataset. + # Warning: enabling this key will leads to divergence between playback and recording. + save_lerobot: bool = False + """Whether to save the playback as a new LeRobot dataset""" + + video_path: str | None = None + """Path to save the output video. If not specified, + will use the nearest folder to dataset and save as playback_video.mp4""" + + num_episodes: int = 1 + """Number of episodes to load and playback/record (loads only the first N episodes from dataset)""" + + intervention: bool = False + """Whether to denote intervention timesteps with colored borders in video frames""" + + ci_test: bool = False + """Whether this is a CI test run, which limits the number of steps for testing purposes""" + + def validate_args(self): + # Validate argument combinations + if self.use_teleop_cmd and not self.use_actions: + raise ValueError("--use-teleop-cmd requires --use-actions to be set") + + # Note: using teleop cmd has playback divergence unlike using wbc goals, as TeleopPolicy has a warmup loop + if self.use_teleop_cmd and self.use_wbc_goals: + raise ValueError("--use-teleop-cmd and --use-wbc-goals are mutually exclusive") + + if (self.use_teleop_cmd or self.use_wbc_goals) and not self.use_actions: + raise ValueError( + "You are using --use-teleop-cmd or --use-wbc-goals but not --use-actions. " + "This will not play back actions whether via teleop or wbc goals. " + "Instead, it'll play back states only." + ) + + if self.save_img_obs and not self.save_lerobot: + raise ValueError("--save-img-obs is only supported with --save-lerobot") + + if self.intervention and not self.save_video: + raise ValueError("--intervention requires --save-video to be enabled for visualization") + + +@dataclass +class WebcamRecorderConfig(BaseConfig): + """Config for running the webcam recorder.""" + + output_dir: str = "logs_experiment" + """Output directory for webcam recordings""" + + device_id: int = 0 + """Camera device ID""" + + fps: int = 30 + """Recording frame rate""" + + duration: Optional[int] = None + """Recording duration in seconds (None for continuous)""" + + +@dataclass +class SimLoopConfig(BaseConfig): + """Config for running the simulation loop.""" + + mp_start_method: str = "spawn" + """Multiprocessing start method""" + + enable_image_publish: bool = False + """Enable image publishing in simulation""" + + camera_port: int = 5555 + """Camera port for image publishing""" + + verbose: bool = False + """Verbose output, override the base config verbose""" + + +@dataclass +class DeploymentConfig(BaseConfig, ComposedCameraClientConfig): + """G1 Robot Deployment Configuration + + Simplified deployment config that inherits all common fields from G1BaseConfig. + All deployment settings are now available in the base config. + """ + + camera_publish_rate: float = 30.0 + """Camera publish rate (Hz)""" + + view_camera: bool = True + """Enable camera viewer""" + # Webcam recording settings + enable_webcam_recording: bool = True + """Enable webcam recording for real robot deployment monitoring""" + + webcam_output_dir: str = "logs_experiment" + """Output directory for webcam recordings""" + + skip_img_transform: bool = False + """Skip image transformation in the model (for faster internet)""" + + sim_in_single_process: bool = False + """Run simulator in a separate process. When True, sets simulator to None in main control loop + and launches run_sim_loop.py separately.""" + + image_publish: bool = False + """Enable image publishing in simulation loop (passed to run_sim_loop.py)""" diff --git a/gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml b/gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml new file mode 100644 index 0000000..8583066 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml @@ -0,0 +1,421 @@ +GEAR_WBC_CONFIG: "gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml" + +# copy from g1_43dof_hist.yaml +ROBOT_TYPE: 'g1_29dof' # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" +ROBOT_SCENE: "gr00t_wbc/control/robot_model/model_data/g1/scene_43dof.xml" # Robot scene, for Sim2Sim +# ROBOT_SCENE: "gr00t_wbc/control/robot_model/model_data/g1/scene_29dof_activated3dex.xml" + +DOMAIN_ID: 0 # Domain id +# Network Interface, "lo" for simulation and the one with "192.168.123.222" for real robot +# INTERFACE: "enxe8ea6a9c4e09" +# INTERFACE: "enxc8a3623c9cb7" +INTERFACE: "lo" +SIMULATOR: "mujoco" # "robocasa" + +USE_JOYSTICK: 0 # Simulate Unitree WirelessController using a gamepad (0: disable, 1: enable) +JOYSTICK_TYPE: "xbox" # support "xbox" and "switch" gamepad layout +JOYSTICK_DEVICE: 0 # Joystick number + +FREE_BASE: False + +PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information of robot +ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1 + +SIMULATE_DT: 0.005 # Need to be larger than the runtime of viewer.sync() +VIEWER_DT: 0.02 # Viewer update time +REWARD_DT: 0.02 +USE_SENSOR: False +USE_HISTORY: True +USE_HISTORY_LOCO: True +USE_HISTORY_MIMIC: True + +GAIT_PERIOD: 0.9 # 1.25 + +MOTOR2JOINT: [0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 10, 11, + 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28] + +JOINT2MOTOR: [0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 10, 11, + 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28] + + +UNITREE_LEGGED_CONST: + HIGHLEVEL: 0xEE + LOWLEVEL: 0xFF + TRIGERLEVEL: 0xF0 + PosStopF: 2146000000.0 + VelStopF: 16000.0 + MODE_MACHINE: 5 + MODE_PR: 0 + +JOINT_KP: [ + 100, 100, 100, 200, 20, 20, + 100, 100, 100, 200, 20, 20, + 400, 400, 400, + 90, 60, 20, 60, 4, 4, 4, + 90, 60, 20, 60, 4, 4, 4 +] + + +JOINT_KD: [ + 2.5, 2.5, 2.5, 5, 0.2, 0.1, + 2.5, 2.5, 2.5, 5, 0.2, 0.1, + 5.0, 5.0, 5.0, + 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2, + 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2 +] + +# arm kp +# soft kp, safe, test it first +# 50, 50, 20, 20, 10, 10, 10 +# hard kp, use only if policy is safe +# 200, 200, 80, 80, 50, 50, 50, + +# MOTOR_KP: [ +# 100, 100, 100, 200, 20, 20, +# 100, 100, 100, 200, 20, 20, +# 400, 400, 400, +# 50, 50, 20, 20, 10, 10, 10, +# 50, 50, 20, 20, 10, 10, 10 +# ] + +MOTOR_KP: [ + 150, 150, 150, 200, 40, 40, + 150, 150, 150, 200, 40, 40, + 250, 250, 250, + 100, 100, 40, 40, 20, 20, 20, + 100, 100, 40, 40, 20, 20, 20 +] + +MOTOR_KD: [ + 2, 2, 2, 4, 2, 2, + 2, 2, 2, 4, 2, 2, + 5, 5, 5, + 5, 5, 2, 2, 2, 2, 2, + 5, 5, 2, 2, 2, 2, 2 +] + +# MOTOR_KP: [ +# 100, 100, 100, 200, 20, 20, +# 100, 100, 100, 200, 20, 20, +# 400, 400, 400, +# 90, 60, 20, 60, 4, 4, 4, +# 90, 60, 20, 60, 4, 4, 4 +# ] + + +# MOTOR_KD: [ +# 2.5, 2.5, 2.5, 5, 0.2, 0.1, +# 2.5, 2.5, 2.5, 5, 0.2, 0.1, +# 5.0, 5.0, 5.0, +# 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2, +# 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2 +# ] + + +WeakMotorJointIndex: + left_hip_yaw_joint: 0 + left_hip_roll_joint: 1 + left_hip_pitch_joint: 2 + left_knee_joint: 3 + left_ankle_pitch_joint: 4 + left_ankle_roll_joint: 5 + right_hip_yaw_joint: 6 + right_hip_roll_joint: 7 + right_hip_pitch_joint: 8 + right_knee_joint: 9 + right_ankle_pitch_joint: 10 + right_ankle_roll_joint: 11 + waist_yaw_joint : 12 + waist_roll_joint : 13 + waist_pitch_joint : 14 + left_shoulder_pitch_joint: 15 + left_shoulder_roll_joint: 16 + left_shoulder_yaw_joint: 17 + left_elbow_joint: 18 + left_wrist_roll_joint: 19 + left_wrist_pitch_joint: 20 + left_wrist_yaw_joint: 21 + right_shoulder_pitch_joint: 22 + right_shoulder_roll_joint: 23 + right_shoulder_yaw_joint: 24 + right_elbow_joint: 25 + right_wrist_roll_joint: 26 + right_wrist_pitch_joint: 27 + right_wrist_yaw_joint: 28 + +NUM_MOTORS: 29 +NUM_JOINTS: 29 +NUM_HAND_MOTORS: 7 +NUM_HAND_JOINTS: 7 +NUM_UPPER_BODY_JOINTS: 17 + +DEFAULT_DOF_ANGLES: [ + -0.1, # left_hip_pitch_joint + 0.0, # left_hip_roll_joint + 0.0, # left_hip_yaw_joint + 0.3, # left_knee_joint + -0.2, # left_ankle_pitch_joint + 0.0, # left_ankle_roll_joint + -0.1, # right_hip_pitch_joint + 0.0, # right_hip_roll_joint + 0.0, # right_hip_yaw_joint + 0.3, # right_knee_joint + -0.2, # right_ankle_pitch_joint + 0.0, # right_ankle_roll_joint + 0.0, # waist_yaw_joint + 0.0, # waist_roll_joint + 0.0, # waist_pitch_joint + 0.0, # left_shoulder_pitch_joint + 0.0, # left_shoulder_roll_joint + 0.0, # left_shoulder_yaw_joint + 0.0, # left_elbow_joint + 0.0, # left_wrist_roll_joint + 0.0, # left_wrist_pitch_joint + 0.0, # left_wrist_yaw_joint + 0.0, # right_shoulder_pitch_joint + 0.0, # right_shoulder_roll_joint + 0.0, # right_shoulder_yaw_joint + 0.0, # right_elbow_joint + 0.0, # right_wrist_roll_joint + 0.0, # right_wrist_pitch_joint + 0.0 # right_wrist_yaw_joint +] + +DEFAULT_MOTOR_ANGLES: [ + -0.1, # left_hip_pitch_joint + 0.0, # left_hip_roll_joint + 0.0, # left_hip_yaw_joint + 0.3, # left_knee_joint + -0.2, # left_ankle_pitch_joint + 0.0, # left_ankle_roll_joint + -0.1, # right_hip_pitch_joint + 0.0, # right_hip_roll_joint + 0.0, # right_hip_yaw_joint + 0.3, # right_knee_joint + -0.2, # right_ankle_pitch_joint + 0.0, # right_ankle_roll_joint + 0.0, # waist_yaw_joint + 0.0, # waist_roll_joint + 0.0, # waist_pitch_joint + 0.0, # left_shoulder_pitch_joint + 0.0, # left_shoulder_roll_joint + 0.0, # left_shoulder_yaw_joint + 0.0, # left_elbow_joint + 0.0, # left_wrist_roll_joint + 0.0, # left_wrist_pitch_joint + 0.0, # left_wrist_yaw_joint + 0.0, # right_shoulder_pitch_joint + 0.0, # right_shoulder_roll_joint + 0.0, # right_shoulder_yaw_joint + 0.0, # right_elbow_joint + 0.0, # right_wrist_roll_joint + 0.0, # right_wrist_pitch_joint + 0.0 # right_wrist_yaw_joint +] + +motor_pos_lower_limit_list: [-2.5307, -0.5236, -2.7576, -0.087267, -0.87267, -0.2618, + -2.5307, -2.9671, -2.7576, -0.087267, -0.87267, -0.2618, + -2.618, -0.52, -0.52, + -3.0892, -1.5882, -2.618, -1.0472, + -1.972222054, -1.61443, -1.61443, + -3.0892, -2.2515, -2.618, -1.0472, + -1.972222054, -1.61443, -1.61443] +motor_pos_upper_limit_list: [2.8798, 2.9671, 2.7576, 2.8798, 0.5236, 0.2618, + 2.8798, 0.5236, 2.7576, 2.8798, 0.5236, 0.2618, + 2.618, 0.52, 0.52, + 2.6704, 2.2515, 2.618, 2.0944, + 1.972222054, 1.61443, 1.61443, + 2.6704, 1.5882, 2.618, 2.0944, + 1.972222054, 1.61443, 1.61443] +motor_vel_limit_list: [32.0, 32.0, 32.0, 20.0, 37.0, 37.0, + 32.0, 32.0, 32.0, 20.0, 37.0, 37.0, + 32.0, 37.0, 37.0, + 37.0, 37.0, 37.0, 37.0, + 37.0, 22.0, 22.0, + 37.0, 37.0, 37.0, 37.0, + 37.0, 22.0, 22.0] +motor_effort_limit_list: [88.0, 88.0, 88.0, 139.0, 50.0, 50.0, + 88.0, 88.0, 88.0, 139.0, 50.0, 50.0, + 88.0, 50.0, 50.0, + 25.0, 25.0, 25.0, 25.0, + 25.0, 5.0, 5.0, + 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, + 25.0, 25.0, 25.0, 25.0, + 25.0, 5.0, 5.0, + 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] +history_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + ref_motion_phase: 4, + sin_phase: 4, + cos_phase: 4 + } +history_loco_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + # command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + sin_phase: 4, + cos_phase: 4 + } +history_loco_height_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + sin_phase: 4, + cos_phase: 4 + } +history_mimic_config: { + base_ang_vel: 4, + projected_gravity: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + ref_motion_phase: 4, + } +obs_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + command_lin_vel: 2, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 1, + ref_upper_dof_pos: 17, # upper body actions + dof_pos: 29, + dof_vel: 29, + # actions: 12, # lower body actions + actions: 29, # full body actions + phase_time: 1, + ref_motion_phase: 1, # mimic motion phase + sin_phase: 1, + cos_phase: 1, + } +obs_loco_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + command_lin_vel: 2, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 1, + ref_upper_dof_pos: 17, # upper body actions + dof_pos: 29, + dof_vel: 29, + actions: 12, # lower body actions + phase_time: 1, + sin_phase: 1, + cos_phase: 1, + } +obs_mimic_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + dof_pos: 29, + dof_vel: 29, + actions: 29, # full body actions + ref_motion_phase: 1, # mimic motion phase + } +obs_scales: { + base_lin_vel: 2.0, + base_ang_vel: 0.25, + projected_gravity: 1.0, + command_lin_vel: 1, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 2, # Yuanhang: it's 2, not 1! + ref_upper_dof_pos: 1.0, + dof_pos: 1.0, + dof_vel: 0.05, + history: 1.0, + history_loco: 1.0, + history_mimic: 1.0, + actions: 1.0, + phase_time: 1.0, + ref_motion_phase: 1.0, + sin_phase: 1.0, + cos_phase: 1.0 + } + +loco_upper_body_dof_pos: [ + 0.0, 0.0, 0.0, # waist + 0.0, 0.3, 0.0, 1.0, # left shoulder and elbow + 0.0, 0.0, 0.0, # left wrist + 0.0, -0.3, 0.0, 1.0, # right shoulder and elbow + 0.0, 0.0, 0.0 # right wrist +] + +robot_dofs: { + "g1_29dof": [1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1], + "g1_29dof_anneal_23dof": [1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0], +} + +mimic_robot_types: { + + "APT_level1": "g1_29dof_anneal_23dof", +} + + + + +# 01281657 +mimic_models: { + "APT_level1": "20250116_225127-TairanTestbed_G129dofANNEAL23dof_dm_APT_video_APT_level1_MinimalFriction-0.3_RfiTrue_Far0.325_RESUME_LARGENOISE-motion_tracking-g1_29dof_anneal_23dof/exported/model_176500.onnx", + +} + + + +start_upper_body_dof_pos: { + + "APT_level1": + [0.19964170455932617, 0.07710712403059006, -0.2882401943206787, + 0.21672365069389343, 0.15629297494888306, -0.5167576670646667, 0.5782126784324646, + 0.0, 0.0, 0.0, + 0.25740593671798706, -0.2504104673862457, 0.22500675916671753, 0.5127624273300171, + 0.0, 0.0, 0.0], + +} + +motion_length_s: { + "APT_level1": 7.66, + +} diff --git a/gr00t_wbc/control/main/teleop/configs/g1_gear_wbc.yaml b/gr00t_wbc/control/main/teleop/configs/g1_gear_wbc.yaml new file mode 100644 index 0000000..4014fae --- /dev/null +++ b/gr00t_wbc/control/main/teleop/configs/g1_gear_wbc.yaml @@ -0,0 +1,45 @@ + +# Simulation parameters +simulation_duration: 60.0 +simulation_dt: 0.002 +control_decimation: 10 + +# PD gains +kps: [ + 150, 150, 150, 300, 40, 40, + 150, 150, 150, 300, 40, 40, + 250, 250, 250, + 100, 100, 40, 40, 20, 20, 20, + 100, 100, 40, 40, 20, 20, 20 +] +kds: [ + 2, 2, 2, 4, 2, 2, + 2, 2, 2, 4, 2, 2, + 5, 5, 5, + 5, 5, 2, 2, 2, 2, 2, + 5, 5, 2, 2, 2, 2, 2 +] + +# Default joint angles for legs +default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + -0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + 0.0, 0.0, 0.0] +# Scaling factors +ang_vel_scale: 0.25 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +action_scale: 0.25 +cmd_scale: [2.0, 2.0, 0.25] + +# Number of actions and observations +num_actions: 15 +num_obs: 570 # 76 * 6 (observation dimension * history length) +obs_history_len: 6 + +# Initial commands +cmd_init: [0.0, 0.0, 0.0] +height_cmd: 0.74 +freq_cmd: 1.50 +roll_cmd: 0.0 +pitch_cmd: 0.0 +yaw_cmd: 0.0 \ No newline at end of file diff --git a/gr00t_wbc/control/main/teleop/configs/identifiers.py b/gr00t_wbc/control/main/teleop/configs/identifiers.py new file mode 100644 index 0000000..0f9b3ad --- /dev/null +++ b/gr00t_wbc/control/main/teleop/configs/identifiers.py @@ -0,0 +1,14 @@ +""" +Constants required during data collection, such as operator usernames and robot IDs. +Additional constants needed for data collection can be added to this file. +""" + +OPERATOR_USERNAMES = [ + "NEW_USER", +] + +G1_ROBOT_IDS = [ + "sim", + "0001", + "0002", +] diff --git a/gr00t_wbc/control/main/teleop/playback_sync_sim_data.py b/gr00t_wbc/control/main/teleop/playback_sync_sim_data.py new file mode 100644 index 0000000..9afbb20 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/playback_sync_sim_data.py @@ -0,0 +1,627 @@ +""" +A convenience script to playback random demonstrations using the gr00t_wbc controller from +a set of demonstrations stored in a hdf5 file. + +Arguments: + --dataset (str): Path to demonstrations + --use-actions (optional): If this flag is provided, the actions are played back + through the MuJoCo simulator, instead of loading the simulator states + one by one. + --use-wbc-goals (optional): If set, will use the stored WBC goals to control the robot, + otherwise will use the actions directly. Only relevant if --use-actions is set. + --use-teleop-cmd (optional): If set, will use teleop IK directly with WBC timing + for action generation. Only relevant if --use-actions is set. + --visualize-gripper (optional): If set, will visualize the gripper site + --save-video (optional): If set, will save video of the playback using offscreen rendering + --video-path (optional): Path to save the output video. If not specified, will use the nearest + folder to dataset and save as playback_video.mp4 + --num-episodes (optional): Number of episodes to playback/record (if None, plays random episodes) + +Example: + $ python gr00t_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/ + --use-actions --use-wbc-goals + + $ python gr00t_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/ + --use-actions --use-teleop-cmd + + # Record video of the first 5 episodes using WBC goals + $ python gr00t_wbc/control/main/teleop/playback_sync_sim_data.py --dataset output/robocasa_datasets/ + --use-actions --use-wbc-goals --save-video --num-episodes 5 +""" + +import json +import os +from pathlib import Path +import time +from typing import Optional + +import cv2 +import numpy as np +import rclpy +from robosuite.environments.robot_env import RobotEnv +from tqdm import tqdm +import tyro + +from gr00t_wbc.control.main.teleop.configs.configs import SyncSimPlaybackConfig +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model +from gr00t_wbc.control.utils.sync_sim_utils import ( + generate_frame, + get_data_exporter, + get_env, + get_policies, +) +from gr00t_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH +from gr00t_wbc.data.exporter import TypedLeRobotDataset + +CONTROL_NODE_NAME = "playback_node" +GREEN_BOLD = "\033[1;32m" +RED_BOLD = "\033[1;31m" +RESET = "\033[0m" + + +def load_lerobot_dataset(root_path, max_episodes=None): + task_name = None + episodes = [] + start_index = 0 + with open(Path(root_path) / "meta/episodes.jsonl", "r") as f: + for line in f: + episode = json.loads(line) + episode["start_index"] = start_index + start_index += episode["length"] + assert ( + task_name is None or task_name == episode["tasks"][0] + ), "All episodes should have the same task name" + task_name = episode["tasks"][0] + episodes.append(episode) + + dataset = TypedLeRobotDataset( + repo_id="tmp/test", + root=root_path, + load_video=False, + ) + + script_config = dataset.meta.info["script_config"] + + assert len(dataset) == start_index, "Dataset length does not match expected length" + + # Limit episodes if specified + if max_episodes is not None: + episodes = episodes[:max_episodes] + print( + f"Loading only first {len(episodes)} episodes (limited by max_episodes={max_episodes})" + ) + + f = {} + seeds = [] + for ep in tqdm(range(len(episodes))): + seed = None + f[f"data/demo_{ep + 1}/states"] = [] + f[f"data/demo_{ep + 1}/actions"] = [] + f[f"data/demo_{ep + 1}/teleop_cmd"] = [] + f[f"data/demo_{ep + 1}/wbc_goal"] = [] + start_index = episodes[ep]["start_index"] + end_index = start_index + episodes[ep]["length"] + for i in tqdm(range(start_index, end_index)): + frame = dataset[i] + # load the seed + assert ( + seed is None or seed == np.array(frame["observation.sim.seed"]).item() + ), "All observations in an episode should have the same seed" + seed = np.array(frame["observation.sim.seed"]).item() + # load the state + mujoco_state_len = frame["observation.sim.mujoco_state_len"] + mujoco_state = frame["observation.sim.mujoco_state"] + f[f"data/demo_{ep + 1}/states"].append(np.array(mujoco_state[:mujoco_state_len])) + # load the action + action = frame["action"] + f[f"data/demo_{ep + 1}/actions"].append(np.array(action)) + + # load the teleop command + teleop_cmd = { + "left_wrist": np.array(frame["observation.sim.left_wrist"].reshape(4, 4)), + "right_wrist": np.array(frame["observation.sim.right_wrist"].reshape(4, 4)), + "left_fingers": { + "position": np.array(frame["observation.sim.left_fingers"].reshape(25, 4, 4)), + }, + "right_fingers": { + "position": np.array(frame["observation.sim.right_fingers"].reshape(25, 4, 4)), + }, + "target_upper_body_pose": np.array(frame["observation.sim.target_upper_body_pose"]), + "base_height_command": np.array(frame["teleop.base_height_command"]), + "navigate_cmd": np.array(frame["teleop.navigate_command"]), + } + f[f"data/demo_{ep + 1}/teleop_cmd"].append(teleop_cmd) + # load the WBC goal + wbc_goal = { + "wrist_pose": np.array(frame["action.eef"]), + "target_upper_body_pose": np.array(frame["observation.sim.target_upper_body_pose"]), + "navigate_cmd": np.array(frame["teleop.navigate_command"]), + "base_height_command": np.array(frame["teleop.base_height_command"]), + } + f[f"data/demo_{ep + 1}/wbc_goal"].append(wbc_goal) + + seeds.append(seed) + + return seeds, f, script_config + + +def validate_state(recorded_state, playback_state, ep, step, tolerance=1e-5): + """Validate that playback state matches recorded state within tolerance.""" + if not np.allclose(recorded_state, playback_state, atol=tolerance): + err = np.linalg.norm(recorded_state - playback_state) + print(f"[warning] state diverged by {err:.12f} for ep {ep} at step {step}") + return False + return True + + +def generate_and_save_frame( + config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter +): + """Generate and save a frame to LeRobot dataset if enabled.""" + if config.save_lerobot: + max_mujoco_state_len, mujoco_state_len, mujoco_state = sync_env.get_mujoco_state_info() + frame = generate_frame( + obs, + wbc_action, + seed, + mujoco_state, + mujoco_state_len, + max_mujoco_state_len, + teleop_cmd, + wbc_goal, + config.save_img_obs, + ) + gr00t_exporter.add_frame(frame) + + +def playback_wbc_goals( + sync_env, + wbc_policy, + wbc_goals, + teleop_cmds, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, +): + """Playback using WBC goals to control the robot.""" + ret = True + num_wbc_goals = len(wbc_goals) if end_steps == -1 else min(end_steps, len(wbc_goals)) + + for jj in range(num_wbc_goals): + wbc_goal = wbc_goals[jj] + obs = sync_env.observe() + wbc_policy.set_observation(obs) + wbc_policy.set_goal(wbc_goal) + wbc_action = wbc_policy.get_action() + sync_env.queue_action(wbc_action) + + # Save frame if needed + if config.save_lerobot: + teleop_cmd = teleop_cmds[jj] + generate_and_save_frame( + config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter + ) + + capture_or_render_frame(env, onscreen, config, video_writer) + + if jj < len(states) - 1: + state_playback = env.sim.get_state().flatten() + if not validate_state(states[jj + 1], state_playback, ep, jj): + ret = False + + return ret + + +def playback_teleop_cmd( + sync_env, + wbc_policy, + teleop_policy, + wbc_goals, + teleop_cmds, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, +): + """Playback using teleop commands to control the robot.""" + ret = True + num_steps = len(wbc_goals) if end_steps == -1 else min(end_steps, len(wbc_goals)) + + for jj in range(num_steps): + wbc_goal = wbc_goals[jj] + teleop_cmd = teleop_cmds[jj] + + # Set IK goal from teleop command + ik_data = { + "body_data": { + teleop_policy.retargeting_ik.body.supplemental_info.hand_frame_names[ + "left" + ]: teleop_cmd["left_wrist"], + teleop_policy.retargeting_ik.body.supplemental_info.hand_frame_names[ + "right" + ]: teleop_cmd["right_wrist"], + }, + "left_hand_data": teleop_cmd["left_fingers"], + "right_hand_data": teleop_cmd["right_fingers"], + } + teleop_policy.retargeting_ik.set_goal(ik_data) + + # Store original and get new upper body pose + target_upper_body_pose = wbc_goal["target_upper_body_pose"].copy() + wbc_goal["target_upper_body_pose"] = teleop_policy.retargeting_ik.get_action() + + # Execute WBC policy + obs = sync_env.observe() + wbc_policy.set_observation(obs) + wbc_policy.set_goal(wbc_goal) + wbc_action = wbc_policy.get_action() + sync_env.queue_action(wbc_action) + + # Save frame if needed + generate_and_save_frame( + config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter + ) + + # Render or capture frame + capture_or_render_frame(env, onscreen, config, video_writer) + + # Validate states + if jj < len(states) - 1: + if not np.allclose( + target_upper_body_pose, wbc_goal["target_upper_body_pose"], atol=1e-5 + ): + err = np.linalg.norm(target_upper_body_pose - wbc_goal["target_upper_body_pose"]) + print( + f"[warning] target_upper_body_pose diverged by {err:.12f} for ep {ep} at step {jj}" + ) + ret = False + + state_playback = env.sim.get_state().flatten() + if not validate_state(states[jj + 1], state_playback, ep, jj): + ret = False + + return ret + + +def playback_actions( + sync_env, + actions, + teleop_cmds, + wbc_goals, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, +): + """Playback using actions directly.""" + ret = True + num_actions = len(actions) if end_steps == -1 else min(end_steps, len(actions)) + + for j in range(num_actions): + sync_env.queue_action({"q": actions[j]}) + + # Save frame if needed + if config.save_lerobot: + obs = sync_env.observe() + teleop_cmd = teleop_cmds[j] + wbc_goal = wbc_goals[j] + wbc_action = {"q": actions[j]} + generate_and_save_frame( + config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter + ) + + capture_or_render_frame(env, onscreen, config, video_writer) + + if j < len(states) - 1: + state_playback = env.sim.get_state().flatten() + if not validate_state(states[j + 1], state_playback, ep, j): + ret = False + + return ret + + +def playback_states( + sync_env, + states, + actions, + teleop_cmds, + wbc_goals, + env, + onscreen, + config, + video_writer, + seed, + gr00t_exporter, + end_steps, + ep, +): + """Playback by forcing mujoco states directly.""" + ret = True + num_states = len(states) if end_steps == -1 else min(end_steps, len(states)) + + for i in range(num_states): + sync_env.reset_to({"states": states[i]}) + sync_env.render() + + # Validate that the state was set correctly + if i < len(states): + state_playback = env.sim.get_state().flatten() + if not validate_state(states[i], state_playback, ep, i): + ret = False + + # Save frame if needed + if config.save_lerobot: + obs = sync_env.observe() + teleop_cmd = teleop_cmds[i] + wbc_goal = wbc_goals[i] + wbc_action = {"q": actions[i]} + generate_and_save_frame( + config, sync_env, obs, wbc_action, seed, teleop_cmd, wbc_goal, gr00t_exporter + ) + + capture_or_render_frame(env, onscreen, config, video_writer) + + return ret + + +def main(config: SyncSimPlaybackConfig): + ret = True + start_time = time.time() + + np.set_printoptions(precision=5, suppress=True, linewidth=120) + + assert config.dataset is not None, "Folder must be specified for playback" + + seeds, f, script_config = load_lerobot_dataset(config.dataset) + + config.update( + script_config, + allowed_keys=[ + "wbc_version", + "wbc_model_path", + "wbc_policy_class", + "control_frequency", + "enable_waist", + "with_hands", + "env_name", + "robot", + "task_name", + "teleop_frequency", + "data_collection_frequency", + "enable_gravity_compensation", + "gravity_compensation_joints", + ], + ) + config.validate_args() + + robot_type, robot_model = get_robot_type_and_model(config.robot, config.enable_waist) + + # Setup rendering + if config.save_video or config.save_img_obs: + onscreen = False + offscreen = True + else: + onscreen = True + offscreen = False + + # Set default video path if not specified + if config.save_video and config.video_path is None: + if os.path.isfile(config.dataset): + video_folder = Path(config.dataset).parent + else: + video_folder = Path(config.dataset) + video_folder.mkdir(parents=True, exist_ok=True) + config.video_path = str(video_folder / "playback_video.mp4") + print(f"Video recording enabled. Output: {config.video_path}") + + sync_env = get_env(config, onscreen=onscreen, offscreen=offscreen) + + gr00t_exporter = None + if config.save_lerobot: + obs = sync_env.observe() + gr00t_exporter = get_data_exporter(config, obs, robot_model) + + # Initialize policies + wbc_policy, teleop_policy = get_policies( + config, robot_type, robot_model, activate_keyboard_listener=False + ) + + # List of all demonstrations episodes + demos = [f"demo_{i + 1}" for i in range(len(seeds))] + print(f"Loaded and will playback {len(demos)} episodes") + env = sync_env.base_env + + # Setup video writer + video_writer = None + fourcc = None + if config.save_video: + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + video_writer = cv2.VideoWriter( + config.video_path, fourcc, 20, (RS_VIEW_CAMERA_WIDTH, RS_VIEW_CAMERA_HEIGHT) + ) + + print("Loaded {} episodes from {}".format(len(demos), config.dataset)) + print("seeds:", seeds) + print("demos:", demos, "\n\n") + + # Handle episode selection - either limited number or infinite random + max_episodes = len(demos) + episode_count = 0 + while True: + if episode_count >= max_episodes: + break + ep = demos[episode_count] + print(f"Playing back episode: {ep}") + episode_count += 1 + + # read the model xml, using the metadata stored in the attribute for this episode + seed = seeds[int(ep.split("_")[-1]) - 1] + sync_env.reset(seed=seed) + + # load the actions and states + states = f["data/{}/states".format(ep)] + actions = f["data/{}/actions".format(ep)] + teleop_cmds = f["data/{}/teleop_cmd".format(ep)] + wbc_goals = f["data/{}/wbc_goal".format(ep)] + + # reset the policies + wbc_policy, teleop_policy, _ = get_policies( + config, robot_type, robot_model, activate_keyboard_listener=False + ) + end_steps = 20 if config.ci_test else -1 + + if config.use_actions: + # load the initial state + sync_env.reset_to({"states": states[0]}) + # load the actions and play them back open-loop + if config.use_wbc_goals: + # use the wbc_goals to control the robot + episode_ret = playback_wbc_goals( + sync_env, + wbc_policy, + wbc_goals, + teleop_cmds, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, + ) + ret = ret and episode_ret + elif config.use_teleop_cmd: + # use the teleop commands to control the robot + episode_ret = playback_teleop_cmd( + sync_env, + wbc_policy, + teleop_policy, + wbc_goals, + teleop_cmds, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, + ) + ret = ret and episode_ret + else: + episode_ret = playback_actions( + sync_env, + actions, + teleop_cmds, + wbc_goals, + states, + env, + onscreen, + config, + video_writer, + ep, + seed, + gr00t_exporter, + end_steps, + ) + ret = ret and episode_ret + else: + # force the sequence of internal mujoco states one by one + episode_ret = playback_states( + sync_env, + states, + actions, + teleop_cmds, + wbc_goals, + env, + onscreen, + config, + video_writer, + seed, + gr00t_exporter, + end_steps, + ep, + ) + ret = ret and episode_ret + + if config.save_lerobot: + gr00t_exporter.save_episode() + + print(f"Episode {ep} playback finished.\n\n") + + # close the env + sync_env.close() + + # Cleanup + if video_writer is not None: + video_writer.release() + print(f"Video saved to: {config.video_path}") + + end_time = time.time() + elapsed_time = end_time - start_time + + if config.save_lerobot: + print(f"LeRobot dataset saved to: {gr00t_exporter.root}") + + print( + f"{GREEN_BOLD}Playback with WBC version: {config.wbc_version}, {config.wbc_model_path}, " + f"{config.wbc_policy_class}, use_actions: {config.use_actions}, use_wbc_goals: {config.use_wbc_goals}, " + f"use_teleop_cmd: {config.use_teleop_cmd}{RESET}" + ) + if ret: + print(f"{GREEN_BOLD}Playback completed successfully in {elapsed_time:.2f} seconds!{RESET}") + else: + print(f"{RED_BOLD}Playback encountered an error in {elapsed_time:.2f} seconds!{RESET}") + + return ret + + +def capture_or_render_frame( + env: RobotEnv, + onscreen: bool, + config: SyncSimPlaybackConfig, + video_writer: Optional[cv2.VideoWriter], +): + """Capture frame for video recording if enabled, or render the environment.""" + if config.save_video: + if hasattr(env, "sim") and hasattr(env.sim, "render"): + img = env.sim.render( + width=RS_VIEW_CAMERA_WIDTH, + height=RS_VIEW_CAMERA_HEIGHT, + camera_name=env.render_camera[0], + ) + img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + img_bgr = np.flipud(img_bgr) + video_writer.write(img_bgr) + elif onscreen: + env.render() + + +if __name__ == "__main__": + config = tyro.cli(SyncSimPlaybackConfig) + + rclpy.init(args=None) + node = rclpy.create_node("playback_gr00t_wbc_control") + + main(config) + + rclpy.shutdown() diff --git a/gr00t_wbc/control/main/teleop/run_camera_viewer.py b/gr00t_wbc/control/main/teleop/run_camera_viewer.py new file mode 100644 index 0000000..298bebb --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_camera_viewer.py @@ -0,0 +1,249 @@ +""" +Camera viewer with manual recording support. + +This script provides a camera viewer that can display multiple camera streams +and record them to video files with manual start/stop controls. + +Features: +- Onscreen mode: Display camera feeds with optional recording +- Offscreen mode: No display, recording only when triggered +- Manual recording control with keyboard (R key to start/stop) + +Usage Examples: + +1. Basic onscreen viewing (with recording capability): + python run_camera_viewer.py --camera-host localhost --camera-port 5555 + +2. Offscreen mode (no display, recording only): + python run_camera_viewer.py --offscreen --camera-host localhost --camera-port 5555 + +3. Custom output directory: + python run_camera_viewer.py --output-path ./my_recordings --camera-host localhost + +Controls: +- R key: Start/Stop recording +- Q key: Quit application + +Output Structure: +camera_output_20241211_143052/ +├── rec_143205/ +│ ├── ego_view_color_image.mp4 +│ ├── head_left_color_image.mp4 +│ └── head_right_color_image.mp4 +└── rec_143410/ + ├── ego_view_color_image.mp4 + └── head_left_color_image.mp4 +""" + +from dataclasses import dataclass +from pathlib import Path +import threading +import time +from typing import Any, Optional + +import cv2 +import rclpy +from sshkeyboard import listen_keyboard, stop_listening +import tyro + +from gr00t_wbc.control.main.teleop.configs.configs import ComposedCameraClientConfig +from gr00t_wbc.control.sensor.composed_camera import ComposedCameraClientSensor +from gr00t_wbc.control.utils.img_viewer import ImageViewer + + +@dataclass +class CameraViewerConfig(ComposedCameraClientConfig): + """Config for running the camera viewer with recording support.""" + + offscreen: bool = False + """Run in offscreen mode (no display, manual recording with R key).""" + + output_path: Optional[str] = None + """Output path for saving videos. If None, auto-generates path.""" + + codec: str = "mp4v" + """Video codec to use for saving (e.g., 'mp4v', 'XVID').""" + + +ArgsConfig = CameraViewerConfig + + +def _get_camera_titles(image_data: dict[str, Any]) -> list[str]: + """ + Detect all the individual camera streams from the image data. + + schema format: + { + "timestamps": {"ego_view": 123.45, "ego_view_left_mono": 123.46}, + "images": {"ego_view": np.ndarray, "ego_view_left_mono": np.ndarray} + } + + Returns list of camera keys (e.g., ["ego_view", "ego_view_left_mono", "ego_view_right_mono"]) + """ + # Extract all camera keys from the images dictionary + camera_titles = list(image_data.get("images", {}).keys()) + return camera_titles + + +def main(config: ArgsConfig): + """Main function to run the camera viewer.""" + # Initialize ROS + rclpy.init(args=None) + node = rclpy.create_node("camera_viewer") + + # Start ROS spin in a separate thread + thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + thread.start() + + image_sub = ComposedCameraClientSensor(server_ip=config.camera_host, port=config.camera_port) + + # pre-fetch a sample image to get the number of camera angles + retry_count = 0 + while True: + _sample_image = image_sub.read() + if _sample_image: + break + retry_count += 1 + time.sleep(0.1) + if retry_count > 10: + raise Exception("Failed to get sample image") + + camera_titles = _get_camera_titles(_sample_image) + + # Setup output directory + if config.output_path is None: + output_dir = Path("camera_recordings") + else: + output_dir = Path(config.output_path) + + # Recording state + is_recording = False + video_writers = {} + frame_count = 0 + recording_start_time = None + should_quit = False + + def on_press(key): + nonlocal is_recording, video_writers, frame_count, recording_start_time, should_quit + + if key == "r": + if not is_recording: + # Start recording + recording_dir = output_dir / f"rec_{time.strftime('%Y%m%d_%H%M%S')}" + recording_dir.mkdir(parents=True, exist_ok=True) + + # Create video writers + fourcc = cv2.VideoWriter_fourcc(*config.codec) + video_writers = {} + + for title in camera_titles: + img = _sample_image["images"].get(title) + if img is not None: + height, width = img.shape[:2] + video_path = recording_dir / f"{title}.mp4" + writer = cv2.VideoWriter( + str(video_path), fourcc, config.fps, (width, height) + ) + video_writers[title] = writer + + is_recording = True + recording_start_time = time.time() + frame_count = 0 + print(f"🔴 Recording started: {recording_dir}") + else: + # Stop recording + is_recording = False + for title, writer in video_writers.items(): + writer.release() + video_writers = {} + + duration = time.time() - recording_start_time if recording_start_time else 0 + print(f"⏹️ Recording stopped - {duration:.1f}s, {frame_count} frames") + elif key == "q": + should_quit = True + stop_listening() + + # Setup keyboard listener in a separate thread + keyboard_thread = threading.Thread( + target=lambda: listen_keyboard(on_press=on_press), daemon=True + ) + keyboard_thread.start() + + # Setup viewer for onscreen mode + viewer = None + if not config.offscreen: + viewer = ImageViewer( + title="Camera Viewer", + figsize=(10, 8), + num_images=len(camera_titles), + image_titles=camera_titles, + ) + + # Print instructions + mode = "Offscreen" if config.offscreen else "Onscreen" + print(f"{mode} mode - Target FPS: {config.fps}") + print(f"Videos will be saved to: {output_dir}") + print("Controls: R key to start/stop recording, Q key to quit, Ctrl+C to exit") + + # Create ROS rate controller + rate = node.create_rate(config.fps) + + try: + while rclpy.ok() and not should_quit: + # Get images from all subscribers + images = [] + image_data = image_sub.read() + if image_data: + for title in camera_titles: + img = image_data["images"].get(title) + images.append(img) + + # Save frame if recording + if is_recording and img is not None and title in video_writers: + # Convert from RGB to BGR for OpenCV + if len(img.shape) == 3 and img.shape[2] == 3: + img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + else: + img_bgr = img + video_writers[title].write(img_bgr) + + # Display images if not offscreen + if not config.offscreen and viewer and any(img is not None for img in images): + status = "🔴 REC" if is_recording else "⏸️ Ready" + viewer._fig.suptitle(f"Camera Viewer - {status}") + viewer.show_multiple(images) + + # Progress feedback + if is_recording: + frame_count += 1 + if frame_count % 100 == 0: + duration = time.time() - recording_start_time + print(f"Recording: {frame_count} frames ({duration:.1f}s)") + + rate.sleep() + + except KeyboardInterrupt: + print("\nExiting...") + finally: + # Cleanup + try: + stop_listening() + except Exception: + pass + + if video_writers: + for title, writer in video_writers.items(): + writer.release() + if is_recording: + duration = time.time() - recording_start_time + print(f"Final: {duration:.1f}s, {frame_count} frames") + + if viewer: + viewer.close() + + rclpy.shutdown() + + +if __name__ == "__main__": + config = tyro.cli(ArgsConfig) + main(config) diff --git a/gr00t_wbc/control/main/teleop/run_g1_control_loop.py b/gr00t_wbc/control/main/teleop/run_g1_control_loop.py new file mode 100644 index 0000000..17cf2b1 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_g1_control_loop.py @@ -0,0 +1,236 @@ +from copy import deepcopy +import time + +import tyro + +from gr00t_wbc.control.envs.g1.g1_env import G1Env +from gr00t_wbc.control.main.constants import ( + CONTROL_GOAL_TOPIC, + DEFAULT_BASE_HEIGHT, + DEFAULT_NAV_CMD, + DEFAULT_WRIST_POSE, + JOINT_SAFETY_STATUS_TOPIC, + LOWER_BODY_POLICY_STATUS_TOPIC, + ROBOT_CONFIG_TOPIC, + STATE_TOPIC_NAME, +) +from gr00t_wbc.control.main.teleop.configs.configs import ControlLoopConfig +from gr00t_wbc.control.policy.wbc_policy_factory import get_wbc_policy +from gr00t_wbc.control.robot_model.instantiation.g1 import ( + instantiate_g1_robot_model, +) +from gr00t_wbc.control.utils.keyboard_dispatcher import ( + KeyboardDispatcher, + KeyboardEStop, + KeyboardListenerPublisher, + ROSKeyboardDispatcher, +) +from gr00t_wbc.control.utils.ros_utils import ( + ROSManager, + ROSMsgPublisher, + ROSMsgSubscriber, + ROSServiceServer, +) +from gr00t_wbc.control.utils.telemetry import Telemetry + +CONTROL_NODE_NAME = "ControlPolicy" + + +def main(config: ControlLoopConfig): + ros_manager = ROSManager(node_name=CONTROL_NODE_NAME) + node = ros_manager.node + + # start the robot config server + ROSServiceServer(ROBOT_CONFIG_TOPIC, config.to_dict()) + + wbc_config = config.load_wbc_yaml() + + data_exp_pub = ROSMsgPublisher(STATE_TOPIC_NAME) + lower_body_policy_status_pub = ROSMsgPublisher(LOWER_BODY_POLICY_STATUS_TOPIC) + joint_safety_status_pub = ROSMsgPublisher(JOINT_SAFETY_STATUS_TOPIC) + + # Initialize telemetry + telemetry = Telemetry(window_size=100) + + waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body" + robot_model = instantiate_g1_robot_model( + waist_location=waist_location, high_elbow_pose=config.high_elbow_pose + ) + + env = G1Env( + env_name=config.env_name, + robot_model=robot_model, + config=wbc_config, + wbc_version=config.wbc_version, + ) + if env.sim and not config.sim_sync_mode: + env.start_simulator() + + wbc_policy = get_wbc_policy("g1", robot_model, wbc_config, config.upper_body_joint_speed) + + keyboard_listener_pub = KeyboardListenerPublisher() + keyboard_estop = KeyboardEStop() + if config.keyboard_dispatcher_type == "raw": + dispatcher = KeyboardDispatcher() + elif config.keyboard_dispatcher_type == "ros": + dispatcher = ROSKeyboardDispatcher() + else: + raise ValueError( + f"Invalid keyboard dispatcher: {config.keyboard_dispatcher_type}, please use 'raw' or 'ros'" + ) + dispatcher.register(env) + dispatcher.register(wbc_policy) + dispatcher.register(keyboard_listener_pub) + dispatcher.register(keyboard_estop) + dispatcher.start() + + rate = node.create_rate(config.control_frequency) + + upper_body_policy_subscriber = ROSMsgSubscriber(CONTROL_GOAL_TOPIC) + + last_teleop_cmd = None + try: + while ros_manager.ok(): + t_start = time.monotonic() + with telemetry.timer("total_loop"): + # Step simulator if in sync mode + with telemetry.timer("step_simulator"): + if env.sim and config.sim_sync_mode: + env.step_simulator() + + # Measure observation time + with telemetry.timer("observe"): + obs = env.observe() + wbc_policy.set_observation(obs) + + # Measure policy setup time + with telemetry.timer("policy_setup"): + upper_body_cmd = upper_body_policy_subscriber.get_msg() + + t_now = time.monotonic() + + wbc_goal = {} + if upper_body_cmd: + wbc_goal = upper_body_cmd.copy() + last_teleop_cmd = upper_body_cmd.copy() + if config.ik_indicator: + env.set_ik_indicator(upper_body_cmd) + # Send goal to policy + if wbc_goal: + wbc_goal["interpolation_garbage_collection_time"] = t_now - 2 * ( + 1 / config.control_frequency + ) + wbc_policy.set_goal(wbc_goal) + + # Measure policy action calculation time + with telemetry.timer("policy_action"): + wbc_action = wbc_policy.get_action(time=t_now) + + # Measure action queue time + with telemetry.timer("queue_action"): + env.queue_action(wbc_action) + + # Publish status information for InteractiveModeController + with telemetry.timer("publish_status"): + # Get policy status - check if the lower body policy has use_policy_action enabled + policy_use_action = False + try: + # Access the lower body policy through the decoupled whole body policy + if hasattr(wbc_policy, "lower_body_policy"): + policy_use_action = getattr( + wbc_policy.lower_body_policy, "use_policy_action", False + ) + except (AttributeError, TypeError): + policy_use_action = False + + policy_status_msg = {"use_policy_action": policy_use_action, "timestamp": t_now} + lower_body_policy_status_pub.publish(policy_status_msg) + + # Get joint safety status from G1Env (which already runs the safety monitor) + joint_safety_ok = env.get_joint_safety_status() + + joint_safety_status_msg = { + "joint_safety_ok": joint_safety_ok, + "timestamp": t_now, + } + joint_safety_status_pub.publish(joint_safety_status_msg) + + # Start or Stop data collection + if wbc_goal.get("toggle_data_collection", False): + dispatcher.handle_key("c") + + # Abort the current episode + if wbc_goal.get("toggle_data_abort", False): + dispatcher.handle_key("x") + + if env.use_sim and wbc_goal.get("reset_env_and_policy", False): + print("Resetting sim environment and policy") + # Reset teleop policy & sim env + dispatcher.handle_key("k") + + # Clear upper body commands + upper_body_policy_subscriber._msg = None + upper_body_cmd = { + "target_upper_body_pose": obs["q"][ + robot_model.get_joint_group_indices("upper_body") + ], + "wrist_pose": DEFAULT_WRIST_POSE, + "base_height_command": DEFAULT_BASE_HEIGHT, + "navigate_cmd": DEFAULT_NAV_CMD, + } + last_teleop_cmd = upper_body_cmd.copy() + + time.sleep(0.5) + + msg = deepcopy(obs) + for key in obs.keys(): + if key.endswith("_image"): + del msg[key] + + # exporting data + if last_teleop_cmd: + msg.update( + { + "action": wbc_action["q"], + "action.eef": last_teleop_cmd.get("wrist_pose", DEFAULT_WRIST_POSE), + "base_height_command": last_teleop_cmd.get( + "base_height_command", DEFAULT_BASE_HEIGHT + ), + "navigate_command": last_teleop_cmd.get( + "navigate_cmd", DEFAULT_NAV_CMD + ), + "timestamps": { + "main_loop": time.time(), + "proprio": time.time(), + }, + } + ) + data_exp_pub.publish(msg) + end_time = time.monotonic() + + if env.sim and (not env.sim.sim_thread or not env.sim.sim_thread.is_alive()): + raise RuntimeError("Simulator thread is not alive") + + rate.sleep() + + # Log timing information every 100 iterations (roughly every 2 seconds at 50Hz) + if config.verbose_timing: + # When verbose timing is enabled, always show timing + telemetry.log_timing_info(context="G1 Control Loop", threshold=0.0) + elif (end_time - t_start) > (1 / config.control_frequency) and not config.sim_sync_mode: + # Only show timing when loop is slow and verbose_timing is disabled + telemetry.log_timing_info(context="G1 Control Loop Missed", threshold=0.001) + + except ros_manager.exceptions() as e: + print(f"ROSManager interrupted by user: {e}") + finally: + print("Cleaning up...") + # the order of the following is important + dispatcher.stop() + ros_manager.shutdown() + env.close() + + +if __name__ == "__main__": + config = tyro.cli(ControlLoopConfig) + main(config) diff --git a/gr00t_wbc/control/main/teleop/run_g1_data_exporter.py b/gr00t_wbc/control/main/teleop/run_g1_data_exporter.py new file mode 100644 index 0000000..5bd95d9 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_g1_data_exporter.py @@ -0,0 +1,364 @@ +from collections import deque +from datetime import datetime +import threading +import time + +import numpy as np +import rclpy +import tyro + +from gr00t_wbc.control.main.constants import ROBOT_CONFIG_TOPIC, STATE_TOPIC_NAME +from gr00t_wbc.control.main.teleop.configs.configs import DataExporterConfig +from gr00t_wbc.control.robot_model.instantiation import g1 +from gr00t_wbc.control.sensor.composed_camera import ComposedCameraClientSensor +from gr00t_wbc.control.utils.episode_state import EpisodeState +from gr00t_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber +from gr00t_wbc.control.utils.ros_utils import ROSMsgSubscriber, ROSServiceClient +from gr00t_wbc.control.utils.telemetry import Telemetry +from gr00t_wbc.control.utils.text_to_speech import TextToSpeech +from gr00t_wbc.data.constants import BUCKET_BASE_PATH +from gr00t_wbc.data.exporter import DataCollectionInfo, Gr00tDataExporter +from gr00t_wbc.data.utils import get_dataset_features, get_modality_config + + +class TimeDeltaException(Exception): + def __init__(self, failure_count: int, reset_timeout_sec: float): + """ + Exception raised when the time delta between two messages exceeds + a threshold for a consecutive number of times + """ + self.failure_count = failure_count + self.reset_timeout_sec = reset_timeout_sec + self.message = f"{self.failure_count} failures in {self.reset_timeout_sec} seconds" + super().__init__(self.message) + + +class TimingThresholdMonitor: + def __init__(self, max_failures=3, reset_timeout_sec=5, time_delta=0.2, raise_exception=False): + """ + Monitor the time diff (between two messages) and optionally raise an exception + if there is a consistent violations + """ + self.max_failures = max_failures + self.reset_timeout_sec = reset_timeout_sec + self.failure_count = 0 + self.last_failure_time = 0 + self.time_delta = time_delta + self.raise_exception = raise_exception + + def reset(self): + self.failure_count = 0 + self.last_failure_time = 0 + + def log_time_delta(self, time_delta_sec: float): + time_delta = abs(time_delta_sec) + if time_delta > self.time_delta: + self.failure_count += 1 + self.last_failure_time = time.monotonic() + + if self.is_threshold_exceeded(): + print( + f"Time delta exception: {self.failure_count} failures in {self.reset_timeout_sec} seconds" + f", time delta: {time_delta}" + ) + if self.raise_exception: + raise TimeDeltaException(self.failure_count, self.reset_timeout_sec) + + def is_threshold_exceeded(self): + if self.failure_count >= self.max_failures: + return True + if time.monotonic() - self.last_failure_time > self.reset_timeout_sec: + self.reset() + return False + + +class Gr00tDataCollector: + def __init__( + self, + node, + camera_host: str, + camera_port: int, + state_topic_name: str, + data_exporter: Gr00tDataExporter, + text_to_speech=None, + frequency=20, + state_act_msg_frequency=50, + ): + + self.text_to_speech = text_to_speech + self.frequency = frequency + self.data_exporter = data_exporter + + self.node = node + + thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) + thread.start() + time.sleep(0.5) + + self._episode_state = EpisodeState() + self._keyboard_listener = KeyboardListenerSubscriber() + self._state_subscriber = ROSMsgSubscriber(state_topic_name) + self._image_subscriber = ComposedCameraClientSensor(server_ip=camera_host, port=camera_port) + self.rate = self.node.create_rate(self.frequency) + + self.obs_act_buffer = deque(maxlen=100) + self.latest_image_msg = None + self.latest_proprio_msg = None + + self.state_polling_rate = 1 / state_act_msg_frequency + self.last_state_poll_time = time.monotonic() + + self.telemetry = Telemetry(window_size=100) + self.timing_threshold_monitor = TimingThresholdMonitor() + + print(f"Recording to {self.data_exporter.meta.root}") + + @property + def current_episode_index(self): + return self.data_exporter.episode_buffer["episode_index"] + + def _print_and_say(self, message: str, say: bool = True): + """Helper to use TextToSpeech print_and_say or fallback to print.""" + if self.text_to_speech is not None: + self.text_to_speech.print_and_say(message, say) + else: + print(message) + + def _check_keyboard_input(self): + key = self._keyboard_listener.read_msg() + if key == "c": + self._episode_state.change_state() + if self._episode_state.get_state() == self._episode_state.RECORDING: + self._print_and_say(f"Started recording {self.current_episode_index}") + elif self._episode_state.get_state() == self._episode_state.NEED_TO_SAVE: + self._print_and_say("Stopping recording, preparing to save") + elif self._episode_state.get_state() == self._episode_state.IDLE: + self._print_and_say("Saved episode and back to idle state") + elif key == "x": + if self._episode_state.get_state() == self._episode_state.RECORDING: + self.data_exporter.save_episode_as_discarded() + self._episode_state.reset_state() + self._print_and_say("Discarded episode") + + def _add_data_frame(self): + t_start = time.monotonic() + + if self.latest_proprio_msg is None or self.latest_image_msg is None: + self._print_and_say( + f"Waiting for message. " + f"Avail msg: proprio {self.latest_proprio_msg is not None} | " + f"image {self.latest_image_msg is not None}", + say=False, + ) + return False + + if self._episode_state.get_state() == self._episode_state.RECORDING: + + # Calculate max time delta between images and proprio + max_time_delta = 0 + for _, image_time in self.latest_image_msg["timestamps"].items(): + time_delta = abs(image_time - self.latest_proprio_msg["timestamps"]["proprio"]) + max_time_delta = max(max_time_delta, time_delta) + + self.timing_threshold_monitor.log_time_delta(max_time_delta) + if (self.timing_threshold_monitor.failure_count + 1) % 100 == 0: + self._print_and_say("Image state delta too high, please discard data") + + frame_data = { + "observation.state": self.latest_proprio_msg["q"], + "observation.eef_state": self.latest_proprio_msg["wrist_pose"], + "action": self.latest_proprio_msg["action"], + "action.eef": self.latest_proprio_msg["action.eef"], + "observation.img_state_delta": ( + np.array( + [max_time_delta], + dtype=np.float32, + ) + ), # lerobot only supports adding numpy arrays + "teleop.navigate_command": np.array( + self.latest_proprio_msg["navigate_command"], dtype=np.float64 + ), + "teleop.base_height_command": np.array( + [self.latest_proprio_msg["base_height_command"]], dtype=np.float64 + ), + } + + # Add images based on dataset features + images = self.latest_image_msg["images"] + for feature_name, feature_info in self.data_exporter.features.items(): + if feature_info.get("dtype") in ["image", "video"]: + # Extract image key from feature name (e.g., "observation.images.ego_view" -> "ego_view") + image_key = feature_name.split(".")[-1] + + if image_key not in images: + raise ValueError( + f"Required image '{image_key}' for feature '{feature_name}' " + f"not found in image message. Available images: {list(images.keys())}" + ) + frame_data[feature_name] = images[image_key] + + self.data_exporter.add_frame(frame_data) + + t_end = time.monotonic() + if t_end - t_start > (1 / self.frequency): + print(f"DataExporter Missed: {t_end - t_start} sec") + + if self._episode_state.get_state() == self._episode_state.NEED_TO_SAVE: + self.data_exporter.save_episode() + self.timing_threshold_monitor.reset() + self._print_and_say("Finished saving episode") + self._episode_state.change_state() + + return True + + def save_and_cleanup(self): + try: + self._print_and_say("saving episode done") + # save on going episode if any + buffer_size = self.data_exporter.episode_buffer.get("size", 0) + if buffer_size > 0: + self.data_exporter.save_episode() + self._print_and_say(f"Recording complete: {self.data_exporter.meta.root}", say=False) + except Exception as e: + self._print_and_say(f"Error saving episode: {e}") + + self.node.destroy_node() + rclpy.shutdown() + self._print_and_say("Shutting down data exporter...", say=False) + + def run(self): + try: + while rclpy.ok(): + t_start = time.monotonic() + with self.telemetry.timer("total_loop"): + # 1. poll proprio msg + with self.telemetry.timer("poll_state"): + msg = self._state_subscriber.get_msg() + if msg is not None: + self.latest_proprio_msg = msg + + # 2. poll image msg + with self.telemetry.timer("poll_image"): + msg = self._image_subscriber.read() + if msg is not None: + self.latest_image_msg = msg + + # 3. check keyboard input + with self.telemetry.timer("check_keyboard"): + self._check_keyboard_input() + + # 4. add frame + with self.telemetry.timer("add_frame"): + self._add_data_frame() + + end_time = time.monotonic() + + self.rate.sleep() + + # Log timing information if we missed our target frequency + if (end_time - t_start) > (1 / self.frequency): + self.telemetry.log_timing_info( + context="Data Exporter Loop Missed", threshold=0.001 + ) + + except KeyboardInterrupt: + print("Data exporter terminated by user") + # The user will trigger a keyboard interrupt if there's something wrong, + # so we flag the ongoing episode as discarded + buffer_size = self.data_exporter.episode_buffer.get("size", 0) + if buffer_size > 0: + self.data_exporter.save_episode_as_discarded() + + finally: + self.save_and_cleanup() + + +def main(config: DataExporterConfig): + + rclpy.init(args=None) + node = rclpy.create_node("data_exporter") + + waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body" + g1_rm = g1.instantiate_g1_robot_model( + waist_location=waist_location, high_elbow_pose=config.high_elbow_pose + ) + + dataset_features = get_dataset_features(g1_rm, config.add_stereo_camera) + modality_config = get_modality_config(g1_rm, config.add_stereo_camera) + + text_to_speech = TextToSpeech() if config.text_to_speech else None + + # Only set DataCollectionInfo if we're creating a new dataset + # When adding to existing dataset, DataCollectionInfo will be ignored + if config.robot_id is not None: + data_collection_info = DataCollectionInfo( + teleoperator_username=config.teleoperator_username, + support_operator_username=config.support_operator_username, + robot_type="g1", + robot_id=config.robot_id, + lower_body_policy=config.lower_body_policy, + wbc_model_path=config.wbc_model_path, + ) + else: + # Use default DataCollectionInfo when adding to existing dataset + # This will be ignored if the dataset already exists + data_collection_info = DataCollectionInfo() + + robot_config_client = ROSServiceClient(ROBOT_CONFIG_TOPIC) + robot_config = robot_config_client.get_config() + + data_exporter = Gr00tDataExporter.create( + save_root=f"{config.root_output_dir}/{config.dataset_name}", + fps=config.data_collection_frequency, + features=dataset_features, + modality_config=modality_config, + task=config.task_prompt, + upload_bucket_path=BUCKET_BASE_PATH, + data_collection_info=data_collection_info, + script_config=robot_config, + ) + + data_collector = Gr00tDataCollector( + node=node, + frequency=config.data_collection_frequency, + data_exporter=data_exporter, + state_topic_name=STATE_TOPIC_NAME, + camera_host=config.camera_host, + camera_port=config.camera_port, + text_to_speech=text_to_speech, + ) + data_collector.run() + + +if __name__ == "__main__": + config = tyro.cli(DataExporterConfig) + config.task_prompt = input("Enter the task prompt: ").strip().lower() + add_to_existing_dataset = input("Add to existing dataset? (y/n): ").strip().lower() + + if add_to_existing_dataset == "y": + config.dataset_name = input("Enter the dataset name: ").strip().lower() + # When adding to existing dataset, we don't need robot_id or operator usernames + # as they should already be set in the existing dataset + elif add_to_existing_dataset == "n": + # robot_id = input("Enter the robot ID: ").strip().lower() + # if robot_id not in G1_ROBOT_IDS: + # raise ValueError(f"Invalid robot ID: {robot_id}. Available robot IDs: {G1_ROBOT_IDS}") + config.robot_id = "sim" + config.dataset_name = f"{datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}-G1-{config.robot_id}" + + # Only ask for operator usernames when creating a new dataset + # print("Available teleoperator usernames:") + # for i, username in enumerate(OPERATOR_USERNAMES): + # print(f"{i}: {username}") + # teleop_idx = int(input("Select teleoperator username index: ")) + # config.teleoperator_username = OPERATOR_USERNAMES[teleop_idx] + config.teleoperator_username = "NEW_USER" + + # print("\nAvailable support operator usernames:") + # for i, username in enumerate(OPERATOR_USERNAMES): + # print(f"{i}: {username}") + # support_idx = int(input("Select support operator username index: ")) + # config.support_operator_username = OPERATOR_USERNAMES[support_idx] + config.support_operator_username = "NEW_USER" + + main(config) diff --git a/gr00t_wbc/control/main/teleop/run_navigation_policy_loop.py b/gr00t_wbc/control/main/teleop/run_navigation_policy_loop.py new file mode 100644 index 0000000..cf672a6 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_navigation_policy_loop.py @@ -0,0 +1,68 @@ +import threading +import time + +import rclpy + +from gr00t_wbc.control.main.constants import NAV_CMD_TOPIC +from gr00t_wbc.control.policy.keyboard_navigation_policy import KeyboardNavigationPolicy +from gr00t_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber +from gr00t_wbc.control.utils.ros_utils import ROSMsgPublisher + +FREQUENCY = 10 +NAV_NODE_NAME = "NavigationPolicy" + + +def main(): + rclpy.init(args=None) + node = rclpy.create_node(NAV_NODE_NAME) + + # Start ROS spin in a separate thread + thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + thread.start() + time.sleep(0.5) + + dict_publisher = ROSMsgPublisher(NAV_CMD_TOPIC) + keyboard_listener = KeyboardListenerSubscriber() + + # Initialize navigation policy + navigation_policy = KeyboardNavigationPolicy() + + # Create rate controller + rate = node.create_rate(FREQUENCY) + + try: + while rclpy.ok(): + t_now = time.monotonic() + # get keyboard input + + navigation_policy.handle_keyboard_button(keyboard_listener.read_msg()) + # Get action from navigation policy + action = navigation_policy.get_action(time=t_now) + + # Add timestamp to the data + action["timestamp"] = t_now + + # Create and publish ByteMultiArray message + dict_publisher.publish(action) + + # Print status periodically (optional) + if int(t_now * 10) % 10 == 0: + nav_cmd = action["navigate_cmd"] + node.get_logger().info( + f"Nav cmd: linear=({nav_cmd[0]:.2f}, {nav_cmd[1]:.2f}), " + f"angular={nav_cmd[2]:.2f}" + ) + + rate.sleep() + + except KeyboardInterrupt: + print("Navigation control loop terminated by user") + + finally: + # Clean shutdown + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/main/teleop/run_sim_loop.py b/gr00t_wbc/control/main/teleop/run_sim_loop.py new file mode 100644 index 0000000..c907988 --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_sim_loop.py @@ -0,0 +1,61 @@ +from typing import Dict + +import tyro + +from gr00t_wbc.control.envs.g1.sim.simulator_factory import SimulatorFactory, init_channel +from gr00t_wbc.control.main.teleop.configs.configs import SimLoopConfig +from gr00t_wbc.control.robot_model.instantiation.g1 import ( + instantiate_g1_robot_model, +) +from gr00t_wbc.control.robot_model.robot_model import RobotModel + +ArgsConfig = SimLoopConfig + + +class SimWrapper: + def __init__(self, robot_model: RobotModel, env_name: str, config: Dict[str, any], **kwargs): + self.robot_model = robot_model + self.config = config + + init_channel(config=self.config) + + # Create simulator using factory + self.sim = SimulatorFactory.create_simulator( + config=self.config, + env_name=env_name, + **kwargs, + ) + + +def main(config: ArgsConfig): + wbc_config = config.load_wbc_yaml() + # NOTE: we will override the interface to local if it is not specified + wbc_config["ENV_NAME"] = config.env_name + + if config.enable_image_publish: + assert ( + config.enable_offscreen + ), "enable_offscreen must be True when enable_image_publish is True" + + robot_model = instantiate_g1_robot_model() + + sim_wrapper = SimWrapper( + robot_model=robot_model, + env_name=config.env_name, + config=wbc_config, + onscreen=wbc_config.get("ENABLE_ONSCREEN", True), + offscreen=wbc_config.get("ENABLE_OFFSCREEN", False), + ) + # Start simulator as independent process + SimulatorFactory.start_simulator( + sim_wrapper.sim, + as_thread=False, + enable_image_publish=config.enable_image_publish, + mp_start_method=config.mp_start_method, + camera_port=config.camera_port, + ) + + +if __name__ == "__main__": + config = tyro.cli(ArgsConfig) + main(config) diff --git a/gr00t_wbc/control/main/teleop/run_sync_sim_data_collection.py b/gr00t_wbc/control/main/teleop/run_sync_sim_data_collection.py new file mode 100644 index 0000000..fc4ca5d --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_sync_sim_data_collection.py @@ -0,0 +1,213 @@ +from pathlib import Path +import time + +import tyro + +from gr00t_wbc.control.main.teleop.configs.configs import SyncSimDataCollectionConfig +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model +from gr00t_wbc.control.utils.keyboard_dispatcher import ( + KeyboardDispatcher, + KeyboardListener, +) +from gr00t_wbc.control.utils.ros_utils import ROSManager +from gr00t_wbc.control.utils.sync_sim_utils import ( + COLLECTION_KEY, + SKIP_KEY, + CITestManager, + EpisodeManager, + generate_frame, + get_data_exporter, + get_env, + get_policies, +) +from gr00t_wbc.control.utils.telemetry import Telemetry + +CONTROL_NODE_NAME = "ControlPolicy" + + +CONTROL_CMD_TOPIC = CONTROL_NODE_NAME + "/q_target" +ENV_NODE_NAME = "SyncEnv" +ENV_OBS_TOPIC = ENV_NODE_NAME + "/obs" + + +def display_controls(config: SyncSimDataCollectionConfig): + """ + Method to pretty print controls. + """ + + def print_command(char, info): + char += " " * (30 - len(char)) + print("{}\t{}".format(char, info)) + + print("") + print_command("Keys", "Command") + if config.manual_control: + print_command(COLLECTION_KEY, "start/stop data collection") + print_command(SKIP_KEY, "skip and collect new episodes") + print_command("w-s-a-d", "move horizontally in x-y plane (press '=' first to enable)") + print_command("q", "rotate (counter-clockwise)") + print_command("e", "rotate (clockwise)") + print_command("space", "reset all velocity to zero") + print("") + + +def main(config: SyncSimDataCollectionConfig): + ros_manager = ROSManager(node_name=CONTROL_NODE_NAME) + node = ros_manager.node + + # Initialize telemetry + telemetry = Telemetry(window_size=100) + + # Initialize robot model + robot_type, robot_model = get_robot_type_and_model( + config.robot, + enable_waist_ik=config.enable_waist, + ) + + # Initialize sim env + env = get_env(config, onscreen=config.enable_onscreen, offscreen=config.save_img_obs) + seed = int(time.time()) + env.reset(seed) + env.render() + obs = env.observe() + robot_model.set_initial_body_pose(obs["q"]) + + # Initialize data exporter + exporter = get_data_exporter( + config, + obs, + robot_model, + save_path=Path("./outputs/ci_test/") if config.ci_test else None, + ) + + # Display control signals + display_controls(config) + + # Initialize policies + wbc_policy, teleop_policy = get_policies(config, robot_type, robot_model) + + dispatcher = KeyboardDispatcher() + keyboard_listener = KeyboardListener() # for data collection keys + dispatcher.register(keyboard_listener) + dispatcher.register(wbc_policy) + dispatcher.register(teleop_policy) + + dispatcher.start() + + rate = node.create_rate(config.control_frequency) + + # Initialize episode manager to handle state transitions and data collection + episode_manager = EpisodeManager(config) + + # Initialize CI test manager + ci_test_manager = CITestManager(config) if config.ci_test else None + + try: + while ros_manager.ok(): + + need_reset = False + keyboard_input = keyboard_listener.pop_key() + + with telemetry.timer("total_loop"): + max_mujoco_state_len, mujoco_state_len, mujoco_state = env.get_mujoco_state_info() + + # Measure observation time + with telemetry.timer("observe"): + obs = env.observe() + wbc_policy.set_observation(obs) + + # Measure policy setup time + with telemetry.timer("policy_setup"): + teleop_cmd = teleop_policy.get_action() + + wbc_goal = {} + + # Note that wbc_goal["navigation_cmd'] could be overwritten by teleop_cmd + if teleop_cmd: + for key, value in teleop_cmd.items(): + wbc_goal[key] = value + # Draw IK indicators + if config.ik_indicator: + env.set_ik_indicator(teleop_cmd) + if wbc_goal: + wbc_policy.set_goal(wbc_goal) + + # Measure policy action calculation time + with telemetry.timer("policy_action"): + wbc_action = wbc_policy.get_action() + + if config.ci_test: + ci_test_manager.check_upper_body_motion(robot_model, wbc_action, config) + + # Measure action queue time + with telemetry.timer("step"): + obs, _, _, _, step_info = env.step(wbc_action) + env.render() + episode_manager.increment_step() + + if config.ci_test and config.ci_test_mode == "pre_merge": + ci_test_manager.check_end_effector_tracking( + teleop_cmd, obs, config, episode_manager.get_step_count() + ) + + # Handle data collection trigger + episode_manager.handle_collection_trigger(wbc_goal, keyboard_input, step_info) + + # Collect data frame + if episode_manager.should_collect_data(): + frame = generate_frame( + obs, + wbc_action, + seed, + mujoco_state, + mujoco_state_len, + max_mujoco_state_len, + teleop_cmd, + wbc_goal, + config.save_img_obs, + ) + # exporting data + exporter.add_frame(frame) + + # if done and task_completion_hold_count is 0, save the episode + need_reset = episode_manager.check_export_and_completion(exporter) + + # check data abort + if episode_manager.handle_skip(wbc_goal, keyboard_input, exporter): + need_reset = True + + if need_reset: + if config.ci_test: + print("CI test: Completed...") + raise KeyboardInterrupt + + seed = int(time.time()) + env.reset(seed) + env.render() + + print("Sleeping for 3 seconds before resetting teleop policy...") + for j in range(3, 0, -1): + print(f"Starting in {j}...") + time.sleep(1) + + wbc_policy, teleop_policy = get_policies(config, robot_type, robot_model) + episode_manager.reset_step_count() + + rate.sleep() + + except ros_manager.exceptions() as e: + print(f"ROSManager interrupted by user: {e}") + finally: + # Cleanup resources + teleop_policy.close() + dispatcher.stop() + ros_manager.shutdown() + env.close() + print("Sync sim data collection loop terminated.") + + return True + + +if __name__ == "__main__": + config = tyro.cli(SyncSimDataCollectionConfig) + main(config) diff --git a/gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py b/gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py new file mode 100644 index 0000000..3c6650b --- /dev/null +++ b/gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py @@ -0,0 +1,110 @@ +import time + +import rclpy +import tyro + +from gr00t_wbc.control.main.constants import CONTROL_GOAL_TOPIC +from gr00t_wbc.control.main.teleop.configs.configs import TeleopConfig +from gr00t_wbc.control.policy.lerobot_replay_policy import LerobotReplayPolicy +from gr00t_wbc.control.policy.teleop_policy import TeleopPolicy +from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model +from gr00t_wbc.control.teleop.solver.hand.instantiation.g1_hand_ik_instantiation import ( + instantiate_g1_hand_ik_solver, +) +from gr00t_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK +from gr00t_wbc.control.utils.ros_utils import ROSManager, ROSMsgPublisher +from gr00t_wbc.control.utils.telemetry import Telemetry + +TELEOP_NODE_NAME = "TeleopPolicy" + + +def main(config: TeleopConfig): + ros_manager = ROSManager(node_name=TELEOP_NODE_NAME) + node = ros_manager.node + + if config.robot == "g1": + waist_location = "lower_and_upper_body" if config.enable_waist else "lower_body" + robot_model = instantiate_g1_robot_model( + waist_location=waist_location, high_elbow_pose=config.high_elbow_pose + ) + left_hand_ik_solver, right_hand_ik_solver = instantiate_g1_hand_ik_solver() + else: + raise ValueError(f"Unsupported robot name: {config.robot}") + + if config.lerobot_replay_path: + teleop_policy = LerobotReplayPolicy( + robot_model=robot_model, parquet_path=config.lerobot_replay_path + ) + else: + print("running teleop policy, waiting teleop policy to be initialized...") + retargeting_ik = TeleopRetargetingIK( + robot_model=robot_model, + left_hand_ik_solver=left_hand_ik_solver, + right_hand_ik_solver=right_hand_ik_solver, + enable_visualization=config.enable_visualization, + body_active_joint_groups=["upper_body"], + ) + teleop_policy = TeleopPolicy( + robot_model=robot_model, + retargeting_ik=retargeting_ik, + body_control_device=config.body_control_device, + hand_control_device=config.hand_control_device, + body_streamer_ip=config.body_streamer_ip, # vive tracker, leap motion does not require + body_streamer_keyword=config.body_streamer_keyword, + enable_real_device=config.enable_real_device, + replay_data_path=config.teleop_replay_path, + ) + + # Create a publisher for the navigation commands + control_publisher = ROSMsgPublisher(CONTROL_GOAL_TOPIC) + + # Create rate controller + rate = node.create_rate(config.teleop_frequency) + iteration = 0 + time_to_get_to_initial_pose = 2 # seconds + + telemetry = Telemetry(window_size=100) + + try: + while rclpy.ok(): + with telemetry.timer("total_loop"): + t_start = time.monotonic() + # Get the current teleop action + with telemetry.timer("get_action"): + data = teleop_policy.get_action() + + # Add timing information to the message + t_now = time.monotonic() + data["timestamp"] = t_now + + # Set target completion time - longer for initial pose, then match control frequency + if iteration == 0: + data["target_time"] = t_now + time_to_get_to_initial_pose + else: + data["target_time"] = t_now + (1 / config.teleop_frequency) + + # Publish the teleop command + with telemetry.timer("publish_teleop_command"): + control_publisher.publish(data) + + # For the initial pose, wait the full duration before continuing + if iteration == 0: + print(f"Moving to initial pose for {time_to_get_to_initial_pose} seconds") + time.sleep(time_to_get_to_initial_pose) + iteration += 1 + end_time = time.monotonic() + if (end_time - t_start) > (1 / config.teleop_frequency): + telemetry.log_timing_info(context="Teleop Policy Loop Missed", threshold=0.001) + rate.sleep() + + except ros_manager.exceptions() as e: + print(f"ROSManager interrupted by user: {e}") + + finally: + print("Cleaning up...") + ros_manager.shutdown() + + +if __name__ == "__main__": + config = tyro.cli(TeleopConfig) + main(config) diff --git a/gr00t_wbc/control/policy/__init__.py b/gr00t_wbc/control/policy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/policy/g1_decoupled_whole_body_policy.py b/gr00t_wbc/control/policy/g1_decoupled_whole_body_policy.py new file mode 100644 index 0000000..ddfee45 --- /dev/null +++ b/gr00t_wbc/control/policy/g1_decoupled_whole_body_policy.py @@ -0,0 +1,157 @@ +import time as time_module +from typing import Optional + +import numpy as np +from pinocchio import rpy + +from gr00t_wbc.control.base.policy import Policy +from gr00t_wbc.control.main.constants import DEFAULT_NAV_CMD + + +class G1DecoupledWholeBodyPolicy(Policy): + """ + This class implements a whole-body policy for the G1 robot by combining an upper-body + policy and a lower-body RL-based policy. + It is designed to work with the G1 robot's specific configuration and control requirements. + """ + + def __init__( + self, + robot_model, + lower_body_policy: Policy, + upper_body_policy: Policy, + ): + self.robot_model = robot_model + self.lower_body_policy = lower_body_policy + self.upper_body_policy = upper_body_policy + self.last_goal_time = time_module.monotonic() + self.is_in_teleop_mode = False # Track if lower body is in teleop mode + + def set_observation(self, observation): + # Upper body policy is open loop (just interpolation), so we don't need to set the observation + self.lower_body_policy.set_observation(observation) + + def set_goal(self, goal): + """ + Set the goal for both upper and lower body policies. + + Args: + goal: Command from the planners + goal["target_upper_body_pose"]: Target pose for the upper body policy + goal["target_time"]: Target goal time + goal["interpolation_garbage_collection_time"]: Waypoints earlier than this time are removed + goal["navigate_cmd"]: Target navigation velocities for the lower body policy + goal["base_height_command"]: Target base height for both upper and lower body policies + """ + # Update goal timestamp for timeout safety + self.last_goal_time = time_module.monotonic() + + upper_body_goal = {} + lower_body_goal = {} + + # Upper body goal keys + upper_body_keys = [ + "target_upper_body_pose", + "base_height_command", + "target_time", + "interpolation_garbage_collection_time", + "navigate_cmd", + ] + for key in upper_body_keys: + if key in goal: + upper_body_goal[key] = goal[key] + + # Always ensure navigate_cmd is present to prevent interpolation from old dangerous values + if "navigate_cmd" not in goal: + # Safety: Inject safe default navigate_cmd to ensure interpolation goes to stop + if "target_time" in goal and isinstance(goal["target_time"], list): + upper_body_goal["navigate_cmd"] = [np.array(DEFAULT_NAV_CMD)] * len( + goal["target_time"] + ) + else: + upper_body_goal["navigate_cmd"] = np.array(DEFAULT_NAV_CMD) + + # Set teleop policy command flag + has_teleop_commands = ("navigate_cmd" in goal) or ("base_height_command" in goal) + self.is_in_teleop_mode = has_teleop_commands # Track teleop state for timeout safety + self.lower_body_policy.set_use_teleop_policy_cmd(has_teleop_commands) + + # Lower body goal keys + lower_body_keys = [ + "toggle_stand_command", + "toggle_policy_action", + ] + for key in lower_body_keys: + if key in goal: + lower_body_goal[key] = goal[key] + + self.upper_body_policy.set_goal(upper_body_goal) + self.lower_body_policy.set_goal(lower_body_goal) + + def get_action(self, time: Optional[float] = None): + current_time = time if time is not None else time_module.monotonic() + + # Safety timeout: Only apply when in teleop mode (communication loss dangerous) + # When in keyboard mode, no timeout needed (user controls directly) + if self.is_in_teleop_mode: + time_since_goal = current_time - self.last_goal_time + if time_since_goal > 1.0: # 1 second timeout + print( + f"SAFETY: Teleop mode timeout after {time_since_goal:.1f}s, injecting safe goal" + ) + # Inject safe goal to trigger all safety mechanisms (gear_wbc reset + interpolation reset) + safe_goal = { + "target_time": current_time + 0.1, + "interpolation_garbage_collection_time": current_time - 1.0, + } + self.set_goal( + safe_goal + ) # This will reset is_in_teleop_mode to False and trigger all safety + + # Get indices for groups + lower_body_indices = self.robot_model.get_joint_group_indices("lower_body") + upper_body_indices = self.robot_model.get_joint_group_indices("upper_body") + + # Initialize full configuration with zeros + q = np.zeros(self.robot_model.num_dofs) + + upper_body_action = self.upper_body_policy.get_action(time) + q[upper_body_indices] = upper_body_action["target_upper_body_pose"] + q_arms = q[self.robot_model.get_joint_group_indices("arms")] + base_height_command = upper_body_action.get("base_height_command", None) + interpolated_navigate_cmd = upper_body_action.get("navigate_cmd", None) + + # Compute torso orientation relative to waist, to pass to lower body policy + self.robot_model.cache_forward_kinematics(q, auto_clip=False) + torso_orientation = self.robot_model.frame_placement("torso_link").rotation + waist_orientation = self.robot_model.frame_placement("pelvis").rotation + # Extract yaw from rotation matrix and create a rotation with only yaw + # The rotation property is a 3x3 numpy array + waist_yaw = np.arctan2(waist_orientation[1, 0], waist_orientation[0, 0]) + # Create a rotation matrix with only yaw using Pinocchio's rpy functions + waist_yaw_only_rotation = rpy.rpyToMatrix(0, 0, waist_yaw) + yaw_only_waist_from_torso = waist_yaw_only_rotation.T @ torso_orientation + torso_orientation_rpy = rpy.matrixToRpy(yaw_only_waist_from_torso) + + lower_body_action = self.lower_body_policy.get_action( + time, q_arms, base_height_command, torso_orientation_rpy, interpolated_navigate_cmd + ) + + # If pelvis is both in upper and lower body, lower body policy takes preference + q[lower_body_indices] = lower_body_action["body_action"][0][ + : len(lower_body_indices) + ] # lower body (legs + waist) + + self.last_action = {"q": q} + + return {"q": q} + + def handle_keyboard_button(self, key): + try: + self.lower_body_policy.locomotion_policy.handle_keyboard_button(key) + except AttributeError: + # Only catch AttributeError, let other exceptions propagate + self.lower_body_policy.handle_keyboard_button(key) + + def activate_policy(self): + self.handle_keyboard_button("]") diff --git a/gr00t_wbc/control/policy/g1_gear_wbc_policy.py b/gr00t_wbc/control/policy/g1_gear_wbc_policy.py new file mode 100644 index 0000000..6b4cd74 --- /dev/null +++ b/gr00t_wbc/control/policy/g1_gear_wbc_policy.py @@ -0,0 +1,295 @@ +import collections +from pathlib import Path +from typing import Any, Dict, Optional + +import numpy as np +import onnxruntime as ort +import torch + +from gr00t_wbc.control.base.policy import Policy +from gr00t_wbc.control.utils.gear_wbc_utils import get_gravity_orientation, load_config + + +class G1GearWbcPolicy(Policy): + """Simple G1 robot policy using OpenGearWbc trained neural network.""" + + def __init__(self, robot_model, config: str, model_path: str): + """Initialize G1GearWbcPolicy. + + Args: + config_path: Path to gear_wbc YAML configuration file + """ + self.config, self.LEGGED_GYM_ROOT_DIR = load_config(config) + self.robot_model = robot_model + self.use_teleop_policy_cmd = False + + package_root = Path(__file__).resolve().parents[2] + self.sim2mujoco_root_dir = str(package_root / "sim2mujoco") + model_path_1, model_path_2 = model_path.split(",") + + self.policy_1 = self.load_onnx_policy( + self.sim2mujoco_root_dir + "/resources/robots/g1/" + model_path_1 + ) + self.policy_2 = self.load_onnx_policy( + self.sim2mujoco_root_dir + "/resources/robots/g1/" + model_path_2 + ) + + # Initialize observation history buffer + self.observation = None + self.obs_history = collections.deque(maxlen=self.config["obs_history_len"]) + self.obs_buffer = np.zeros(self.config["num_obs"], dtype=np.float32) + self.counter = 0 + + # Initialize state variables + self.use_policy_action = False + self.action = np.zeros(self.config["num_actions"], dtype=np.float32) + self.target_dof_pos = self.config["default_angles"].copy() + self.cmd = self.config["cmd_init"].copy() + self.height_cmd = self.config["height_cmd"] + self.freq_cmd = self.config["freq_cmd"] + self.roll_cmd = self.config["rpy_cmd"][0] + self.pitch_cmd = self.config["rpy_cmd"][1] + self.yaw_cmd = self.config["rpy_cmd"][2] + self.gait_indices = torch.zeros((1), dtype=torch.float32) + + def load_onnx_policy(self, model_path: str): + print(f"Loading ONNX policy from {model_path}") + model = ort.InferenceSession(model_path) + + def run_inference(input_tensor): + ort_inputs = {model.get_inputs()[0].name: input_tensor.cpu().numpy()} + ort_outs = model.run(None, ort_inputs) + return torch.tensor(ort_outs[0], device="cpu") + + print(f"Successfully loaded ONNX policy from {model_path}") + + return run_inference + + def compute_observation(self, observation: Dict[str, Any]) -> tuple[np.ndarray, int]: + """Compute the observation vector from current state""" + # Get body joint indices (excluding waist roll and pitch) + self.gait_indices = torch.remainder(self.gait_indices + 0.02 * self.freq_cmd, 1.0) + durations = torch.full_like(self.gait_indices, 0.5) + phases = 0.5 + foot_indices = [ + self.gait_indices + phases, # FL + self.gait_indices, # FR + ] + self.foot_indices = torch.remainder( + torch.cat([foot_indices[i].unsqueeze(1) for i in range(2)], dim=1), 1.0 + ) + for fi in foot_indices: + stance = fi < durations + swing = fi >= durations + fi[stance] = fi[stance] * (0.5 / durations[stance]) + fi[swing] = 0.5 + (fi[swing] - durations[swing]) * (0.5 / (1 - durations[swing])) + + self.clock_inputs = torch.stack([torch.sin(2 * np.pi * fi) for fi in foot_indices], dim=1) + + body_indices = self.robot_model.get_joint_group_indices("body") + body_indices = [idx for idx in body_indices] + + n_joints = len(body_indices) + + # Extract joint data + qj = observation["q"][body_indices].copy() + dqj = observation["dq"][body_indices].copy() + + # Extract floating base data + quat = observation["floating_base_pose"][3:7].copy() # quaternion + omega = observation["floating_base_vel"][3:6].copy() # angular velocity + + # Handle default angles padding + if len(self.config["default_angles"]) < n_joints: + padded_defaults = np.zeros(n_joints, dtype=np.float32) + padded_defaults[: len(self.config["default_angles"])] = self.config["default_angles"] + else: + padded_defaults = self.config["default_angles"][:n_joints] + + # Scale the values + qj_scaled = (qj - padded_defaults) * self.config["dof_pos_scale"] + dqj_scaled = dqj * self.config["dof_vel_scale"] + gravity_orientation = get_gravity_orientation(quat) + omega_scaled = omega * self.config["ang_vel_scale"] + + # Calculate single observation dimension + single_obs_dim = 86 # 3 + 1 + 3 + 3 + 3 + n_joints + n_joints + 15, n_joints = 29 + + # Create single observation + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:3] = self.cmd[:3] * self.config["cmd_scale"] + single_obs[3:4] = np.array([self.height_cmd]) + single_obs[4:7] = np.array([self.roll_cmd, self.pitch_cmd, self.yaw_cmd]) + single_obs[7:10] = omega_scaled + single_obs[10:13] = gravity_orientation + # single_obs[14:17] = omega_scaled_torso + # single_obs[17:20] = gravity_torso + single_obs[13 : 13 + n_joints] = qj_scaled + single_obs[13 + n_joints : 13 + 2 * n_joints] = dqj_scaled + single_obs[13 + 2 * n_joints : 13 + 2 * n_joints + 15] = self.action + # single_obs[13 + 2 * n_joints + 15 : 13 + 2 * n_joints + 15 + 2] = ( + # processed_clock_inputs.detach().cpu().numpy() + # ) + return single_obs, single_obs_dim + + def set_observation(self, observation: Dict[str, Any]): + """Update the policy's current observation of the environment. + + Args: + observation: Dictionary containing single observation from current state + Should include 'obs' key with current single observation + """ + + # Extract the single observation + self.observation = observation + single_obs, single_obs_dim = self.compute_observation(observation) + + # Update observation history every control_decimation steps + # if self.counter % self.config['control_decimation'] == 0: + # Add current observation to history + self.obs_history.append(single_obs) + + # Fill history with zeros if not enough observations yet + while len(self.obs_history) < self.config["obs_history_len"]: + self.obs_history.appendleft(np.zeros_like(single_obs)) + + # Construct full observation with history + single_obs_dim = len(single_obs) + for i, hist_obs in enumerate(self.obs_history): + start_idx = i * single_obs_dim + end_idx = start_idx + single_obs_dim + self.obs_buffer[start_idx:end_idx] = hist_obs + + # Convert to tensor for policy + self.obs_tensor = torch.from_numpy(self.obs_buffer).unsqueeze(0) + # self.counter += 1 + + assert self.obs_tensor.shape[1] == self.config["num_obs"] + + def set_use_teleop_policy_cmd(self, use_teleop_policy_cmd: bool): + self.use_teleop_policy_cmd = use_teleop_policy_cmd + # Safety: When teleop is disabled, reset navigation to stop + if not use_teleop_policy_cmd: + self.nav_cmd = self.config["cmd_init"].copy() # Reset to safe default + + def set_goal(self, goal: Dict[str, Any]): + """Set the goal for the policy. + + Args: + goal: Dictionary containing the goal for the policy + """ + + if "toggle_policy_action" in goal: + if goal["toggle_policy_action"]: + self.use_policy_action = not self.use_policy_action + + def get_action( + self, + time: Optional[float] = None, + arms_target_pose: Optional[np.ndarray] = None, + base_height_command: Optional[np.ndarray] = None, + torso_orientation_rpy: Optional[np.ndarray] = None, + interpolated_navigate_cmd: Optional[np.ndarray] = None, + ) -> Dict[str, Any]: + """Compute and return the next action based on current observation. + + Args: + time: Optional "monotonic time" for time-dependent policies (unused) + + Returns: + Dictionary containing the action to be executed + """ + if self.obs_tensor is None: + raise ValueError("No observation set. Call set_observation() first.") + + if base_height_command is not None and self.use_teleop_policy_cmd: + self.height_cmd = ( + base_height_command[0] + if isinstance(base_height_command, list) + else base_height_command + ) + + if interpolated_navigate_cmd is not None and self.use_teleop_policy_cmd: + self.cmd = interpolated_navigate_cmd + + if torso_orientation_rpy is not None and self.use_teleop_policy_cmd: + self.roll_cmd = torso_orientation_rpy[0] + self.pitch_cmd = torso_orientation_rpy[1] + self.yaw_cmd = torso_orientation_rpy[2] + + # Run policy inference + with torch.no_grad(): + # Select appropriate policy based on command magnitude + if np.linalg.norm(self.cmd) < 0.05: + # Use standing policy for small commands + policy = self.policy_1 + else: + # Use walking policy for movement commands + policy = self.policy_2 + + self.action = policy(self.obs_tensor).detach().numpy().squeeze() + + # Transform action to target_dof_pos + if self.use_policy_action: + cmd_q = self.action * self.config["action_scale"] + self.config["default_angles"] + else: + cmd_q = self.observation["q"][self.robot_model.get_joint_group_indices("lower_body")] + + cmd_dq = np.zeros(self.config["num_actions"]) + cmd_tau = np.zeros(self.config["num_actions"]) + + return {"body_action": (cmd_q, cmd_dq, cmd_tau)} + + def handle_keyboard_button(self, key): + if key == "]": + self.use_policy_action = True + elif key == "o": + self.use_policy_action = False + elif key == "w": + self.cmd[0] += 0.2 + elif key == "s": + self.cmd[0] -= 0.2 + elif key == "a": + self.cmd[1] += 0.2 + elif key == "d": + self.cmd[1] -= 0.2 + elif key == "q": + self.cmd[2] += 0.2 + elif key == "e": + self.cmd[2] -= 0.2 + elif key == "z": + self.cmd[0] = 0.0 + self.cmd[1] = 0.0 + self.cmd[2] = 0.0 + elif key == "1": + self.height_cmd += 0.1 + elif key == "2": + self.height_cmd -= 0.1 + elif key == "n": + self.freq_cmd -= 0.1 + self.freq_cmd = max(1.0, self.freq_cmd) + elif key == "m": + self.freq_cmd += 0.1 + self.freq_cmd = min(2.0, self.freq_cmd) + elif key == "3": + self.roll_cmd -= np.deg2rad(10) + elif key == "4": + self.roll_cmd += np.deg2rad(10) + elif key == "5": + self.pitch_cmd -= np.deg2rad(10) + elif key == "6": + self.pitch_cmd += np.deg2rad(10) + elif key == "7": + self.yaw_cmd -= np.deg2rad(10) + elif key == "8": + self.yaw_cmd += np.deg2rad(10) + + if key: + print("--------------------------------") + print(f"Linear velocity command: {self.cmd}") + print(f"Base height command: {self.height_cmd}") + print(f"Use policy action: {self.use_policy_action}") + print(f"roll deg angle: {np.rad2deg(self.roll_cmd)}") + print(f"pitch deg angle: {np.rad2deg(self.pitch_cmd)}") + print(f"yaw deg angle: {np.rad2deg(self.yaw_cmd)}") + print(f"Gait frequency: {self.freq_cmd}") diff --git a/gr00t_wbc/control/policy/identity_policy.py b/gr00t_wbc/control/policy/identity_policy.py new file mode 100644 index 0000000..d4e5087 --- /dev/null +++ b/gr00t_wbc/control/policy/identity_policy.py @@ -0,0 +1,25 @@ +from copy import deepcopy +from typing import Optional + +import gymnasium as gym + +from gr00t_wbc.control.base.policy import Policy + + +class IdentityPolicy(Policy): + def __init__(self): + self.reset() + + def get_action(self, time: Optional[float] = None) -> dict[str, any]: + return self.goal + + def set_goal(self, goal: dict[str, any]) -> None: + self.goal = deepcopy(goal) + self.goal.pop("interpolation_garbage_collection_time", None) + self.goal.pop("target_time", None) + + def observation_space(self) -> gym.spaces.Dict: + return gym.spaces.Dict() + + def action_space(self) -> gym.spaces.Dict: + return gym.spaces.Dict() diff --git a/gr00t_wbc/control/policy/interpolation_policy.py b/gr00t_wbc/control/policy/interpolation_policy.py new file mode 100644 index 0000000..f190c4a --- /dev/null +++ b/gr00t_wbc/control/policy/interpolation_policy.py @@ -0,0 +1,297 @@ +import numbers +import time as time_module +from typing import Any, Dict, Optional, Union + +import gymnasium as gym +import numpy as np +import scipy.interpolate as si + +from gr00t_wbc.control.base.policy import Policy + + +class InterpolationPolicy(Policy): + def __init__( + self, + init_time: float, + init_values: dict[str, np.ndarray], + max_change_rate: float, + ): + """ + Args: + init_time: The time of recording the initial values. + init_values: The initial values of the features. + The keys are the names of the features, and the values + are the initial values of the features (1D array). + max_change_rate: The maximum change rate. + """ + super().__init__() + self.last_action = init_values # Vecs are 1D arrays + self.concat_order = sorted(init_values.keys()) + self.concat_dims = [] + for key in self.concat_order: + vec = np.array(init_values[key]) + if vec.ndim == 2 and vec.shape[0] == 1: + vec = vec[0] + init_values[key] = vec + assert vec.ndim == 1, f"The shape of {key} should be (D,). Got {vec.shape}." + self.concat_dims.append(vec.shape[0]) + + self.init_values_concat = self._concat_vecs(init_values, 1) + self.max_change_rate = max_change_rate + self.reset(init_time) + + def reset(self, init_time: float = time_module.monotonic()): + self.interp = PoseTrajectoryInterpolator(np.array([init_time]), self.init_values_concat) + self.last_waypoint_time = init_time + self.max_change_rate = self.max_change_rate + + def _concat_vecs(self, values: dict[str, np.ndarray], length: int) -> np.ndarray: + """ + Concatenate the vectors into a 2D array to be used for interpolation. + Args: + values: The values to concatenate. + length: The length of the concatenated vectors (time dimension). + Returns: + The concatenated vectors (T, D) arrays. + """ + concat_vecs = [] + for key in self.concat_order: + if key in values: + vec = np.array(values[key]) + if vec.ndim == 1: + # If the vector is 1D, tile it to the length of the time dimension + vec = np.tile(vec, (length, 1)) + assert vec.ndim == 2, f"The shape of {key} should be (T, D). Got {vec.shape}." + concat_vecs.append(vec) + else: + # If the vector is not in the values, use the last action + # Since the last action is 1D, we need to tile it to the length of the time dimension + concat_vecs.append(np.tile(self.last_action[key], (length, 1))) + return np.concatenate(concat_vecs, axis=1) # Vecs are 2D (T, D) arrays + + def _unconcat_vecs(self, concat_vec: np.ndarray) -> dict[str, np.ndarray]: + curr_idx = 0 + action = {} + assert ( + concat_vec.ndim == 1 + ), f"The shape of the concatenated vectors should be (T, D). Got {concat_vec.shape}." + for key, dim in zip(self.concat_order, self.concat_dims): + action[key] = concat_vec[curr_idx : curr_idx + dim] + curr_idx += dim + return action # Vecs are 1D arrays + + def __call__( + self, observation: Dict[str, Any], goal: Dict[str, Any], time: float + ) -> Dict[str, np.ndarray]: + raise NotImplementedError( + "`InterpolationPolicy` accepts goal and provide action in two separate methods." + ) + + def set_goal(self, goal: Dict[str, Any]) -> None: + if "target_time" not in goal: + return + assert ( + "interpolation_garbage_collection_time" in goal + ), "`interpolation_garbage_collection_time` is required." + target_time = goal.pop("target_time") + interpolation_garbage_collection_time = goal.pop("interpolation_garbage_collection_time") + + if isinstance(target_time, list): + for key, vec in goal.items(): + assert isinstance(vec, list) + assert len(vec) == len(target_time), ( + f"The length of {key} and `target_time` should be the same. " + f"Got {len(vec)} and {len(target_time)}." + ) + else: + target_time = [target_time] + for key in goal: + goal[key] = [goal[key]] + + # Concatenate all vectors in goal + concat_vecs = self._concat_vecs(goal, len(target_time)) + assert concat_vecs.shape[0] == len(target_time), ( + f"The length of the concatenated goal and `target_time` should be the same. " + f"Got {concat_vecs.shape[0]} and {len(target_time)}." + ) + + for tt, vec in zip(target_time, concat_vecs): + if tt < interpolation_garbage_collection_time: + continue + self.interp = self.interp.schedule_waypoint( + pose=vec, + time=tt, + max_change_rate=self.max_change_rate, + interpolation_garbage_collection_time=interpolation_garbage_collection_time, + last_waypoint_time=self.last_waypoint_time, + ) + self.last_waypoint_time = tt + + def get_action(self, time: Optional[float] = None) -> dict[str, Any]: + """Get the next action based on the (current) monotonic time.""" + if time is None: + time = time_module.monotonic() + concat_vec = self.interp(time) + self.last_action.update(self._unconcat_vecs(concat_vec)) + return self.last_action + + def observation_space(self) -> gym.spaces.Dict: + """Return the observation space.""" + pass + + def action_space(self) -> gym.spaces.Dict: + """Return the action space.""" + pass + + def close(self) -> None: + """Clean up resources.""" + pass + + +class PoseTrajectoryInterpolator: + def __init__(self, times: np.ndarray, poses: np.ndarray): + assert len(times) >= 1 + assert len(poses) == len(times) + + times = np.asarray(times) + poses = np.asarray(poses) + + self.num_joint = len(poses[0]) + + if len(times) == 1: + # special treatment for single step interpolation + self.single_step = True + self._times = times + self._poses = poses + else: + self.single_step = False + assert np.all(times[1:] >= times[:-1]) + self.pose_interp = si.interp1d(times, poses, axis=0, assume_sorted=True) + + @property + def times(self) -> np.ndarray: + if self.single_step: + return self._times + else: + return self.pose_interp.x + + @property + def poses(self) -> np.ndarray: + if self.single_step: + return self._poses + else: + return self.pose_interp.y + + def trim(self, start_t: float, end_t: float) -> "PoseTrajectoryInterpolator": + assert start_t <= end_t + times = self.times + should_keep = (start_t < times) & (times < end_t) + keep_times = times[should_keep] + all_times = np.concatenate([[start_t], keep_times, [end_t]]) + # remove duplicates, Slerp requires strictly increasing x + all_times = np.unique(all_times) + # interpolate + all_poses = self(all_times) + return PoseTrajectoryInterpolator(times=all_times, poses=all_poses) + + def schedule_waypoint( + self, + pose, + time, + max_change_rate=np.inf, + interpolation_garbage_collection_time=None, + last_waypoint_time=None, + ) -> "PoseTrajectoryInterpolator": + if not isinstance(max_change_rate, np.ndarray): + max_change_rate = np.array([max_change_rate] * self.num_joint) + + assert len(max_change_rate) == self.num_joint + assert np.max(max_change_rate) > 0 + + if last_waypoint_time is not None: + assert interpolation_garbage_collection_time is not None + + # trim current interpolator to between interpolation_garbage_collection_time and last_waypoint_time + start_time = self.times[0] + end_time = self.times[-1] + assert start_time <= end_time + if interpolation_garbage_collection_time is not None: + if time <= interpolation_garbage_collection_time: + # if insert time is earlier than current time + # no effect should be done to the interpolator + return self + # now, interpolation_garbage_collection_time < time + start_time = max(interpolation_garbage_collection_time, start_time) + + if last_waypoint_time is not None: + # if last_waypoint_time is earlier than start_time + # use start_time + if time <= last_waypoint_time: + end_time = interpolation_garbage_collection_time + else: + end_time = max(last_waypoint_time, interpolation_garbage_collection_time) + else: + end_time = interpolation_garbage_collection_time + + end_time = min(end_time, time) + start_time = min(start_time, end_time) + # end time should be the latest of all times except time + # after this we can assume order (proven by zhenjia, due to the 2 min operations) + + # Constraints: + # start_time <= end_time <= time (proven by zhenjia) + # interpolation_garbage_collection_time <= start_time (proven by zhenjia) + # interpolation_garbage_collection_time <= time (proven by zhenjia) + + # time can't change + # last_waypoint_time can't change + # interpolation_garbage_collection_time can't change + assert start_time <= end_time + assert end_time <= time + if last_waypoint_time is not None: + if time <= last_waypoint_time: + assert end_time == interpolation_garbage_collection_time + else: + assert end_time == max(last_waypoint_time, interpolation_garbage_collection_time) + + if interpolation_garbage_collection_time is not None: + assert interpolation_garbage_collection_time <= start_time + assert interpolation_garbage_collection_time <= time + trimmed_interp = self.trim(start_time, end_time) + # after this, all waypoints in trimmed_interp is within start_time and end_time + # and is earlier than time + + # determine speed + duration = time - end_time + end_pose = trimmed_interp(end_time) + pose_min_duration = np.max(np.abs(end_pose - pose) / max_change_rate) + duration = max(duration, pose_min_duration) + assert duration >= 0 + last_waypoint_time = end_time + duration + + # insert new pose + times = np.append(trimmed_interp.times, [last_waypoint_time], axis=0) + poses = np.append(trimmed_interp.poses, [pose], axis=0) + + # create new interpolator + final_interp = PoseTrajectoryInterpolator(times, poses) + return final_interp + + def __call__(self, t: Union[numbers.Number, np.ndarray]) -> np.ndarray: + is_single = False + if isinstance(t, numbers.Number): + is_single = True + t = np.array([t]) + + pose = np.zeros((len(t), self.num_joint)) + if self.single_step: + pose[:] = self._poses[0] + else: + start_time = self.times[0] + end_time = self.times[-1] + t = np.clip(t, start_time, end_time) + pose = self.pose_interp(t) + + if is_single: + pose = pose[0] + return pose diff --git a/gr00t_wbc/control/policy/keyboard_navigation_policy.py b/gr00t_wbc/control/policy/keyboard_navigation_policy.py new file mode 100644 index 0000000..347a7a0 --- /dev/null +++ b/gr00t_wbc/control/policy/keyboard_navigation_policy.py @@ -0,0 +1,87 @@ +from typing import Any, Dict, Optional + +import numpy as np + +from gr00t_wbc.control.base.policy import Policy + + +class KeyboardNavigationPolicy(Policy): + def __init__( + self, + max_linear_velocity: float = 0.5, + max_angular_velocity: float = 0.5, + verbose: bool = True, + **kwargs, + ): + """ + Initialize the navigation policy. + + Args: + max_linear_velocity: Maximum linear velocity in m/s (for x and y components) + max_angular_velocity: Maximum angular velocity in rad/s (for yaw component) + **kwargs: Additional arguments passed to the base Policy class + """ + super().__init__(**kwargs) + self.max_linear_velocity = max_linear_velocity + self.max_angular_velocity = max_angular_velocity + self.verbose = verbose + + # Initialize velocity commands + self.lin_vel_command = np.zeros(2, dtype=np.float32) # [vx, vy] + self.ang_vel_command = np.zeros(1, dtype=np.float32) # [wz] + + def get_action(self, time: Optional[float] = None) -> Dict[str, Any]: + """ + Get the action to execute based on current state. + + Args: + time: Current time (optional) + + Returns: + Dict containing the action to execute with: + - navigate_cmd: np.array([vx, vy, wz]) where: + - vx: linear velocity in x direction (m/s) + - vy: linear velocity in y direction (m/s) + - wz: angular velocity around z axis (rad/s) + """ + # Combine linear and angular velocities into a single command + # Ensure velocities are within limits + vx = np.clip(self.lin_vel_command[0], -self.max_linear_velocity, self.max_linear_velocity) + vy = np.clip(self.lin_vel_command[1], -self.max_linear_velocity, self.max_linear_velocity) + wz = np.clip(self.ang_vel_command[0], -self.max_angular_velocity, self.max_angular_velocity) + + navigate_cmd = np.array([vx, vy, wz], dtype=np.float32) + + action = {"navigate_cmd": navigate_cmd} + return action + + def handle_keyboard_button(self, keycode: str): + """ + Handle keyboard inputs for navigation control. + + Args: + keycode: The key that was pressed + """ + if keycode == "w": + self.lin_vel_command[0] += 0.1 # Increase forward velocity + elif keycode == "s": + self.lin_vel_command[0] -= 0.1 # Increase backward velocity + elif keycode == "a": + self.lin_vel_command[1] += 0.1 # Increase left velocity + elif keycode == "d": + self.lin_vel_command[1] -= 0.1 # Increase right velocity + elif keycode == "q": + self.ang_vel_command[0] += 0.1 # Increase counter-clockwise rotation + elif keycode == "e": + self.ang_vel_command[0] -= 0.1 # Increase clockwise rotation + elif keycode == "z": + # Reset all velocities + self.lin_vel_command[:] = 0.0 + self.ang_vel_command[:] = 0.0 + if self.verbose: + print("Navigation policy: Reset all velocity commands to zero") + + # Print current velocities after any keyboard input + if self.verbose: + print(f"Nav lin vel: ({self.lin_vel_command[0]:.2f}, {self.lin_vel_command[1]:.2f})") + print(f"Nav ang vel: {self.ang_vel_command[0]:.2f}") diff --git a/gr00t_wbc/control/policy/lerobot_replay_policy.py b/gr00t_wbc/control/policy/lerobot_replay_policy.py new file mode 100644 index 0000000..2783ca8 --- /dev/null +++ b/gr00t_wbc/control/policy/lerobot_replay_policy.py @@ -0,0 +1,107 @@ +import time + +import pandas as pd + +from gr00t_wbc.control.base.policy import Policy +from gr00t_wbc.control.main.constants import DEFAULT_BASE_HEIGHT, DEFAULT_NAV_CMD, DEFAULT_WRIST_POSE +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.data.viz.rerun_viz import RerunViz + + +class LerobotReplayPolicy(Policy): + """Replay policy for Lerobot dataset, so we can replay the dataset + and just use the action from the dataset. + + Args: + parquet_path: Path to the parquet file containing the dataset. + """ + + is_active = True # by default, the replay policy is active + + def __init__(self, robot_model: RobotModel, parquet_path: str, use_viz: bool = False): + # self.dataset = LerobotDataset(dataset_path) + self.parquet_path = parquet_path + self._ctr = 0 + # read the parquet file + self.df = pd.read_parquet(self.parquet_path) + self._max_ctr = len(self.df) + # get the action from the dataframe + self.action = self.df.iloc[self._ctr]["action"] + self.use_viz = use_viz + if self.use_viz: + self.viz = RerunViz( + image_keys=["egoview_image"], + tensor_keys=[ + "left_arm_qpos", + "left_hand_qpos", + "right_arm_qpos", + "right_hand_qpos", + ], + window_size=5.0, + ) + self.robot_model = robot_model + self.upper_body_joint_indices = self.robot_model.get_joint_group_indices("upper_body") + + def get_action(self) -> dict[str, any]: + # get the action from the dataframe + action = self.df.iloc[self._ctr]["action"] + wrist_pose = self.df.iloc[self._ctr]["action.eef"] + navigate_cmd = self.df.iloc[self._ctr].get("teleop.navigate_command", DEFAULT_NAV_CMD) + base_height_cmd = self.df.iloc[self._ctr].get( + "teleop.base_height_command", DEFAULT_BASE_HEIGHT + ) + + self._ctr += 1 + if self._ctr >= self._max_ctr: + self._ctr = 0 + # print(f"Replay {self._ctr} / {self._max_ctr}") + if self.use_viz: + self.viz.plot_tensors( + { + "left_arm_qpos": action[self.robot_model.get_joint_group_indices("left_arm")] + + 15, + "left_hand_qpos": action[self.robot_model.get_joint_group_indices("left_hand")] + + 15, + "right_arm_qpos": action[self.robot_model.get_joint_group_indices("right_arm")] + + 15, + "right_hand_qpos": action[ + self.robot_model.get_joint_group_indices("right_hand") + ] + + 15, + }, + time.monotonic(), + ) + + return { + "target_upper_body_pose": action[self.upper_body_joint_indices], + "wrist_pose": wrist_pose, + "navigate_cmd": navigate_cmd, + "base_height_cmd": base_height_cmd, + "timestamp": time.time(), + } + + def action_to_cmd(self, action: dict[str, any]) -> dict[str, any]: + action["target_upper_body_pose"] = action["q"][ + self.robot_model.get_joint_group_indices("upper_body") + ] + del action["q"] + return action + + def set_observation(self, observation: dict[str, any]): + pass + + def get_observation(self) -> dict[str, any]: + return { + "wrist_pose": self.df.iloc[self._ctr - 1].get( + "observation.eef_state", DEFAULT_WRIST_POSE + ), + "timestamp": time.time(), + } + + +if __name__ == "__main__": + policy = LerobotReplayPolicy( + parquet_path="outputs/g1-open-hands-may7/data/chunk-000/episode_000000.parquet" + ) + action = policy.get_action() + print(action) diff --git a/gr00t_wbc/control/policy/teleop_policy.py b/gr00t_wbc/control/policy/teleop_policy.py new file mode 100644 index 0000000..26d5bf6 --- /dev/null +++ b/gr00t_wbc/control/policy/teleop_policy.py @@ -0,0 +1,207 @@ +from contextlib import contextmanager +import time +from typing import Optional + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.base.policy import Policy +from gr00t_wbc.control.robot_model import RobotModel +from gr00t_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK +from gr00t_wbc.control.teleop.teleop_streamer import TeleopStreamer + + +class TeleopPolicy(Policy): + """ + Robot-agnostic teleop policy. + Clean separation: IK processing vs command passing. + All robot-specific properties are abstracted through robot_model and hand_ik_solvers. + """ + + def __init__( + self, + body_control_device: str, + hand_control_device: str, + robot_model: RobotModel, + retargeting_ik: TeleopRetargetingIK, + body_streamer_ip: str = "192.168.?.?", + body_streamer_keyword: str = "shoulder", + enable_real_device: bool = True, + replay_data_path: Optional[str] = None, + replay_speed: float = 1.0, + wait_for_activation: int = 5, + activate_keyboard_listener: bool = True, + ): + if activate_keyboard_listener: + from gr00t_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber + + self.keyboard_listener = KeyboardListenerSubscriber() + else: + self.keyboard_listener = None + + self.wait_for_activation = wait_for_activation + + self.teleop_streamer = TeleopStreamer( + robot_model=robot_model, + body_control_device=body_control_device, + hand_control_device=hand_control_device, + enable_real_device=enable_real_device, + body_streamer_ip=body_streamer_ip, + body_streamer_keyword=body_streamer_keyword, + replay_data_path=replay_data_path, + replay_speed=replay_speed, + ) + self.robot_model = robot_model + self.retargeting_ik = retargeting_ik + self.is_active = False + + self.latest_left_wrist_data = np.eye(4) + self.latest_right_wrist_data = np.eye(4) + self.latest_left_fingers_data = {"position": np.zeros((25, 4, 4))} + self.latest_right_fingers_data = {"position": np.zeros((25, 4, 4))} + + def set_goal(self, goal: dict[str, any]): + # The current teleop policy doesn't take higher level commands yet. + pass + + def get_action(self) -> dict[str, any]: + # Get structured data + streamer_output = self.teleop_streamer.get_streamer_data() + + # Handle activation using teleop_data commands + self.check_activation( + streamer_output.teleop_data, wait_for_activation=self.wait_for_activation + ) + + action = {} + + # Process streamer data if active + if self.is_active and streamer_output.ik_data: + body_data = streamer_output.ik_data["body_data"] + left_hand_data = streamer_output.ik_data["left_hand_data"] + right_hand_data = streamer_output.ik_data["right_hand_data"] + + left_wrist_name = self.robot_model.supplemental_info.hand_frame_names["left"] + right_wrist_name = self.robot_model.supplemental_info.hand_frame_names["right"] + self.latest_left_wrist_data = body_data[left_wrist_name] + self.latest_right_wrist_data = body_data[right_wrist_name] + self.latest_left_fingers_data = left_hand_data + self.latest_right_fingers_data = right_hand_data + + # TODO: This stores the same data again + ik_data = { + "body_data": body_data, + "left_hand_data": left_hand_data, + "right_hand_data": right_hand_data, + } + action["ik_data"] = ik_data + + # Wrist poses (pos and quat) + # TODO: This stores the same wrist poses in two different formats + left_wrist_matrix = self.latest_left_wrist_data + right_wrist_matrix = self.latest_right_wrist_data + left_wrist_pose = np.concatenate( + [ + left_wrist_matrix[:3, 3], + R.from_matrix(left_wrist_matrix[:3, :3]).as_quat(scalar_first=True), + ] + ) + right_wrist_pose = np.concatenate( + [ + right_wrist_matrix[:3, 3], + R.from_matrix(right_wrist_matrix[:3, :3]).as_quat(scalar_first=True), + ] + ) + + # Combine IK results with control commands (no teleop_data commands) + action.update( + { + "left_wrist": self.latest_left_wrist_data, + "right_wrist": self.latest_right_wrist_data, + "left_fingers": self.latest_left_fingers_data, + "right_fingers": self.latest_right_fingers_data, + "wrist_pose": np.concatenate([left_wrist_pose, right_wrist_pose]), + **streamer_output.control_data, # Only control & data collection commands pass through + **streamer_output.data_collection_data, + } + ) + + # Run retargeting IK + if "ik_data" in action: + self.retargeting_ik.set_goal(action["ik_data"]) + action["target_upper_body_pose"] = self.retargeting_ik.get_action() + + return action + + def close(self) -> bool: + self.teleop_streamer.stop_streaming() + return True + + def check_activation(self, teleop_data: dict, wait_for_activation: int = 5): + """Activation logic only looks at teleop data commands""" + key = self.keyboard_listener.read_msg() if self.keyboard_listener else "" + toggle_activation_by_keyboard = key == "l" + reset_teleop_policy_by_keyboard = key == "k" + toggle_activation_by_teleop = teleop_data.get("toggle_activation", False) + + if reset_teleop_policy_by_keyboard: + print("Resetting teleop policy") + self.reset() + + if toggle_activation_by_keyboard or toggle_activation_by_teleop: + self.is_active = not self.is_active + if self.is_active: + print("Starting teleop policy") + + if wait_for_activation > 0 and toggle_activation_by_keyboard: + print(f"Sleeping for {wait_for_activation} seconds before starting teleop...") + for i in range(wait_for_activation, 0, -1): + print(f"Starting in {i}...") + time.sleep(1) + + # dda: calibration logic should use current IK data + self.teleop_streamer.calibrate() + print("Teleop policy calibrated") + else: + print("Stopping teleop policy") + + @contextmanager + def activate(self): + try: + yield self + finally: + self.close() + + def handle_keyboard_button(self, keycode): + """ + Handle keyboard input with proper state toggle. + """ + if keycode == "l": + # Toggle start state + self.is_active = not self.is_active + # Reset initialization when stopping + if not self.is_active: + self._initialized = False + if keycode == "k": + print("Resetting teleop policy") + self.reset() + + def activate_policy(self, wait_for_activation: int = 5): + """activate the teleop policy""" + self.is_active = False + self.check_activation( + teleop_data={"toggle_activation": True}, wait_for_activation=wait_for_activation + ) + + def reset(self, wait_for_activation: int = 5, auto_activate: bool = False): + """Reset the teleop policy to the initial state, and re-activate it.""" + self.teleop_streamer.reset() + self.retargeting_ik.reset() + self.is_active = False + self.latest_left_wrist_data = np.eye(4) + self.latest_right_wrist_data = np.eye(4) + self.latest_left_fingers_data = {"position": np.zeros((25, 4, 4))} + self.latest_right_fingers_data = {"position": np.zeros((25, 4, 4))} + + if auto_activate: + self.activate_policy(wait_for_activation) diff --git a/gr00t_wbc/control/policy/wbc_policy_factory.py b/gr00t_wbc/control/policy/wbc_policy_factory.py new file mode 100644 index 0000000..c315e36 --- /dev/null +++ b/gr00t_wbc/control/policy/wbc_policy_factory.py @@ -0,0 +1,65 @@ +import os +from pathlib import Path +import time + +import numpy as np + +import gr00t_wbc +from gr00t_wbc.control.main.constants import DEFAULT_BASE_HEIGHT, DEFAULT_NAV_CMD +from gr00t_wbc.control.policy.g1_gear_wbc_policy import G1GearWbcPolicy +from gr00t_wbc.control.policy.identity_policy import IdentityPolicy +from gr00t_wbc.control.policy.interpolation_policy import InterpolationPolicy + +from .g1_decoupled_whole_body_policy import G1DecoupledWholeBodyPolicy + +WBC_VERSIONS = ["gear_wbc"] + + +def get_wbc_policy( + robot_type, + robot_model, + wbc_config, + init_time=time.monotonic(), +): + current_upper_body_pose = robot_model.get_initial_upper_body_pose() + + if robot_type == "g1": + upper_body_policy_type = wbc_config.get("upper_body_policy_type", "interpolation") + if upper_body_policy_type == "identity": + upper_body_policy = IdentityPolicy() + else: + upper_body_policy = InterpolationPolicy( + init_time=init_time, + init_values={ + "target_upper_body_pose": current_upper_body_pose, + "base_height_command": np.array([DEFAULT_BASE_HEIGHT]), + "navigate_cmd": np.array([DEFAULT_NAV_CMD]), + }, + max_change_rate=wbc_config["upper_body_max_joint_speed"], + ) + + lower_body_policy_type = wbc_config.get("VERSION", "gear_wbc") + if lower_body_policy_type not in ["gear_wbc"]: + raise ValueError( + f"Invalid lower body policy version: {lower_body_policy_type}. " + f"Only 'gear_wbc' is supported." + ) + + # Get the base path to gr00t_wbc and convert to Path object + package_path = Path(os.path.dirname(gr00t_wbc.__file__)) + gear_wbc_config = str(package_path / ".." / wbc_config["GEAR_WBC_CONFIG"]) + if lower_body_policy_type == "gear_wbc": + lower_body_policy = G1GearWbcPolicy( + robot_model=robot_model, + config=gear_wbc_config, + model_path=wbc_config["model_path"], + ) + + wbc_policy = G1DecoupledWholeBodyPolicy( + robot_model=robot_model, + upper_body_policy=upper_body_policy, + lower_body_policy=lower_body_policy, + ) + else: + raise ValueError(f"Invalid robot type: {robot_type}") + return wbc_policy diff --git a/gr00t_wbc/control/robot_model/__init__.py b/gr00t_wbc/control/robot_model/__init__.py new file mode 100644 index 0000000..774d303 --- /dev/null +++ b/gr00t_wbc/control/robot_model/__init__.py @@ -0,0 +1,3 @@ +from .robot_model import ReducedRobotModel, RobotModel + +__all__ = ["RobotModel", "ReducedRobotModel"] diff --git a/gr00t_wbc/control/robot_model/instantiation/__init__.py b/gr00t_wbc/control/robot_model/instantiation/__init__.py new file mode 100644 index 0000000..79b21df --- /dev/null +++ b/gr00t_wbc/control/robot_model/instantiation/__init__.py @@ -0,0 +1,15 @@ +from .g1 import instantiate_g1_robot_model + + +def get_robot_type_and_model(robot: str, enable_waist_ik: bool = False): + """Get the robot type from the robot name.""" + if robot.lower().startswith("g1"): + if "FixedLowerBody" in robot or "FloatingBody" in robot: + waist_location = "upper_body" + elif enable_waist_ik: + waist_location = "lower_and_upper_body" + else: + waist_location = "lower_body" + return "g1", instantiate_g1_robot_model(waist_location=waist_location) + else: + raise ValueError(f"Invalid robot name: {robot}") diff --git a/gr00t_wbc/control/robot_model/instantiation/g1.py b/gr00t_wbc/control/robot_model/instantiation/g1.py new file mode 100644 index 0000000..6191fbc --- /dev/null +++ b/gr00t_wbc/control/robot_model/instantiation/g1.py @@ -0,0 +1,62 @@ +import os +from pathlib import Path +from typing import Literal + +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.robot_model.supplemental_info.g1.g1_supplemental_info import ( + ElbowPose, + G1SupplementalInfo, + WaistLocation, +) + + +def instantiate_g1_robot_model( + waist_location: Literal["lower_body", "upper_body", "lower_and_upper_body"] = "lower_body", + high_elbow_pose: bool = False, +): + """ + Instantiate a G1 robot model with configurable waist location and pose. + + Args: + waist_location: Whether to put waist in "lower_body" (default G1 behavior), + "upper_body" (waist controlled with arms/manipulation via IK), + or "lower_and_upper_body" (waist reference from arms/manipulation + via IK then passed to lower body policy) + high_elbow_pose: Whether to use high elbow pose configuration for default joint positions + + Returns: + RobotModel: Configured G1 robot model + """ + project_root = Path(__file__).resolve().parent.parent.parent.parent.parent + robot_model_config = { + "asset_path": os.path.join(project_root, "gr00t_wbc/control/robot_model/model_data/g1"), + "urdf_path": os.path.join( + project_root, "gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf" + ), + } + assert waist_location in [ + "lower_body", + "upper_body", + "lower_and_upper_body", + ], f"Invalid waist_location: {waist_location}. Must be 'lower_body' or 'upper_body' or 'lower_and_upper_body'" + + # Map string values to enums + waist_location_enum = { + "lower_body": WaistLocation.LOWER_BODY, + "upper_body": WaistLocation.UPPER_BODY, + "lower_and_upper_body": WaistLocation.LOWER_AND_UPPER_BODY, + }[waist_location] + + elbow_pose_enum = ElbowPose.HIGH if high_elbow_pose else ElbowPose.LOW + + # Create single configurable supplemental info instance + robot_model_supplemental_info = G1SupplementalInfo( + waist_location=waist_location_enum, elbow_pose=elbow_pose_enum + ) + + robot_model = RobotModel( + robot_model_config["urdf_path"], + robot_model_config["asset_path"], + supplemental_info=robot_model_supplemental_info, + ) + return robot_model diff --git a/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof.urdf b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof.urdf new file mode 100644 index 0000000..5f91f34 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof.urdf @@ -0,0 +1,1091 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_old.xml b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_old.xml new file mode 100755 index 0000000..4fd64d1 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_old.xml @@ -0,0 +1,568 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf new file mode 100644 index 0000000..e057336 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf @@ -0,0 +1,1497 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.xml b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.xml new file mode 100644 index 0000000..a53cd99 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.xml @@ -0,0 +1,748 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand_rev_1_0_activatedfinger.xml b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand_rev_1_0_activatedfinger.xml new file mode 100644 index 0000000..b508fe4 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand_rev_1_0_activatedfinger.xml @@ -0,0 +1,669 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + --> + + + + + + + + + + + + + diff --git a/gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml b/gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml new file mode 100644 index 0000000..3ac8a3f --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/head_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/head_link.STL new file mode 100644 index 0000000..401f822 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/head_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:005fb67fbd3eff94aa8bf4a6e83238174e9f91b6721f7111594322f223724411 +size 932784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 0000000..7cd7052 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d49e3abc6f5b12e532062cd575b87b5ef40cd2a3fc18f54a1ca5bba4f773d51d +size 71184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 0000000..cb69f65 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4092af943141d4d9f74232f3cfa345afc6565f46a077793b8ae0e68b39dc33f +size 653384 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link.STL new file mode 100644 index 0000000..7fa0527 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fa752198accd104d5c4c3a01382e45165b944fbbc5acce085059223324e5bed3 +size 88784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link_merge.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link_merge.STL new file mode 100644 index 0000000..f287918 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_elbow_link_merge.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:73c8adc6346b900eb6a2ae7159e25dfda1be60cd98affe13536e01ab33a6c1de +size 3136784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_palm_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_palm_link.STL new file mode 100644 index 0000000..c8fcc3e --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:23a486b75bd78a9bf03cec25d84d87f97f3dae038cf21a743b6d469b337e4004 +size 2140184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_0_link.STL new file mode 100644 index 0000000..7fb38c1 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a90c721661c0622685488a3c74d1e122c7da89242d3a1daef75edb83422d05e0 +size 8884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_1_link.STL new file mode 100644 index 0000000..54f7754 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:445c54a45bc13ce36001556f66bc0f49c83cb40321205ae4d676bb2874325684 +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_2_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_2_link.STL new file mode 100644 index 0000000..3e4f124 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3d8dbe5085acfc213d21aa8b0782e89cd79084e9678f3a85fc7b04a86b029db5 +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000..4cf7475 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4725168105ee768ee31638ef22b53f6be2d7641bfd7cfefe803488d884776fa4 +size 181684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_roll_link.STL new file mode 100644 index 0000000..585f604 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:91f25922ee8a7c3152790051bebad17b4d9cd243569c38fe340285ff93a97acf +size 192184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 0000000..b46a741 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a16d88aa6ddac8083aa7ad55ed317bea44b1fa003d314fba88965b7ed0f3b55b +size 296284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_knee_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_knee_link.STL new file mode 100644 index 0000000..2dcf84e --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d92b9e3d3a636761150bb8025e32514c4602b91c7028d523ee42b3e632de477 +size 854884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_rubber_hand.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_rubber_hand.STL new file mode 100644 index 0000000..04a2fa2 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cff2221a690fa69303f61fce68f2d155c1517b52efb6ca9262dd56e0bc6e70fe +size 2287484 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 0000000..926d980 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0d1cfd02fcf0d42f95e678eeca33da3afbcc366ffba5c052847773ec4f31d52 +size 176784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 0000000..4c6840b --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fb9df21687773522598dc384f1a2945c7519f11cbc8bd372a49170316d6eee88 +size 400284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 0000000..89b0e06 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1aa97e9748e924336567992181f78c7cd0652fd52a4afcca3df6b2ef6f9e712e +size 249184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 0000000..f9c9e4f --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b251d8e05047f695d0f536cd78c11973cfa4e78d08cfe82759336cc3471de3a9 +size 85984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 0000000..2097ca3 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:edc387c9a0ba8c2237e9b296d32531426fabeb6f53e58df45c76106bca74148c +size 356184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_rubber_hand.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..7c58819 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e81030abd023bd9e4a308ef376d814a2c12d684d8a7670c335bbd5cd7809c909 +size 3484884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 0000000..692f4b0 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/left_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:83f8fb3a726bf9613d65dd14f0f447cb918c3c95b3938042a0c9c09749267d3b +size 318684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/logo_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/logo_link.STL new file mode 100644 index 0000000..6c25961 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/logo_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8571a0a19bc4916fa55f91449f51d5fdefd751000054865a842449429d5f155b +size 243384 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis.STL new file mode 100644 index 0000000..f98a88d --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ba6bbc888e630550140d3c26763f10206da8c8bd30ed886b8ede41c61f57a31 +size 1060884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis_contour_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis_contour_link.STL new file mode 100644 index 0000000..8025bc0 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/pelvis_contour_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5cc5c2c7a312329e3feeb2b03d3fc09fc29705bd01864f6767e51be959662420 +size 1805184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 0000000..94a586a --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:15be426539ec1be70246d4d82a168806db64a41301af8b35c197a33348c787a9 +size 71184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 0000000..3c38507 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b66222ea56653e627711b56d0a8949b4920da5df091da0ceb343f54e884e3a5 +size 653784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link.STL new file mode 100644 index 0000000..52aa0eb --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1be925d7aa268bb8fddf5362b9173066890c7d32092c05638608126e59d1e2ab +size 88784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link_merge.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link_merge.STL new file mode 100644 index 0000000..b042703 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_elbow_link_merge.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e6def7100e5bbb276c7498c5b7a8c8812f560ca35d6bd09c376ebab93595be7f +size 3058284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_palm_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_palm_link.STL new file mode 100644 index 0000000..fd2f7f0 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:86c0b231cc44477d64a6493e5a427ba16617a00738112dd187c652675b086fb9 +size 2140184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_0_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_0_link.STL new file mode 100644 index 0000000..a385d96 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:544298d0ea1088f5b276a10cc6a6a9e533efdd91594955fdc956c46211d07f83 +size 8884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_1_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_1_link.STL new file mode 100644 index 0000000..c118de7 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0a9a820da8dd10f298778b714f1364216e8a5976f4fd3a05689ea26327d44bf6 +size 475984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_2_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_2_link.STL new file mode 100644 index 0000000..0979fb6 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3f1bfb37668e8f61801c8d25f171fa1949e08666be86c67acad7e0079937cc45 +size 1521784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 0000000..064085f --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e4f3c99d7f4a7d34eadbef9461fc66e3486cb5442db1ec50c86317d459f1a9c6 +size 181284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_roll_link.STL new file mode 100644 index 0000000..6544025 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4c254ef66a356f492947f360dd931965477b631e3fcc841f91ccc46d945d54f6 +size 192684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 0000000..0ad7bee --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e479c2936ca2057e9eb2f7dff6c189b7419d7b8484dea0b298cbb36a2a6aa668 +size 296284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_knee_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_knee_link.STL new file mode 100644 index 0000000..65e8a70 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:63c4008449c9bbe701a6e2b557b7a252e90cf3a5abcf54cee46862b9a69f8ec8 +size 852284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_rubber_hand.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_rubber_hand.STL new file mode 100644 index 0000000..58148fb --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:99533b778bca6246144fa511bb9d4e555e075c641f2a0251e04372869cd99d67 +size 2192684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 0000000..48a1c46 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24cdb387e0128dfe602770a81c56cdce3a0181d34d039a11d1aaf8819b7b8c02 +size 176784 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 0000000..2a5d22f --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:962b97c48f9ce9e8399f45dd9522e0865d19aa9fd299406b2d475a8fc4a53e81 +size 401884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 0000000..0882a56 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0b76489271da0c72461a344c9ffb0f0c6e64f019ea5014c1624886c442a2fe5 +size 249984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_pitch_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 0000000..2efd25c --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d22f8f3b3127f15a63e5be1ee273cd5075786c3142f1c3d9f76cbf43d2a26477 +size 79584 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 0000000..77d23a7 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a7ee9212ff5b94d6cb7f52bb1bbf3f352194d5b598acff74f4c77d340c5b344f +size 356084 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_rubber_hand.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..6f122af --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0729aff1ac4356f9314de13a46906267642e58bc47f0d8a7f17f6590a6242ccf +size 3481584 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 0000000..77edbb4 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/right_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc9dece2d12509707e0057ba2e48df8f3d56db0c79410212963a25e8a50f61a6 +size 341484 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_link.STL new file mode 100644 index 0000000..cc2cbbf --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:82be7f93e85b3d303a1d1e1847e2c916939bd61c424ed1ebd28691ec33909dd1 +size 203584 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_rod_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_rod_link.STL new file mode 100644 index 0000000..dd439bf --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_L_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c10de1effa7ea797ac268006aa2a739036c7e1f326b2012d711ee2c20c5a6e96 +size 74884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_link.STL new file mode 100644 index 0000000..422ffe4 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:54ded433a3a0c76027365856fdbd55215643de88846f7d436598a4071e682725 +size 203584 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_rod_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_rod_link.STL new file mode 100644 index 0000000..6df46c8 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_constraint_R_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cb83fd38a9f06c99e3f301c70f12d94ae770a8f0cf9501f83580a27f908990b4 +size 74884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link.STL new file mode 100644 index 0000000..e4fb87c --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e96d023f0368a4e3450b86ca5d4f10227d8141156a373e7da8cb3c93266523e0 +size 2232984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link_rev_1_0.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link_rev_1_0.STL new file mode 100644 index 0000000..836b992 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/torso_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11ddb46f2098efbbd8816b1d65893632d8e78be936376c7cdcd6771899ccc723 +size 2570584 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_L.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_L.STL new file mode 100644 index 0000000..6ec689b --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ebafdcb4de6871113f0ca2c356618d6e46b1d50f6c0bf9e37f47b9d8e100d99 +size 114684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_R.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_R.STL new file mode 100644 index 0000000..69fd76a --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_constraint_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:791902a291ffbd35ab97383b7b44ea5d975de7c80eef838797c970790b382ca9 +size 114684 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link.STL new file mode 100644 index 0000000..007f56d --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34f0aa73f41131230be4d25876c944fdf6c24d62553f199ff8b980c15e8913df +size 24184 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link_rev_1_0.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link_rev_1_0.STL new file mode 100644 index 0000000..36a5a70 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_roll_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b67c347a05abc3e8ddae600d98a082bee273bb39f8e651647708b0a7140a8a97 +size 85884 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_support_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_support_link.STL new file mode 100644 index 0000000..4a50f94 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_support_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fae9e1bb609848a1667d32eed8d6083ae443538a306843056a2a660f1b2926a +size 150484 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link.STL new file mode 100644 index 0000000..c049deb --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2883f20e03f09b669b5b4ce10677ee6b5191c0934b584d7cbaef2d0662856ffb +size 336284 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link_rev_1_0.STL b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link_rev_1_0.STL new file mode 100644 index 0000000..dc628fb --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/meshes/waist_yaw_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec6db442b11f25eed898b5add07940c85d804f300de24dcbd264ccd8be7d554c +size 619984 diff --git a/gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml b/gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml new file mode 100644 index 0000000..cf4d4da --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml b/gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml new file mode 100644 index 0000000..cb61b70 --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof.xml b/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof.xml new file mode 100644 index 0000000..0b60f2f --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof_activated3dex.xml b/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof_activated3dex.xml new file mode 100644 index 0000000..2916dfb --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/scene_29dof_activated3dex.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/control/robot_model/model_data/g1/scene_43dof.xml b/gr00t_wbc/control/robot_model/model_data/g1/scene_43dof.xml new file mode 100644 index 0000000..d66b23d --- /dev/null +++ b/gr00t_wbc/control/robot_model/model_data/g1/scene_43dof.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/control/robot_model/robot_model.py b/gr00t_wbc/control/robot_model/robot_model.py new file mode 100644 index 0000000..a3d94c8 --- /dev/null +++ b/gr00t_wbc/control/robot_model/robot_model.py @@ -0,0 +1,772 @@ +from typing import List, Optional, Set, Union + +import numpy as np +import pinocchio as pin + +from gr00t_wbc.control.robot_model.supplemental_info import RobotSupplementalInfo + + +class RobotModel: + def __init__( + self, + urdf_path, + asset_path, + set_floating_base=False, + supplemental_info: Optional[RobotSupplementalInfo] = None, + ): + self.pinocchio_wrapper = pin.RobotWrapper.BuildFromURDF( + filename=urdf_path, + package_dirs=[asset_path], + root_joint=pin.JointModelFreeFlyer() if set_floating_base else None, + ) + self.is_floating_base_model = set_floating_base + + self.joint_to_dof_index = {} + # Assume we only have single-dof joints + # First two names correspond to universe and floating base joints + names = ( + self.pinocchio_wrapper.model.names[2:] + if set_floating_base + else self.pinocchio_wrapper.model.names[1:] + ) + for name in names: + j_id = self.pinocchio_wrapper.model.getJointId(name) + jmodel = self.pinocchio_wrapper.model.joints[j_id] + self.joint_to_dof_index[name] = jmodel.idx_q + + # Store joint limits only for actual joints (excluding floating base) + # if set floating base is true and the robot can move in the world + # then we don't want to impose joint limits for the 7 dofs corresponding + # to the floating base dofs. + root_nq = 7 if set_floating_base else 0 + self.upper_joint_limits = self.pinocchio_wrapper.model.upperPositionLimit[root_nq:].copy() + self.lower_joint_limits = self.pinocchio_wrapper.model.lowerPositionLimit[root_nq:].copy() + + # Set up supplemental info if provided + self.supplemental_info = supplemental_info + if self.supplemental_info is not None: + # Cache indices for body and hand actuated joints separately + self._body_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.body_actuated_joints + ] + self._left_hand_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.left_hand_actuated_joints + ] + self._right_hand_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.right_hand_actuated_joints + ] + self._hand_actuated_joint_indices = ( + self._left_hand_actuated_joint_indices + self._right_hand_actuated_joint_indices + ) + + # Cache indices for joint groups, handling nested groups + self._joint_group_indices = {} + for group_name, group_info in self.supplemental_info.joint_groups.items(): + indices = [] + # Add indices for direct joints + indices.extend([self.dof_index(name) for name in group_info["joints"]]) + # Add indices from subgroups + for subgroup_name in group_info["groups"]: + indices.extend(self.get_joint_group_indices(subgroup_name)) + self._joint_group_indices[group_name] = sorted(set(indices)) + + # Update joint limits from supplemental info if available + if ( + hasattr(self.supplemental_info, "joint_limits") + and self.supplemental_info.joint_limits + ): + for joint_name, limits in self.supplemental_info.joint_limits.items(): + if joint_name in self.joint_to_dof_index: + idx = self.joint_to_dof_index[joint_name] - root_nq + self.lower_joint_limits[idx] = limits[0] + self.upper_joint_limits[idx] = limits[1] + + # Initialize default body pose + self.default_body_pose = self.q_zero.copy() + + # Update with supplemental info if available + if self.supplemental_info is not None: + default_joint_q = self.supplemental_info.default_joint_q + for joint, joint_values in default_joint_q.items(): + # Get the joint name mapping for this type + joint_mapping = self.supplemental_info.joint_name_mapping[joint] + + # Handle both single joint names and left/right mappings + if isinstance(joint_mapping, str): + # Single joint (e.g., waist joints) + if joint_mapping in self.joint_to_dof_index: + joint_idx = self.dof_index(joint_mapping) + self.default_body_pose[joint_idx] = ( + joint_values # joint_values is the value for single joints + ) + else: + # Left/right mapping (e.g., arm joints) + for side, value in joint_values.items(): + if side in joint_mapping and joint_mapping[side] in self.joint_to_dof_index: + joint_idx = self.dof_index(joint_mapping[side]) + self.default_body_pose[joint_idx] = value + + # Initialize initial body pose + self.initial_body_pose = self.default_body_pose.copy() + + @property + def num_dofs(self) -> int: + """Get the number of degrees of freedom of the robot (floating base pose + joints).""" + return self.pinocchio_wrapper.model.nq + + @property + def q_zero(self) -> np.ndarray: + """Get the zero pose of the robot.""" + return self.pinocchio_wrapper.q0 + + @property + def joint_names(self) -> List[str]: + """Get the names of the joints of the robot.""" + return list(self.joint_to_dof_index.keys()) + + @property + def num_joints(self) -> int: + """Get the number of joints of the robot.""" + return len(self.joint_to_dof_index) + + def dof_index(self, joint_name: str) -> int: + """ + Get the index in the degrees of freedom vector corresponding + to the single-DoF joint with name `joint_name`. + """ + if joint_name not in self.joint_to_dof_index: + raise ValueError( + f"Unknown joint name: '{joint_name}'. " + f"Available joints: {list(self.joint_to_dof_index.keys())}" + ) + return self.joint_to_dof_index[joint_name] + + def get_body_actuated_joint_indices(self) -> List[int]: + """ + Get the indices of body actuated joints in the full configuration. + Ordering is that of the actuated joints as defined in the supplemental info. + Requires supplemental_info to be provided. + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + return self._body_actuated_joint_indices + + def get_hand_actuated_joint_indices(self, side: str = "both") -> List[int]: + """ + Get the indices of hand actuated joints in the full configuration. + Ordering is that of the actuated joints as defined in the supplemental info. + Requires supplemental_info to be provided. + + Args: + side: String specifying which hand to get indices for ('left', 'right', or 'both') + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + if side.lower() == "both": + return self._hand_actuated_joint_indices + elif side.lower() == "left": + return self._left_hand_actuated_joint_indices + elif side.lower() == "right": + return self._right_hand_actuated_joint_indices + else: + raise ValueError("side must be 'left', 'right', or 'both'") + + def get_joint_group_indices(self, group_names: Union[str, Set[str]]) -> List[int]: + """ + Get the indices of joints in one or more groups in the full configuration. + Requires supplemental_info to be provided. + The returned indices are sorted in ascending order, so that the joint ordering + of the full model is preserved. + + Args: + group_names: Either a single group name (str) or a set of group names (Set[str]) + + Returns: + List of joint indices in sorted order with no duplicates + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Convert single string to set for uniform handling + if isinstance(group_names, str): + group_names = {group_names} + + # Collect indices from all groups + all_indices = set() + for group_name in group_names: + if group_name not in self._joint_group_indices: + raise ValueError(f"Unknown joint group: {group_name}") + all_indices.update(self._joint_group_indices[group_name]) + + return sorted(all_indices) + + def cache_forward_kinematics(self, q: np.ndarray, auto_clip=True) -> None: + """ + Perform forward kinematics to update the pose of every joint and frame + in the Pinocchio data structures for the given configuration `q`. + + :param q: A numpy array of shape (num_dofs,) representing the robot configuration. + """ + if q.shape[0] != self.num_dofs: + raise ValueError(f"Expected q of length {self.num_dofs}, got {q.shape[0]} instead.") + + # Apply auto-clip if enabled + if auto_clip: + q = self.clip_configuration(q) + + pin.framesForwardKinematics(self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q) + + def compute_gravity_compensation_torques( + self, q: np.ndarray, joint_groups: Union[str, List[str], Set[str]] = None, auto_clip=True + ) -> np.ndarray: + """ + Compute gravity compensation torques for specified joint groups using pinocchio. + + :param q: Robot configuration (joint positions) + :param joint_groups: Joint groups to compensate (e.g., "arms", ["left_arm", "waist"], + {"left_arm", "waist"}). If None, compensates all joints + :param auto_clip: Whether to automatically clip joint values to limits + :return: Array of gravity compensation torques for all DOFs (zero for non-compensated joints) + """ + if q.shape[0] != self.num_dofs: + raise ValueError(f"Expected q of length {self.num_dofs}, got {q.shape[0]} instead.") + + # Apply auto-clip if enabled + if auto_clip: + q = self.clip_configuration(q) + + try: + # Cache forward kinematics for the current configuration + self.cache_forward_kinematics(q, auto_clip=False) # Already clipped if needed + + # Compute gravity vector using RNEA with zero velocity and acceleration + v = np.zeros(self.num_dofs) + a = np.zeros(self.num_dofs) + + gravity_torques_full = pin.rnea( + self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q, v, a + ) + + # If no joint groups specified, return full gravity torques + if joint_groups is None: + return gravity_torques_full + + # Convert list to set for get_joint_group_indices compatibility + if isinstance(joint_groups, list): + joint_groups = set(joint_groups) + + # Get joint indices for specified groups - get_joint_group_indices handles str and Set[str] + try: + compensated_joint_indices = self.get_joint_group_indices(joint_groups) + except ValueError as e: + raise ValueError(f"Error resolving joint groups {joint_groups}: {e}") + + # Create mask for joints that should receive gravity compensation + compensation_mask = np.zeros(self.num_dofs, dtype=bool) + for joint_idx in compensated_joint_indices: + if 0 <= joint_idx < len(compensation_mask): + compensation_mask[joint_idx] = True + + # Apply mask to only compensate specified joints + compensated_torques = np.zeros_like(gravity_torques_full) + compensated_torques[compensation_mask] = gravity_torques_full[compensation_mask] + + return compensated_torques + + except Exception as e: + raise RuntimeError(f"Error computing gravity compensation: {e}") + + def clip_configuration(self, q: np.ndarray, margin: float = 1e-6) -> np.ndarray: + """ + Clip the configuration to stay within joint limits with a small tolerance. + + :param q: Configuration to clip + :param margin: Tolerance to keep away from joint limits + :return: Clipped configuration + """ + q_clipped = q.copy() + + # Only clip joint positions, not floating base + root_nq = 7 if self.is_floating_base_model else 0 + q_clipped[root_nq:] = np.clip( + q[root_nq:], self.lower_joint_limits + margin, self.upper_joint_limits - margin + ) + + return q_clipped + + def frame_placement(self, frame_name: str) -> pin.SE3: + """ + Returns the SE3 transform of the specified frame in the world coordinate system. + Note: make sure cache_forward_kinematics() has been previously called. + + :param frame_name: Name of the frame, e.g. "link_elbow_frame", "hand_imu_frame", etc. + :return: A pin.SE3 object representing the pose of the frame. + """ + model = self.pinocchio_wrapper.model + data = self.pinocchio_wrapper.data + + frame_id = model.getFrameId(frame_name) + if frame_id < 0 or frame_id >= len(model.frames): + valid_frames = [f.name for f in model.frames] + raise ValueError(f"Unknown frame '{frame_name}'. Valid frames: {valid_frames}") + + # Pinocchio's data.oMf[frame_id] is a pin.SE3. + return data.oMf[frame_id].copy() + + def get_body_actuated_joints(self, q: np.ndarray) -> np.ndarray: + """ + Get the configuration of body actuated joints from a full configuration. + + :param q: Configuration in full space + :return: Configuration of body actuated joints + """ + indices = self.get_body_actuated_joint_indices() + + return q[indices] + + def get_hand_actuated_joints(self, q: np.ndarray, side: str = "both") -> np.ndarray: + """ + Get the configuration of hand actuated joints from a full configuration. + + Args: + q: Configuration in full space + side: String specifying which hand to get joints for ('left', 'right', or 'both') + """ + indices = self.get_hand_actuated_joint_indices(side) + return q[indices] + + def get_configuration_from_actuated_joints( + self, + body_actuated_joint_values: np.ndarray, + hand_actuated_joint_values: Optional[np.ndarray] = None, + left_hand_actuated_joint_values: Optional[np.ndarray] = None, + right_hand_actuated_joint_values: Optional[np.ndarray] = None, + ) -> np.ndarray: + """ + Get the full configuration from the body and hand actuated joint configurations. + Can specify either both hands together or left and right hands separately. + + Args: + body_actuated_joint_values: Configuration of body actuated joints + hand_actuated_joint_values: Configuration of both hands' actuated joints (optional) + left_hand_actuated_joint_values: Configuration of left hand actuated joints (optional) + right_hand_actuated_joint_values: Configuration of right hand actuated joints (optional) + + Returns: + Full configuration including body and hand joints + """ + q = self.pinocchio_wrapper.q0.copy() + q[self.get_body_actuated_joint_indices()] = body_actuated_joint_values + + # Handle hand configurations + if hand_actuated_joint_values is not None: + # Use combined hand configuration + q[self.get_hand_actuated_joint_indices("both")] = hand_actuated_joint_values + else: + # Use separate hand configurations + if left_hand_actuated_joint_values is not None: + q[self.get_hand_actuated_joint_indices("left")] = left_hand_actuated_joint_values + if right_hand_actuated_joint_values is not None: + q[self.get_hand_actuated_joint_indices("right")] = right_hand_actuated_joint_values + + return q + + def reset_forward_kinematics(self) -> None: + """ + Reset the forward kinematics to the initial configuration. + """ + self.cache_forward_kinematics(self.q_zero) + + def get_initial_upper_body_pose(self) -> np.ndarray: + """ + Get the initial upper body pose of the robot. + """ + return self.initial_body_pose[self.get_joint_group_indices("upper_body")] + + def get_default_body_pose(self) -> np.ndarray: + """ + Get the default body pose of the robot. + """ + return self.default_body_pose + + def set_initial_body_pose(self, q: np.ndarray, q_idx=None) -> None: + """ + Set the initial body pose of the robot. + """ + if q_idx is None: + self.initial_body_pose = q + else: + self.initial_body_pose[q_idx] = q + + +class ReducedRobotModel(RobotModel): + """ + A class that creates a reduced order robot model by fixing certain joints. + This class maintains a mapping between the reduced state space and the full state space. + """ + + def __init__( + self, + full_robot_model: RobotModel, + fixed_joints: List[str], + fixed_values: Optional[List[float]] = None, + ): + """ + Create a reduced order robot model by fixing specified joints. + + :param full_robot_model: The original robot model + :param fixed_joints: List of joint names to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + """ + self.full_robot = full_robot_model + self.supplemental_info = full_robot_model.supplemental_info + + # If fixed_values is None, use q0 from the full robot model + if fixed_values is None: + fixed_values = [] + for joint_name in fixed_joints: + full_idx = full_robot_model.dof_index(joint_name) + fixed_values.append(full_robot_model.pinocchio_wrapper.q0[full_idx]) + elif len(fixed_joints) != len(fixed_values): + raise ValueError("fixed_joints and fixed_values must have the same length") + + # Store fixed joints and their values + self.fixed_joints = fixed_joints + self.fixed_values = fixed_values + + # Create mapping between reduced and full state spaces + self.reduced_to_full = [] + self.full_to_reduced = {} + + # Initialize with floating base indices if present + if full_robot_model.is_floating_base_model: + self.reduced_to_full.extend(range(7)) # Floating base indices + for i in range(7): + self.full_to_reduced[i] = i + + # Add active joint indices + for joint_name in full_robot_model.joint_names: + if joint_name not in fixed_joints: + full_idx = full_robot_model.dof_index(joint_name) + reduced_idx = len(self.reduced_to_full) + self.reduced_to_full.append(full_idx) + self.full_to_reduced[full_idx] = reduced_idx + + # Create a reduced Pinocchio model using buildReducedModel + # First, get the list of joint IDs to lock + locked_joint_ids = [] + for joint_name in fixed_joints: + joint_id = full_robot_model.pinocchio_wrapper.model.getJointId(joint_name) + if (full_robot_model.is_floating_base_model and joint_id > 1) or ( + not full_robot_model.is_floating_base_model and joint_id > 0 + ): + locked_joint_ids.append(joint_id) + + # First build the reduced kinematic model + reduced_model = pin.buildReducedModel( + full_robot_model.pinocchio_wrapper.model, + locked_joint_ids, + full_robot_model.pinocchio_wrapper.q0, + ) + + # Then build the reduced geometry models using the reduced kinematic model + self.pinocchio_wrapper = pin.RobotWrapper( + model=reduced_model, + ) + + # Create joint to dof index mapping + self.joint_to_dof_index = {} + # Assume we only have single-dof joints + # First two names correspond to universe and floating base joints + names = ( + self.pinocchio_wrapper.model.names[2:] + if self.full_robot.is_floating_base_model + else self.pinocchio_wrapper.model.names[1:] + ) + for name in names: + j_id = self.pinocchio_wrapper.model.getJointId(name) + jmodel = self.pinocchio_wrapper.model.joints[j_id] + self.joint_to_dof_index[name] = jmodel.idx_q + + # Initialize joint limits + root_nq = 7 if self.full_robot.is_floating_base_model else 0 + self.lower_joint_limits = self.pinocchio_wrapper.model.lowerPositionLimit[root_nq:].copy() + self.upper_joint_limits = self.pinocchio_wrapper.model.upperPositionLimit[root_nq:].copy() + + # Update joint limits from supplemental info if available + if self.supplemental_info is not None: + if ( + hasattr(self.supplemental_info, "joint_limits") + and self.supplemental_info.joint_limits + ): + for joint_name, limits in self.supplemental_info.joint_limits.items(): + if joint_name in self.joint_to_dof_index: + idx = self.joint_to_dof_index[joint_name] - root_nq + self.lower_joint_limits[idx] = limits[0] + self.upper_joint_limits[idx] = limits[1] + + # Get full indices for body and hand actuated joints + full_body_indices = full_robot_model.get_body_actuated_joint_indices() + full_hand_indices = full_robot_model.get_hand_actuated_joint_indices("both") + full_left_hand_indices = full_robot_model.get_hand_actuated_joint_indices("left") + full_right_hand_indices = full_robot_model.get_hand_actuated_joint_indices("right") + + # Map to reduced indices + self._body_actuated_joint_indices = [] + for idx in full_body_indices: + if idx in self.full_to_reduced: + self._body_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._hand_actuated_joint_indices = [] + for idx in full_hand_indices: + if idx in self.full_to_reduced: + self._hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._left_hand_actuated_joint_indices = [] + for idx in full_left_hand_indices: + if idx in self.full_to_reduced: + self._left_hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._right_hand_actuated_joint_indices = [] + for idx in full_right_hand_indices: + if idx in self.full_to_reduced: + self._right_hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + # Cache indices for joint groups in reduced space + self._joint_group_indices = {} + for group_name in self.supplemental_info.joint_groups: + full_indices = full_robot_model.get_joint_group_indices(group_name) + reduced_indices = [] + for idx in full_indices: + if idx in self.full_to_reduced: + reduced_indices.append(self.full_to_reduced[idx]) + self._joint_group_indices[group_name] = sorted(set(reduced_indices)) + + # Initialize default body pose in reduced space + self.default_body_pose = self.full_to_reduced_configuration( + full_robot_model.default_body_pose + ) + + # Initialize initial body pose in reduced space + self.initial_body_pose = self.full_to_reduced_configuration( + full_robot_model.initial_body_pose + ) + + @property + def num_joints(self) -> int: + """Get the number of active joints in the reduced model.""" + return len(self.joint_names) + + @property + def joint_names(self) -> List[str]: + """Get the names of the active joints in the reduced model.""" + return [name for name in self.full_robot.joint_names if name not in self.fixed_joints] + + @classmethod + def from_fixed_groups( + cls, + full_robot_model: RobotModel, + fixed_group_names: List[str], + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints in specified groups. + + :param full_robot_model: The original robot model + :param fixed_group_names: List of joint group names to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + if full_robot_model.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Get all joints in the groups, including those from subgroups + fixed_joints = set() # Use a set to avoid duplicates + + for group_name in fixed_group_names: + if group_name not in full_robot_model.supplemental_info.joint_groups: + raise ValueError(f"Unknown joint group: {group_name}") + + group_info = full_robot_model.supplemental_info.joint_groups[group_name] + + # Add direct joints + fixed_joints.update(group_info["joints"]) + + # Add joints from subgroups + for subgroup_name in group_info["groups"]: + subgroup_joints = full_robot_model.get_joint_group_indices(subgroup_name) + fixed_joints.update([full_robot_model.joint_names[idx] for idx in subgroup_joints]) + + # Convert set back to list for compatibility with the original constructor + return cls(full_robot_model, list(fixed_joints), fixed_values) + + @classmethod + def from_fixed_group( + cls, + full_robot_model: RobotModel, + fixed_group_name: str, + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints in a specified group. + This is a convenience method that calls from_fixed_groups with a single group. + + :param full_robot_model: The original robot model + :param fixed_group_name: Name of the joint group to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + return cls.from_fixed_groups(full_robot_model, [fixed_group_name], fixed_values) + + @classmethod + def from_active_group( + cls, + full_robot_model: RobotModel, + active_group_name: str, + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints EXCEPT those in the specified group. + This is a convenience method that calls from_active_groups with a single group. + + :param full_robot_model: The original robot model + :param active_group_name: Name of the joint group to keep active (all other joints will be fixed) + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + return cls.from_active_groups(full_robot_model, [active_group_name], fixed_values) + + @classmethod + def from_active_groups( + cls, + full_robot_model: RobotModel, + active_group_names: List[str], + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints EXCEPT those in the specified groups. + This is useful when you want to keep multiple groups active and fix everything else. + + :param full_robot_model: The original robot model + :param active_group_names: List of joint group names to keep active (all other joints will be fixed) + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + if full_robot_model.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Get all joints in the active groups, including those from subgroups + active_joints = set() + + def add_group_joints(group_name: str): + if group_name not in full_robot_model.supplemental_info.joint_groups: + raise ValueError(f"Unknown joint group: {group_name}") + + group_info = full_robot_model.supplemental_info.joint_groups[group_name] + + # Add direct joints + if "joints" in group_info: + active_joints.update(group_info["joints"]) + + # Add joints from subgroups + if "groups" in group_info: + for subgroup_name in group_info["groups"]: + add_group_joints(subgroup_name) + + for group_name in active_group_names: + add_group_joints(group_name) + + # Get all joints from the model + all_joints = set(full_robot_model.joint_names) + + # The fixed joints are all joints minus the active joints + fixed_joints = list(all_joints - active_joints) + + return cls(full_robot_model, fixed_joints, fixed_values) + + def reduced_to_full_configuration(self, q_reduced: np.ndarray) -> np.ndarray: + """ + Convert a reduced configuration to the full configuration space. + + :param q_reduced: Configuration in reduced space + :return: Configuration in full space with fixed joints set to their fixed values + """ + if q_reduced.shape[0] != self.num_dofs: + raise ValueError( + f"Expected q_reduced of length {self.num_dofs}, got {q_reduced.shape[0]} instead" + ) + + q_full = np.zeros(self.full_robot.num_dofs) + + # Set active joints + for reduced_idx, full_idx in enumerate(self.reduced_to_full): + q_full[full_idx] = q_reduced[reduced_idx] + + # Set fixed joints + for joint_name, value in zip(self.fixed_joints, self.fixed_values): + full_idx = self.full_robot.dof_index(joint_name) + q_full[full_idx] = value + + return q_full + + def full_to_reduced_configuration(self, q_full: np.ndarray) -> np.ndarray: + """ + Convert a full configuration to the reduced configuration space. + + :param q_full: Configuration in full space + :return: Configuration in reduced space + """ + if q_full.shape[0] != self.full_robot.num_dofs: + raise ValueError( + f"Expected q_full of length {self.full_robot.num_dofs}, got {q_full.shape[0]} instead" + ) + + q_reduced = np.zeros(self.num_dofs) + + # Copy active joints + for reduced_idx, full_idx in enumerate(self.reduced_to_full): + q_reduced[reduced_idx] = q_full[full_idx] + + return q_reduced + + def cache_forward_kinematics(self, q_reduced: np.ndarray, auto_clip=True) -> None: + """ + Perform forward kinematics using the reduced configuration. + + :param q_reduced: Configuration in reduced space + """ + # First update the full robot's forward kinematics + q_full = self.reduced_to_full_configuration(q_reduced) + self.full_robot.cache_forward_kinematics(q_full, auto_clip) + + # Then update the reduced model's forward kinematics + pin.framesForwardKinematics( + self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q_reduced + ) + + def clip_configuration(self, q_reduced: np.ndarray, margin: float = 1e-6) -> np.ndarray: + """ + Clip the reduced configuration to stay within joint limits with a small tolerance. + + :param q_reduced: Configuration to clip + :param margin: Tolerance to keep away from joint limits + :return: Clipped configuration + """ + q_full = self.reduced_to_full_configuration(q_reduced) + q_full_clipped = self.full_robot.clip_configuration(q_full, margin) + return self.full_to_reduced_configuration(q_full_clipped) + + def reset_forward_kinematics(self): + """ + Reset the forward kinematics to the initial configuration. + """ + # Reset full robot's forward kinematics + self.full_robot.reset_forward_kinematics() + # Reset reduced model's forward kinematics + self.cache_forward_kinematics(self.q_zero) diff --git a/gr00t_wbc/control/robot_model/supplemental_info/__init__.py b/gr00t_wbc/control/robot_model/supplemental_info/__init__.py new file mode 100644 index 0000000..b9c987d --- /dev/null +++ b/gr00t_wbc/control/robot_model/supplemental_info/__init__.py @@ -0,0 +1,5 @@ +from gr00t_wbc.control.robot_model.supplemental_info.robot_supplemental_info import ( + RobotSupplementalInfo, +) + +__all__ = ["RobotSupplementalInfo"] diff --git a/gr00t_wbc/control/robot_model/supplemental_info/g1/__init__.py b/gr00t_wbc/control/robot_model/supplemental_info/g1/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/robot_model/supplemental_info/g1/g1_supplemental_info.py b/gr00t_wbc/control/robot_model/supplemental_info/g1/g1_supplemental_info.py new file mode 100644 index 0000000..a68b4cd --- /dev/null +++ b/gr00t_wbc/control/robot_model/supplemental_info/g1/g1_supplemental_info.py @@ -0,0 +1,330 @@ +from dataclasses import dataclass +from enum import Enum + +import numpy as np + +from gr00t_wbc.control.robot_model.supplemental_info.robot_supplemental_info import ( + RobotSupplementalInfo, +) + + +class WaistLocation(Enum): + """Enum for waist location configuration.""" + + LOWER_BODY = "lower_body" + UPPER_BODY = "upper_body" + LOWER_AND_UPPER_BODY = "lower_and_upper_body" + + +class ElbowPose(Enum): + """Enum for elbow pose configuration.""" + + LOW = "low" + HIGH = "high" + + +@dataclass +class G1SupplementalInfo(RobotSupplementalInfo): + """ + Supplemental information for the G1 robot. + + Args: + waist_location: Where to place waist joints in the joint groups + elbow_pose: Which elbow pose configuration to use for default joint positions + """ + + def __init__( + self, + waist_location: WaistLocation = WaistLocation.LOWER_BODY, + elbow_pose: ElbowPose = ElbowPose.LOW, + ): + name = "G1_G1ThreeFinger" + + # Define all actuated joints + body_actuated_joints = [ + # Left leg + "left_hip_pitch_joint", + "left_hip_roll_joint", + "left_hip_yaw_joint", + "left_knee_joint", + "left_ankle_pitch_joint", + "left_ankle_roll_joint", + # Right leg + "right_hip_pitch_joint", + "right_hip_roll_joint", + "right_hip_yaw_joint", + "right_knee_joint", + "right_ankle_pitch_joint", + "right_ankle_roll_joint", + # Waist + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + # Left arm + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + # Right arm + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ] + + left_hand_actuated_joints = [ + # Left hand + "left_hand_thumb_0_joint", + "left_hand_thumb_1_joint", + "left_hand_thumb_2_joint", + "left_hand_index_0_joint", + "left_hand_index_1_joint", + "left_hand_middle_0_joint", + "left_hand_middle_1_joint", + ] + + right_hand_actuated_joints = [ + # Right hand + "right_hand_thumb_0_joint", + "right_hand_thumb_1_joint", + "right_hand_thumb_2_joint", + "right_hand_index_0_joint", + "right_hand_index_1_joint", + "right_hand_middle_0_joint", + "right_hand_middle_1_joint", + ] + + # Define joint limits from URDF + joint_limits = { + # Left leg + "left_hip_pitch_joint": [-2.5307, 2.8798], + "left_hip_roll_joint": [-0.5236, 2.9671], + "left_hip_yaw_joint": [-2.7576, 2.7576], + "left_knee_joint": [-0.087267, 2.8798], + "left_ankle_pitch_joint": [-0.87267, 0.5236], + "left_ankle_roll_joint": [-0.2618, 0.2618], + # Right leg + "right_hip_pitch_joint": [-2.5307, 2.8798], + "right_hip_roll_joint": [-2.9671, 0.5236], + "right_hip_yaw_joint": [-2.7576, 2.7576], + "right_knee_joint": [-0.087267, 2.8798], + "right_ankle_pitch_joint": [-0.87267, 0.5236], + "right_ankle_roll_joint": [-0.2618, 0.2618], + # Waist + "waist_yaw_joint": [-2.618, 2.618], + "waist_roll_joint": [-0.52, 0.52], + "waist_pitch_joint": [-0.52, 0.52], + # Left arm + "left_shoulder_pitch_joint": [-3.0892, 2.6704], + "left_shoulder_roll_joint": [0.19, 2.2515], + "left_shoulder_yaw_joint": [-2.618, 2.618], + "left_elbow_joint": [-1.0472, 2.0944], + "left_wrist_roll_joint": [-1.972222054, 1.972222054], + "left_wrist_pitch_joint": [-1.614429558, 1.614429558], + "left_wrist_yaw_joint": [-1.614429558, 1.614429558], + # Right arm + "right_shoulder_pitch_joint": [-3.0892, 2.6704], + "right_shoulder_roll_joint": [-2.2515, -0.19], + "right_shoulder_yaw_joint": [-2.618, 2.618], + "right_elbow_joint": [-1.0472, 2.0944], + "right_wrist_roll_joint": [-1.972222054, 1.972222054], + "right_wrist_pitch_joint": [-1.614429558, 1.614429558], + "right_wrist_yaw_joint": [-1.614429558, 1.614429558], + # Left hand + "left_hand_thumb_0_joint": [-1.04719755, 1.04719755], + "left_hand_thumb_1_joint": [-0.72431163, 1.04719755], + "left_hand_thumb_2_joint": [0, 1.74532925], + "left_hand_index_0_joint": [-1.57079632, 0], + "left_hand_index_1_joint": [-1.74532925, 0], + "left_hand_middle_0_joint": [-1.57079632, 0], + "left_hand_middle_1_joint": [-1.74532925, 0], + # Right hand + "right_hand_thumb_0_joint": [-1.04719755, 1.04719755], + "right_hand_thumb_1_joint": [-0.72431163, 1.04719755], + "right_hand_thumb_2_joint": [0, 1.74532925], + "right_hand_index_0_joint": [-1.57079632, 0], + "right_hand_index_1_joint": [-1.74532925, 0], + "right_hand_middle_0_joint": [-1.57079632, 0], + "right_hand_middle_1_joint": [-1.74532925, 0], + } + + # Define joint groups + joint_groups = { + # Body groups + "waist": { + "joints": ["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"], + "groups": [], + }, + # Leg groups + "left_leg": { + "joints": [ + "left_hip_pitch_joint", + "left_hip_roll_joint", + "left_hip_yaw_joint", + "left_knee_joint", + "left_ankle_pitch_joint", + "left_ankle_roll_joint", + ], + "groups": [], + }, + "right_leg": { + "joints": [ + "right_hip_pitch_joint", + "right_hip_roll_joint", + "right_hip_yaw_joint", + "right_knee_joint", + "right_ankle_pitch_joint", + "right_ankle_roll_joint", + ], + "groups": [], + }, + "legs": {"joints": [], "groups": ["left_leg", "right_leg"]}, + # Arm groups + "left_arm": { + "joints": [ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ], + "groups": [], + }, + "right_arm": { + "joints": [ + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ], + "groups": [], + }, + "arms": {"joints": [], "groups": ["left_arm", "right_arm"]}, + # Hand groups + "left_hand": { + "joints": [ + "left_hand_index_0_joint", + "left_hand_index_1_joint", + "left_hand_middle_0_joint", + "left_hand_middle_1_joint", + "left_hand_thumb_0_joint", + "left_hand_thumb_1_joint", + "left_hand_thumb_2_joint", + ], + "groups": [], + }, + "right_hand": { + "joints": [ + "right_hand_index_0_joint", + "right_hand_index_1_joint", + "right_hand_middle_0_joint", + "right_hand_middle_1_joint", + "right_hand_thumb_0_joint", + "right_hand_thumb_1_joint", + "right_hand_thumb_2_joint", + ], + "groups": [], + }, + "hands": {"joints": [], "groups": ["left_hand", "right_hand"]}, + # Full body groups + "lower_body": {"joints": [], "groups": ["waist", "legs"]}, + "upper_body_no_hands": {"joints": [], "groups": ["arms"]}, + "body": {"joints": [], "groups": ["lower_body", "upper_body_no_hands"]}, + "upper_body": {"joints": [], "groups": ["upper_body_no_hands", "hands"]}, + } + + # Define joint name mapping from generic types to robot-specific names + joint_name_mapping = { + # Waist joints + "waist_pitch": "waist_pitch_joint", + "waist_roll": "waist_roll_joint", + "waist_yaw": "waist_yaw_joint", + # Shoulder joints + "shoulder_pitch": { + "left": "left_shoulder_pitch_joint", + "right": "right_shoulder_pitch_joint", + }, + "shoulder_roll": { + "left": "left_shoulder_roll_joint", + "right": "right_shoulder_roll_joint", + }, + "shoulder_yaw": { + "left": "left_shoulder_yaw_joint", + "right": "right_shoulder_yaw_joint", + }, + # Elbow joints + "elbow_pitch": {"left": "left_elbow_joint", "right": "right_elbow_joint"}, + # Wrist joints + "wrist_pitch": {"left": "left_wrist_pitch_joint", "right": "right_wrist_pitch_joint"}, + "wrist_roll": {"left": "left_wrist_roll_joint", "right": "right_wrist_roll_joint"}, + "wrist_yaw": {"left": "left_wrist_yaw_joint", "right": "right_wrist_yaw_joint"}, + } + + root_frame_name = "pelvis" + + hand_frame_names = {"left": "left_wrist_yaw_link", "right": "right_wrist_yaw_link"} + + elbow_calibration_joint_angles = {"left": 0.0, "right": 0.0} + + hand_rotation_correction = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) + + # Configure default joint positions based on elbow pose + if elbow_pose == ElbowPose.HIGH: + default_joint_q = { + "shoulder_roll": {"left": 0.5, "right": -0.5}, + "shoulder_pitch": {"left": -0.2, "right": -0.2}, + "shoulder_yaw": {"left": -0.5, "right": 0.5}, + "wrist_roll": {"left": -0.5, "right": 0.5}, + "wrist_yaw": {"left": 0.5, "right": -0.5}, + "wrist_pitch": {"left": -0.2, "right": -0.2}, + } + else: # ElbowPose.LOW + default_joint_q = { + "shoulder_roll": {"left": 0.2, "right": -0.2}, + } + + teleop_upper_body_motion_scale = 1.0 + + # Configure joint groups based on waist location + modified_joint_groups = joint_groups.copy() + if waist_location == WaistLocation.UPPER_BODY: + # Move waist from lower_body to upper_body_no_hands + modified_joint_groups["lower_body"] = {"joints": [], "groups": ["legs"]} + modified_joint_groups["upper_body_no_hands"] = { + "joints": [], + "groups": ["arms", "waist"], + } + elif waist_location == WaistLocation.LOWER_AND_UPPER_BODY: + # Add waist to upper_body_no_hands while keeping it in lower_body + modified_joint_groups["upper_body_no_hands"] = { + "joints": [], + "groups": ["arms", "waist"], + } + # For LOWER_BODY, keep default joint_groups as is + + super().__init__( + name=name, + body_actuated_joints=body_actuated_joints, + left_hand_actuated_joints=left_hand_actuated_joints, + right_hand_actuated_joints=right_hand_actuated_joints, + joint_limits=joint_limits, + joint_groups=modified_joint_groups, + root_frame_name=root_frame_name, + hand_frame_names=hand_frame_names, + elbow_calibration_joint_angles=elbow_calibration_joint_angles, + joint_name_mapping=joint_name_mapping, + hand_rotation_correction=hand_rotation_correction, + default_joint_q=default_joint_q, + teleop_upper_body_motion_scale=teleop_upper_body_motion_scale, + ) diff --git a/gr00t_wbc/control/robot_model/supplemental_info/robot_supplemental_info.py b/gr00t_wbc/control/robot_model/supplemental_info/robot_supplemental_info.py new file mode 100644 index 0000000..d4d66cf --- /dev/null +++ b/gr00t_wbc/control/robot_model/supplemental_info/robot_supplemental_info.py @@ -0,0 +1,91 @@ +from dataclasses import dataclass +from typing import Dict, List, Union + +import numpy as np + + +@dataclass +class RobotSupplementalInfo: + """ + Base class for robot-specific information that is not easily extractable from URDF. + This includes information about actuated joints, joint hierarchies, etc. + """ + + name: str + + # List of body actuated joint names (excluding hands) + body_actuated_joints: List[str] + + # List of left hand actuated joint names + left_hand_actuated_joints: List[str] + + # List of right hand actuated joint names + right_hand_actuated_joints: List[str] + + # Dictionary of joint groups, where each group is a dictionary with: + # - "joints": list of joint names + # - "groups": list of subgroup names (optional) + # Example: { + # "right_arm": { + # "joints": ["right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_elbow_joint"], + # "groups": [] + # }, + # "left_arm": { + # "joints": ["left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_elbow_joint"], + # "groups": [] + # }, + # "upper_body": { + # "joints": ["torso_pitch_joint", "torso_yaw_joint", "torso_roll_joint"], + # "groups": ["right_arm", "left_arm"] + # } + # } + joint_groups: Dict[str, Dict[str, List[str]]] + + # Name of the root frame + root_frame_name: str + + # Dictionary of hand frame names + # Example: { + # "left": "left_hand_frame", + # "right": "right_hand_frame" + # } + hand_frame_names: Dict[str, str] + + # Dictionary of joint limits + # Example: { + # "left_shoulder_pitch_joint": [-np.pi / 2, np.pi / 2], + # "right_shoulder_pitch_joint": [-np.pi / 2, np.pi / 2] + # } + joint_limits: Dict[str, List[float]] + + # Dictionary of elbow calibration joint angles + # Example: { + # "left": -90.0, + # "right": -90.0 + # } + elbow_calibration_joint_angles: Dict[str, float] + + # Dictionary of joint name mapping from generic types to robot-specific names + # Example: { + # "waist_pitch": "waist_pitch_joint", + # "shoulder_pitch": { + # "left": "left_shoulder_pitch_joint", + # "right": "right_shoulder_pitch_joint" + # }, + # "elbow_pitch": { + # "left": "left_elbow_pitch_joint", + # "right": "right_elbow_pitch_joint" + # } + # } + joint_name_mapping: Dict[str, Union[str, Dict[str, str]]] + + # Maps from generic joint names to robot-specific joint values + # Example: { + # "waist_roll": 0.2, + # "elbow_pitch": {"left": 1.0, "right": 1.0} + # } + default_joint_q: Dict[str, Union[float, Dict[str, float]]] + + hand_rotation_correction: np.ndarray + + teleop_upper_body_motion_scale: float diff --git a/gr00t_wbc/control/sensor/__init__.py b/gr00t_wbc/control/sensor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/sensor/composed_camera.py b/gr00t_wbc/control/sensor/composed_camera.py new file mode 100644 index 0000000..042a9f5 --- /dev/null +++ b/gr00t_wbc/control/sensor/composed_camera.py @@ -0,0 +1,440 @@ +from collections import deque +from dataclasses import dataclass +import queue +import threading +import time +from typing import Any, Dict, Optional + +# we need to import these first in this order to avoid TSL segmentation fault +# caused by zed and oak libraries +try: + import cv2 # noqa + import depthai as dai # noqa + import pyzed.sl as sl # noqa +except ImportError: + print( + """ + Some of the camera specific dependencies are not installed. If you are + not running this on the robot, having these libraries is optional. + """ + ) + +import numpy as np # noqa + +from gr00t_wbc.control.base.sensor import Sensor +from gr00t_wbc.control.sensor.sensor_server import ( + ImageMessageSchema, + SensorClient, + SensorServer, + CameraMountPosition, +) + + +def read_qr_code(data): + current_time = time.monotonic() + detector = cv2.QRCodeDetector() + for key, img in data["images"].items(): + decoded_time, bbox, _ = detector.detectAndDecode(img) + if bbox is not None and decoded_time: + print(f"{key} latency: {(current_time - float(decoded_time)) * 1e3:.1f} ms") + else: + print(f"{key} QR code not detected.") + + +@dataclass +class ComposedCameraConfig: + """Camera configuration for composed camera""" + + ego_view_camera: Optional[str] = "oak" + """Camera type for ego view: oak, realsense, zed, or None""" + + ego_view_device_id: Optional[str] = None + """Device ID for ego view camera (optional, used for OAK cameras)""" + + head_camera: Optional[str] = None + """Camera type for head view: oak, oak_mono, realsense, zed or None""" + + head_device_id: Optional[str] = None + """Device ID for head camera (optional, used for OAK cameras)""" + + left_wrist_camera: Optional[str] = None + """Camera type for left wrist view: oak, realsense, zed or None""" + + left_wrist_device_id: Optional[str] = None + """Device ID for left wrist camera (optional, used for OAK cameras)""" + + right_wrist_camera: Optional[str] = None + """Camera type for right wrist view: oak, realsense, zed or None""" + + right_wrist_device_id: Optional[str] = None + """Device ID for right wrist camera (optional, used for OAK cameras)""" + + fps: int = 30 + """Rate at which the composed camera will publish the images. Since composed camera + can read from multiple cameras, it will publish all the images. + Note that OAK can only run at 30 FPS. 20 FPS will cause large latency. + """ + + # Server configuration + run_as_server: bool = True + """Whether to run as server or client""" + + server: bool = True + """Whether to run the camera as a server""" + + port: int = 5555 + """Port number for server/client communication""" + + test_latency: bool = False + """Whether to test latency""" + + # Queue configuration + queue_size: int = 3 + """Size of each camera's image queue""" + + def __post_init__(self): + # runyu: Note that this is a hack to make the config work with G1 camera server in orin + # we should not use this hack in the future + self.run_as_server: bool = self.server + + +class ComposedCameraSensor(Sensor, SensorServer): + + def __init__(self, config: ComposedCameraConfig): + self.config = config + self.camera_queues: Dict[str, queue.Queue] = {} + self.camera_threads: Dict[str, threading.Thread] = {} + self.shutdown_events: Dict[str, threading.Event] = {} + self.error_events: Dict[str, threading.Event] = {} + self.error_messages: Dict[str, str] = {} + self._observation_spaces: Dict[str, Any] = {} + + camera_configs = self._get_camera_configs() + + # Then create worker threads + for mount_position, camera_config in camera_configs.items(): + # Create queue and shutdown event for this camera + camera_queue = queue.Queue(maxsize=config.queue_size) + shutdown_event = threading.Event() + error_event = threading.Event() + + self.camera_queues[mount_position] = camera_queue + self.shutdown_events[mount_position] = shutdown_event + self.error_events[mount_position] = error_event + + # Start camera thread + thread = threading.Thread( + target=self._camera_worker_wrapper, + args=( + mount_position, + camera_config["camera_type"], + camera_config["device_id"], + camera_queue, + shutdown_event, + error_event, + ), + ) + thread.start() + self.camera_threads[mount_position] = thread + + if config.run_as_server: + self.start_server(config.port) + + def _get_camera_configs(self) -> Dict[str, str]: + """Get camera configurations as mount_position -> camera_type mapping""" + camera_configs = {} + + if self.config.ego_view_camera is not None: + camera_configs[CameraMountPosition.EGO_VIEW.value] = { + "camera_type": self.config.ego_view_camera, + "device_id": self.config.ego_view_device_id, + } + + if self.config.head_camera is not None: + camera_configs[CameraMountPosition.HEAD.value] = { + "camera_type": self.config.head_camera, + "device_id": self.config.head_device_id, + } + + if self.config.left_wrist_camera is not None: + camera_configs[CameraMountPosition.LEFT_WRIST.value] = { + "camera_type": self.config.left_wrist_camera, + "device_id": self.config.left_wrist_device_id, + } + + if self.config.right_wrist_camera is not None: + camera_configs[CameraMountPosition.RIGHT_WRIST.value] = { + "camera_type": self.config.right_wrist_camera, + "device_id": self.config.right_wrist_device_id, + } + + return camera_configs + + def _camera_worker_wrapper( + self, + mount_position: str, + camera_type: str, + device_id: Optional[str], + image_queue: queue.Queue, + shutdown_event: threading.Event, + error_event: threading.Event, + ): + """Worker thread that continuously captures from a single camera""" + try: + camera = self._instantiate_camera(mount_position, camera_type, device_id) + self._observation_spaces[mount_position] = camera.observation_space() + + consecutive_failures = 0 + max_consecutive_failures = 5 + + while not shutdown_event.is_set(): + frame = camera.read() + if frame: + consecutive_failures = 0 # Reset on successful read + # Non-blocking queue put with frame dropping + try: + image_queue.put_nowait(frame) + except queue.Full: + # Remove oldest frame and add new one + try: + image_queue.get_nowait() + image_queue.put_nowait(frame) + except queue.Empty: + pass + else: + consecutive_failures += 1 + if consecutive_failures >= max_consecutive_failures: + error_msg = ( + f"Camera {mount_position} ({camera_type}) dropped: " + f"failed to read {consecutive_failures} consecutive frames" + ) + print(f"[ERROR] {error_msg}") + self.error_messages[mount_position] = error_msg + error_event.set() + break + + camera.close() + + except Exception as e: + error_msg = f"Camera {mount_position} ({camera_type}) error: {str(e)}" + print(f"[ERROR] {error_msg}") + self.error_messages[mount_position] = error_msg + error_event.set() + + def _instantiate_camera( + self, mount_position: str, camera_type: str, device_id: Optional[str] = None + ) -> Sensor: + """ + Instantiate a camera sensor based on the camera type. + + Args: + camera_type: Type of camera ("oak", "oak_mono", "realsense", "zed") + device_id: Optional device ID for the camera (used for OAK cameras) + + Returns: + Sensor instance for the specified camera type + """ + if camera_type in ("oak", "oak_mono"): + from gr00t_wbc.control.sensor.oak import OAKConfig, OAKSensor + + oak_config = OAKConfig() + if camera_type == "oak_mono": + oak_config.enable_mono_cameras = True + print("Initializing OAK sensor for camera type: ", camera_type) + return OAKSensor(config=oak_config, mount_position=mount_position, device_id=device_id) + elif camera_type == "realsense": + from gr00t_wbc.control.sensor.realsense import RealSenseSensor + + print("Initializing RealSense sensor for camera type: ", camera_type) + return RealSenseSensor(mount_position=mount_position) + elif camera_type == "zed": + from gr00t_wbc.control.sensor.zed import ZEDSensor + + print("Initializing ZED sensor for camera type: ", camera_type) + return ZEDSensor(mount_position=mount_position) + elif camera_type.endswith(".mp4"): + from gr00t_wbc.control.sensor.dummy import ReplayDummySensor + + print("Initializing Replay Dummy Sensor for camera type: ", camera_type) + return ReplayDummySensor(video_path=camera_type) + else: + raise ValueError(f"Unsupported camera type: {camera_type}") + + def _check_for_errors(self): + """Check if any camera thread has encountered an error and raise exception if so.""" + for mount_position, error_event in self.error_events.items(): + if error_event.is_set(): + error_msg = self.error_messages.get( + mount_position, f"Camera {mount_position} encountered an unknown error" + ) + raise RuntimeError(error_msg) + + def read(self): + """Read frames from all cameras.""" + # Check for errors from camera threads + self._check_for_errors() + + message = {} + for mount_position, camera_queue in self.camera_queues.items(): + frame = self._get_latest_from_queue(camera_queue) + if frame is not None: + message[mount_position] = frame + return message + + def _get_latest_from_queue(self, camera_queue: queue.Queue) -> Optional[Dict[str, Any]]: + """Get most recent frame, discard older ones""" + latest = None + try: + while True: + latest = camera_queue.get_nowait() + except queue.Empty: + pass + return latest + + def close(self): + """Close all cameras.""" + # Signal all worker threads to shutdown + for shutdown_event in self.shutdown_events.values(): + shutdown_event.set() + + # Wait for all threads to finish + for thread in self.camera_threads.values(): + thread.join(timeout=5.0) + + # Clear queues + for camera_queue in self.camera_queues.values(): + try: + while True: + camera_queue.get_nowait() + except queue.Empty: + pass + + # Stop server if running + if self.config.run_as_server: + self.stop_server() + + def serialize_message(self, message: Dict[str, Any]) -> Dict[str, Any]: + """Merge all camera data into a single ImageMessageSchema.""" + all_timestamps = {} + all_images = {} + + for _, camera_data in message.items(): + all_timestamps.update(camera_data.get("timestamps", {})) + all_images.update(camera_data.get("images", {})) + + # Create a single ImageMessageSchema with all data + img_schema = ImageMessageSchema(timestamps=all_timestamps, images=all_images) + return img_schema.serialize() + + def run_server(self): + """Run the server.""" + idx = 0 + server_start_time = time.monotonic() + fps_print_time = time.monotonic() + frame_interval = 1.0 / self.config.fps + + while True: + # Calculate when this frame should ideally complete + target_time = server_start_time + (idx + 1) * frame_interval + + message = self.read() + if message: + if self.config.test_latency: + read_qr_code(message) + + serialized_message = self.serialize_message(message) + self.send_message(serialized_message) + idx += 1 + + if idx % 10 == 0: + print(f"Image sending FPS: {10 / (time.monotonic() - fps_print_time):.2f}") + fps_print_time = time.monotonic() + + # Sleep to maintain precise timing + current_time = time.monotonic() + sleep_time = target_time - current_time + if sleep_time > 0: + time.sleep(sleep_time) + else: + # If we're behind, increment idx to stay on schedule + if not message: + idx += 1 + + def observation_space(self): + """Return the observation space.""" + import gymnasium as gym + + return gym.spaces.Dict(self._observation_spaces) + + +class ComposedCameraClientSensor(Sensor, SensorClient): + """Class that serves as client for multiple different cameras.""" + + def __init__(self, server_ip: str = "localhost", port: int = 5555): + self.start_client(server_ip, port) + + # Initialize tracking variables + self._latest_message = {} + self._avg_time_per_frame = deque(maxlen=20) + self._msg_received_time = 0 + self._start_time = 0.0 # Initialize _start_time + self.idx = 0 + + print("Initialized composed camera client sensor") + + def read(self, **kwargs) -> Optional[Dict[str, Any]]: + self._start_time = time.time() + message = self.receive_message() + if not message: + return None + self.idx += 1 + + self._latest_message = ImageMessageSchema.deserialize(message).asdict() + + # if self.idx % 10 == 0: + # for image_key, image_time in self._latest_message["timestamps"].items(): + # image_latency = (time.time() - image_time) * 1000 + # print(f"Image latency for {image_key}: {image_latency:.2f} ms") + + self._msg_received_time = time.time() + self._avg_time_per_frame.append(self._msg_received_time - self._start_time) + + return self._latest_message + + def close(self): + """Close the client connection.""" + self.stop_client() + + def fps(self) -> float: + """Get the current FPS of the client.""" + if len(self._avg_time_per_frame) == 0: + return 0.0 + return float(1 / np.mean(self._avg_time_per_frame)) + + +if __name__ == "__main__": + """Test function for ComposedCamera.""" + import tyro + + config = tyro.cli(ComposedCameraConfig) + + if config.run_as_server: + composed_camera = ComposedCameraSensor(config) + print("Running composed camera server...") + composed_camera.run_server() + + else: + # Client mode + composed_client = ComposedCameraClientSensor(server_ip="localhost", port=config.port) + + try: + while True: + data = composed_client.read() + if data is not None: + print(f"FPS: {composed_client.fps():.2f}") + if "timestamp" in data: + print(f"Timestamp: {data['timestamp']}") + time.sleep(0.1) + except KeyboardInterrupt: + print("Stopping client...") + composed_client.close() diff --git a/gr00t_wbc/control/sensor/oak.py b/gr00t_wbc/control/sensor/oak.py new file mode 100644 index 0000000..f1913df --- /dev/null +++ b/gr00t_wbc/control/sensor/oak.py @@ -0,0 +1,324 @@ +import time +from typing import Any, Dict, Optional, Tuple + +import cv2 +import depthai as dai +import gymnasium as gym +import numpy as np + +from gr00t_wbc.control.base.sensor import Sensor +from gr00t_wbc.control.sensor.sensor_server import ( + CameraMountPosition, + ImageMessageSchema, + SensorServer, +) + + +class OAKConfig: + """Configuration for the OAK camera.""" + + color_image_dim: Tuple[int, int] = (640, 480) # RGB camera resolution + monochrome_image_dim: Tuple[int, int] = (640, 480) # Monochrome camera resolution + fps: int = 30 + enable_color: bool = True # Enable CAM_A (RGB) + enable_mono_cameras: bool = False # Enable CAM_B & CAM_C (Monochrome stereo pair) + mount_position: str = CameraMountPosition.EGO_VIEW.value + + +class OAKSensor(Sensor, SensorServer): + """Sensor for the OAK camera family.""" + + def __init__( + self, + run_as_server: bool = False, + port: int = 5555, + config: OAKConfig = OAKConfig(), + device_id: Optional[str] = None, + mount_position: str = CameraMountPosition.EGO_VIEW.value, + ): + """Initialize the OAK camera.""" + self.config = config + self.mount_position = mount_position + self._run_as_server = run_as_server + + device_infos = dai.Device.getAllAvailableDevices() + assert len(device_infos) > 0, f"No OAK devices found for {mount_position}" + print(f"Device infos: {device_infos}") + if device_id is not None: + device_found = False + for device_info in device_infos: + if device_info.getDeviceId() == device_id: + self.device = dai.Device(device_info) + device_found = True + break + if not device_found: + raise ValueError(f"Device with ID {device_id} not found") + else: + self.device = dai.Device() + + print(f"Connected to OAK device: {self.device.getDeviceName(), self.device.getDeviceId()}") + print(f"Device ID: {self.device.getDeviceId()}") + + sockets: list[dai.CameraBoardSocket] = self.device.getConnectedCameras() + print(f"Available cameras: {[str(s) for s in sockets]}") + + # Create pipeline (without context manager to persist across method calls) + self.pipeline = dai.Pipeline(self.device) + self.output_queues = {} + + # Configure RGB camera (CAM_A) + if config.enable_color and dai.CameraBoardSocket.CAM_A in sockets: + self.cam_rgb = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_A + self.cam_rgb = self.cam_rgb.build(cam_socket) + # Create RGB output queue + self.output_queues["color"] = self.cam_rgb.requestOutput( + config.color_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_A (RGB)") + + # Configure Monochrome cameras (CAM_B & CAM_C) + if config.enable_mono_cameras: + if dai.CameraBoardSocket.CAM_B in sockets: + self.cam_mono_left = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_B + self.cam_mono_left = self.cam_mono_left.build(cam_socket) + # Create mono left output queue + self.output_queues["mono_left"] = self.cam_mono_left.requestOutput( + config.monochrome_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_B (Monochrome Left)") + + if dai.CameraBoardSocket.CAM_C in sockets: + self.cam_mono_right = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_C + self.cam_mono_right = self.cam_mono_right.build(cam_socket) + # Create mono right output queue + self.output_queues["mono_right"] = self.cam_mono_right.requestOutput( + config.monochrome_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_C (Monochrome Right)") + + assert len(self.output_queues) > 0, "No output queues enabled" + # auto exposure compensation, for CoRL demo + # cam_q_in = self.cam_rgb.inputControl.createInputQueue() + # ctrl = dai.CameraControl() + # ctrl.setAutoExposureEnable() + # ctrl.setAutoExposureCompensation(-2) + # cam_q_in.send(ctrl) + + # Start pipeline on device + self.pipeline.start() + + if run_as_server: + self.start_server(port) + + def read(self) -> Optional[Dict[str, Any]]: + """Read images from the camera.""" + if not self.pipeline.isRunning(): + print(f"[ERROR] OAK pipeline stopped for {self.mount_position}") + return None + + # Check if device is still connected + if not self.device.isPipelineRunning(): + print(f"[ERROR] OAK device disconnected for {self.mount_position}") + return None + + timestamps = {} + images = {} + rgb_frame_time = None + + # Get color frame if enabled + if "color" in self.output_queues: + try: + rgb_frame = self.output_queues["color"].get() + rgb_frame_time = rgb_frame.getTimestamp() + if rgb_frame is not None: + images[self.mount_position] = rgb_frame.getCvFrame()[..., ::-1] # BGR to RGB + timestamps[self.mount_position] = ( + rgb_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read color frame from {self.mount_position}: {e}") + return None + + # Get mono frames if enabled + if "mono_left" in self.output_queues: + try: + mono_left_frame = self.output_queues["mono_left"].get() + mono_left_frame_time = mono_left_frame.getTimestamp() + if mono_left_frame is not None: + key = f"{self.mount_position}_left_mono" + images[key] = mono_left_frame.getCvFrame() + timestamps[key] = ( + mono_left_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read mono_left frame from {self.mount_position}: {e}") + return None + + if "mono_right" in self.output_queues: + try: + mono_right_frame = self.output_queues["mono_right"].get() + mono_right_frame_time = mono_right_frame.getTimestamp() + if mono_right_frame is not None: + key = f"{self.mount_position}_right_mono" + images[key] = mono_right_frame.getCvFrame() + timestamps[key] = ( + mono_right_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read mono_right frame from {self.mount_position}: {e}") + return None + + if ( + rgb_frame_time is not None + and (rgb_frame_time - dai.Clock.now()).total_seconds() <= -0.2 + ): + print( + f"[{self.mount_position}] OAK latency too large: " + f"{(dai.Clock.now() - rgb_frame_time).total_seconds() * 1000}ms" + ) + + return { + "timestamps": timestamps, + "images": images, + } + + def serialize(self, data: Dict[str, Any]) -> Dict[str, Any]: + """Serialize data using ImageMessageSchema.""" + serialized_msg = ImageMessageSchema(timestamps=data["timestamps"], images=data["images"]) + return serialized_msg.serialize() + + def observation_space(self) -> gym.Space: + spaces = {} + + if self.config.enable_color: + spaces["color_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.color_image_dim[1], self.config.color_image_dim[0], 3), + dtype=np.uint8, + ) + + if self.config.enable_mono_cameras: + spaces["mono_left_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]), + dtype=np.uint8, + ) + spaces["mono_right_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]), + dtype=np.uint8, + ) + + return gym.spaces.Dict(spaces) + + def close(self): + """Close the camera connection.""" + if self._run_as_server: + self.stop_server() + if hasattr(self, "pipeline") and self.pipeline.isRunning(): + self.pipeline.stop() + self.device.close() + + def run_server(self): + """Run the server.""" + if not self._run_as_server: + raise ValueError("This function is only available when run_as_server is True") + + while True: + frame = self.read() + if frame is None: + continue + + msg = self.serialize(frame) + self.send_message({self.mount_position: msg}) + + def __del__(self): + self.close() + + +if __name__ == "__main__": + """Test function for OAK camera.""" + + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--server", action="store_true", help="Run as server") + parser.add_argument("--client", action="store_true", help="Run as client") + parser.add_argument("--host", type=str, default="localhost", help="Server IP address") + parser.add_argument("--port", type=int, default=5555, help="Port number") + parser.add_argument("--device-id", type=str, default=None, help="Specific device ID") + parser.add_argument( + "--enable-mono", action="store_true", help="Enable monochrome cameras (CAM_B & CAM_C)" + ) + parser.add_argument("--mount-position", type=str, default="ego_view", help="Mount position") + parser.add_argument("--show-image", action="store_true", help="Display images") + args = parser.parse_args() + + oak_config = OAKConfig() + if args.enable_mono: + oak_config.enable_mono_cameras = True + + if args.server: + # Run as server + oak = OAKSensor( + run_as_server=True, + port=args.port, + config=oak_config, + device_id=args.device_id, + mount_position=args.mount_position, + ) + print(f"Starting OAK server on port {args.port}") + oak.run_server() + + else: + # Run standalone + oak = OAKSensor(run_as_server=False, config=oak_config, device_id=args.device_id) + print("Running OAK camera in standalone mode") + + while True: + frame = oak.read() + if frame is None: + print("Waiting for frame...") + time.sleep(0.5) + continue + + if "color_image" in frame: + print(f"Color image shape: {frame['color_image'].shape}") + if "mono_left_image" in frame: + print(f"Mono left image shape: {frame['mono_left_image'].shape}") + if "mono_right_image" in frame: + print(f"Mono right image shape: {frame['mono_right_image'].shape}") + if "depth_image" in frame: + print(f"Depth image shape: {frame['depth_image'].shape}") + + if args.show_image: + if "color_image" in frame: + cv2.imshow("Color Image", frame["color_image"]) + + if "mono_left_image" in frame: + cv2.imshow("Mono Left", frame["mono_left_image"]) + if "mono_right_image" in frame: + cv2.imshow("Mono Right", frame["mono_right_image"]) + + if "depth_image" in frame: + depth_colormap = cv2.applyColorMap( + cv2.convertScaleAbs(frame["depth_image"], alpha=0.03), cv2.COLORMAP_JET + ) + cv2.imshow("Depth Image", depth_colormap) + + if cv2.waitKey(1) == ord("q"): + break + + time.sleep(0.01) + + cv2.destroyAllWindows() + oak.close() diff --git a/gr00t_wbc/control/sensor/sensor_server.py b/gr00t_wbc/control/sensor/sensor_server.py new file mode 100644 index 0000000..2845337 --- /dev/null +++ b/gr00t_wbc/control/sensor/sensor_server.py @@ -0,0 +1,128 @@ +import base64 +from dataclasses import dataclass +from enum import Enum +from typing import Any, Dict + +import cv2 +import msgpack +import msgpack_numpy as m +import numpy as np +import zmq + + +@dataclass +class ImageMessageSchema: + """ + This is a standardized message schema for image data. + Any camera should use this schema to serialize (send to queue) and + deserialize (receive from queue) the image data. + + """ + + timestamps: Dict[str, float] + """Dictionary of timestamps, keyed by image identifier (e.g., {"ego_view": 123.45})""" + images: Dict[str, np.ndarray] + """Dictionary of images, keyed by image identifier (e.g., {"ego_view": array, "ego_view_left_mono": array})""" + + def serialize(self) -> Dict[str, Any]: + """Serialize the message for transmission.""" + serialized_msg = {"timestamps": self.timestamps, "images": {}} + for key, image in self.images.items(): + serialized_msg["images"][key] = ImageUtils.encode_image(image) + return serialized_msg + + @staticmethod + def deserialize(data: Dict[str, Any]) -> "ImageMessageSchema": + """Deserialize received message data.""" + timestamps = data.get("timestamps", {}) + images = {} + for key, value in data.get("images", {}).items(): + if isinstance(value, str): + images[key] = ImageUtils.decode_image(value) + else: + images[key] = value + return ImageMessageSchema(timestamps=timestamps, images=images) + + def asdict(self) -> Dict[str, Any]: + """Convert to dictionary format.""" + return {"timestamps": self.timestamps, "images": self.images} + + +class SensorServer: + def start_server(self, port: int): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.PUB) + self.socket.setsockopt(zmq.SNDHWM, 20) # high water mark + self.socket.setsockopt(zmq.LINGER, 0) + self.socket.bind(f"tcp://*:{port}") + print(f"Sensor server running at tcp://*:{port}") + + self.message_sent = 0 + self.message_dropped = 0 + + def stop_server(self): + self.socket.close() + self.context.term() + + def send_message(self, data: Dict[str, Any]): + try: + packed = msgpack.packb(data, use_bin_type=True) + self.socket.send(packed, flags=zmq.NOBLOCK) + except zmq.Again: + self.message_dropped += 1 + print(f"[Warning] message dropped: {self.message_dropped}") + self.message_sent += 1 + + if self.message_sent % 100 == 0: + print( + f"[Sensor server] Message sent: {self.message_sent}, message dropped: {self.message_dropped}" + ) + + +class SensorClient: + def start_client(self, server_ip: str, port: int): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.SUB) + self.socket.setsockopt_string(zmq.SUBSCRIBE, "") + self.socket.setsockopt(zmq.CONFLATE, True) # last msg only. + self.socket.setsockopt(zmq.RCVHWM, 3) # queue size 3 for receive buffer + self.socket.connect(f"tcp://{server_ip}:{port}") + + def stop_client(self): + self.socket.close() + self.context.term() + + def receive_message(self): + packed = self.socket.recv() + return msgpack.unpackb(packed, object_hook=m.decode) + + +class CameraMountPosition(Enum): + EGO_VIEW = "ego_view" + HEAD = "head" + LEFT_WRIST = "left_wrist" + RIGHT_WRIST = "right_wrist" + + +class ImageUtils: + @staticmethod + def encode_image(image: np.ndarray) -> str: + _, color_buffer = cv2.imencode(".jpg", image, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) + return base64.b64encode(color_buffer).decode("utf-8") + + @staticmethod + def encode_depth_image(image: np.ndarray) -> str: + depth_compressed = cv2.imencode(".png", image)[1].tobytes() + return base64.b64encode(depth_compressed).decode("utf-8") + + @staticmethod + def decode_image(image: str) -> np.ndarray: + color_data = base64.b64decode(image) + color_array = np.frombuffer(color_data, dtype=np.uint8) + return cv2.imdecode(color_array, cv2.IMREAD_COLOR) + + @staticmethod + def decode_depth_image(image: str) -> np.ndarray: + depth_data = base64.b64decode(image) + depth_array = np.frombuffer(depth_data, dtype=np.uint8) + return cv2.imdecode(depth_array, cv2.IMREAD_UNCHANGED) diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientLogging.hpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientLogging.hpp new file mode 100644 index 0000000..a7469d7 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientLogging.hpp @@ -0,0 +1,158 @@ +#ifndef __CLIENT_LOGGING_HPP__ +#define __CLIENT_LOGGING_HPP__ +#include +#include +#include +#include + +namespace ManusSDK +{ + class ClientLog + { + public: + //Simple + static void debug(const char* p_String) + { + std::cout << "DEBUG: " << p_String << std::endl; + } + + static void info(const char* p_String) + { + std::cout << "INFO: " << p_String << std::endl; + } + + static void warn(const char* p_String) + { + std::cout << "WARN: " << p_String << std::endl; + } + + static void error(const char* p_String) + { + std::cout << "ERROR: " << p_String << std::endl; + } + + static void print(const char* p_String) + { + std::cout << p_String << std::endl; + } + + //Templated + template + static void debug(const char* p_Format, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholders(formatted_string, std::forward(p_Args)...); + std::cout << "DEBUG: " << formatted_string << std::endl; + } + + template + static void info(const char* p_Format, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholders(formatted_string, std::forward(p_Args)...); + std::cout << "INFO: " << formatted_string << std::endl; + } + + template + static void warn(const char* p_Format, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholders(formatted_string, std::forward(p_Args)...); + std::cout << "WARN: " << formatted_string << std::endl; + } + + template + static void error(const char* p_Format, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholders(formatted_string, std::forward(p_Args)...); + std::cout << "ERROR: " << formatted_string << std::endl; + } + + template + static void print(const char* p_Format, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholders(formatted_string, std::forward(p_Args)...); + std::cout << formatted_string << std::endl; + } + + template + static void printWithPadding(const char* p_Format, int p_Padding, Args &&...p_Args) + { + std::string formatted_string = p_Format; + replacePlaceholdersWithPadding(formatted_string, p_Padding, std::forward(p_Args)...); + std::cout << formatted_string << std::endl; + } + + private: + // Base case for recursion: no arguments left + static void replacePlaceholders(std::string&) { + // No more arguments, so nothing to replace + } + + // Replace placeholders {} with actual values (only supports empty curly brackets, no formatting) + template + static void replacePlaceholders(std::string& p_Format, T&& p_Value, Args&&... p_Args) { + size_t pos = p_Format.find("{}"); + if (pos != std::string::npos) { + p_Format.replace(pos, 2, to_string(std::forward(p_Value))); + replacePlaceholders(p_Format, std::forward(p_Args)...); + } + } + + // Base case for recursion: no arguments left + static void replacePlaceholdersWithPadding(std::string&, int p_Padding) { + // No more arguments, so nothing to replace + } + // Replace placeholders {} with actual values (only supports empty curly brackets, no formatting) + template + static void replacePlaceholdersWithPadding(std::string& p_Format, int p_Padding, T&& p_Value, Args&&... p_Args) { + size_t pos = p_Format.find("{}"); + if (pos != std::string::npos) { + std::ostringstream oss; + oss << std::setw(p_Padding) << to_string(std::forward(p_Value)); + p_Format.replace(pos, 2, oss.str()); + replacePlaceholdersWithPadding(p_Format, p_Padding, std::forward(p_Args)...); + } + } + + // Utility to convert various types to string + template + static std::string to_string(const T& p_Value) { + std::ostringstream oss; + oss << p_Value; + return oss.str(); + } + + // Specialization for uint8_t + static std::string to_string(const uint8_t& p_Value) { + std::ostringstream oss; + oss << static_cast(p_Value); + return oss.str(); + } + + // Specialization for uint16_t + static std::string to_string(const uint16_t& p_Value) { + std::ostringstream oss; + oss << static_cast(p_Value); + return oss.str(); + } + + // Specialization for uint32_t + static std::string to_string(const uint32_t& p_Value) { + std::ostringstream oss; + oss << std::hex << p_Value; + return oss.str(); + } + + // Specialization for bool + static std::string to_string(const bool& p_Value) { + std::ostringstream oss; + oss << std::setw(5) << (p_Value ? "true" : "false"); + return oss.str(); + } + }; +} + +#endif diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.cpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.cpp new file mode 100644 index 0000000..3c70281 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.cpp @@ -0,0 +1,559 @@ +#include "ClientPlatformSpecific.hpp" + +// signal types +#include +// std::filesystem +#include +// std::*fstream +#include +// CoreSdk_ShutDown +#include "ManusSDK.h" +// std::map +#include +// Ncurses terminal functions. +#include +// Termios terminal functions. +#include +#include +#include +// spdlog replacement +#include "ClientLogging.hpp" + +using namespace ManusSDK; + +/// @brief Reset a signal handler to its default, and then call it. +/// For signal types and explanation, see: +/// https://www.gnu.org/software/libc/manual/html_node/Standard-Signals.html +#define CALL_DEFAULT_SIGNAL_HANDLER(p_SignalType) \ + /* Reset the handler for this signal to the default. */ \ + signal(p_SignalType, SIG_DFL); \ + /* Re-raise this signal, causing the normal handler to run. */ \ + raise(p_SignalType); + +const std::string SDKClientPlatformSpecific::s_SlashForFilesystemPath = "/"; + +/// @brief Handle a signal telling the SDK client to quit. +/// A generic signal used to "politely ask a program to terminate". +/// On Linux, this can be sent by using the Gnome System Monitor and telling +/// the SDK client process to end. +static void HandleTerminationSignal(int p_Parameter) +{ + ClientLog::error( + "Termination signal sent with parameter {}.", + p_Parameter); + + CoreSdk_ShutDown(); + + CALL_DEFAULT_SIGNAL_HANDLER(SIGTERM); +} + +/// @brief Handle an interrupt signal. +/// Called when the INTR character is typed - usually ctrl + c. +static void HandleInterruptSignal(int p_Parameter) +{ + ClientLog::error( + "Interrupt signal sent with parameter {}.", + p_Parameter); + + CoreSdk_ShutDown(); + + CALL_DEFAULT_SIGNAL_HANDLER(SIGINT); +} + +/// @brief Handle a quit signal. +/// Called when the QUIT character is typed - usually ctrl + \. +static void HandleQuitSignal(int p_Parameter) +{ + ClientLog::error( + "Quit signal sent with parameter {}.", + p_Parameter); + + CoreSdk_ShutDown(); + + CALL_DEFAULT_SIGNAL_HANDLER(SIGQUIT); +} + +/// @brief Handle a hangup signal. +/// Called to report that the user's terminal has disconnected. +/// This can happen when connecting over the network, for example. +/// It also happens if the terminal window is closed while debugging. +static void HandleHangupSignal(int p_Parameter) +{ + ClientLog::error( + "Hang-up signal sent with parameter {}.", + p_Parameter); + + CoreSdk_ShutDown(); + + CALL_DEFAULT_SIGNAL_HANDLER(SIGHUP); +} + +/// @brief Initialise Ncurses so that we can check for input. +static bool InitializeNcurses(void) +{ + const WINDOW* const t_Window = initscr(); + if (!t_Window) + { + ClientLog::error("Failed to initialise the screen."); + + return false; + } + + // Don't buffer input until a newline or carriage return is typed. + if (cbreak() != OK) + { + ClientLog::error("Failed to make input unbuffered."); + + return false; + } + + // Don't echo input. + if (noecho() != OK) + { + ClientLog::error("Failed to disable input echoing."); + + return false; + } + + // Don't make newlines when the return key is pressed. + if (nonl() != OK) + { + ClientLog::error("Failed to disable newlines."); + + return false; + } + + // Do not flush the screen when interrupt/break/quit is pressed. + if (intrflush(stdscr, FALSE) != OK) + { + ClientLog::error("Failed to disable screen flushing."); + + return false; + } + + // Make getch non-blocking. + if (nodelay(stdscr, TRUE) != OK) + { + ClientLog::error("Failed to make nodelay non-blocking."); + + return false; + } + + // Enable handling the keypad ("function keys" like the arrow keys). + if (keypad(stdscr, TRUE) != OK) + { + ClientLog::error("Failed to enable keypad input."); + + return false; + } + + return true; +} + +/// @brief Initialise Termios so that terminal output is correct. +static bool InitializeTermios(void) +{ + // For some reason, printf and spdlog strings require a carriage return + // in addition to a newline after running initscr(). + // This code sets the "output modes" flag to treat newline characters + // as newline+carriage-return characters. + // https://arstechnica.com/civis/viewtopic.php?t=70699 + termios t_Settings; + if (tcgetattr(STDIN_FILENO, &t_Settings) != 0) + { + ClientLog::error("Failed to get Termios settings."); + + return false; + } + + t_Settings.c_oflag |= ONLCR; + if (tcsetattr(0, TCSANOW, &t_Settings) != 0) + { + ClientLog::error("Failed to set Termios settings."); + + return false; + } + + return true; +} + +/// @brief Register our signal handling functions. +static bool SetUpSignalHandlers(void) +{ + { + const __sighandler_t t_OldTerminationHandler = signal( + SIGTERM, + HandleTerminationSignal); + if (t_OldTerminationHandler == SIG_ERR) + { + ClientLog::error("Failed to set termination signal handler."); + return false; + } + } + + { + const __sighandler_t t_OldInterruptHandler = signal( + SIGINT, + HandleInterruptSignal); + if (t_OldInterruptHandler == SIG_ERR) + { + ClientLog::error("Failed to set interrupt signal handler."); + return false; + } + } + + { + const __sighandler_t t_OldQuitHandler = signal( + SIGQUIT, + HandleQuitSignal); + if (t_OldQuitHandler == SIG_ERR) + { + ClientLog::error("Failed to set quit signal handler."); + return false; + } + } + + { + const __sighandler_t t_OldHangupHandler = signal( + SIGHUP, + HandleHangupSignal); + if (t_OldHangupHandler == SIG_ERR) + { + ClientLog::error("Failed to set hang-up signal handler."); + return false; + } + } + + return true; +} + +/// @brief Handles keyboard input. +/// Requires things to be set up with Ncurses and Termios. +class ClientInput +{ +public: + /// @brief Update the state of the keyboard. + void Update(void) + { + // Reset the state of the last update. + for ( + InputMap_t::iterator t_Key = m_PressedLastUpdate.begin(); + t_Key != m_PressedLastUpdate.end(); + t_Key++) + { + t_Key->second = false; + } + + // Copy the current state to the last update's state, and clear the + // current state. + for ( + InputMap_t::iterator t_Key = m_CurrentlyPressed.begin(); + t_Key != m_CurrentlyPressed.end(); + t_Key++) + { + m_PressedLastUpdate[t_Key->first] = + t_Key->second; + t_Key->second = false; + } + + // Get the new state. + int t_Ch = getch(); + while (t_Ch != ERR) + { + if (t_Ch >= 'a' && t_Ch <= 'z') + { + // Unlike with Windows' GetAsyncKeyState(), upper case and + // lower case characters have different key numbers with + // getch(). + // Since all WasKeyPressed calls (as of writing this) use + // upper case, lower case keys need to be converted to work + // on Linux. + // Note that this does break the ability to check for lower + // case key presses. + t_Ch = toupper(t_Ch); + } + + m_CurrentlyPressed[t_Ch] = true; + + t_Ch = getch(); + } + } + + /// @brief Get the key's current state. + /// Note that unlike IsPressed(), this also stores the result for use in + /// the next key state check. + bool GetKey(const int p_Key) + { + bool t_IsPressed = IsPressed(p_Key); + m_PreviousKeyState[p_Key] = t_IsPressed; + + return t_IsPressed; + } + + /// @brief Was this key pressed since the last check? + /// Note that unlike WasJustPressed(), this checks if the key was pressed + /// since the last time a GetKey* function was called. + bool GetKeyDown(const int p_Key) + { + const bool t_IsPressed = IsPressed(p_Key); + + const auto t_PreviousState = m_PreviousKeyState.find(p_Key); + const bool t_PreviousValue = + t_PreviousState == m_PreviousKeyState.end() + ? false + : t_PreviousState->second; + + const bool t_Down = t_IsPressed && !t_PreviousValue; + m_PreviousKeyState[p_Key] = t_Down; + + return t_Down; + } + + /// @brief Was this key released since the last check? + /// Note that unlike WasJustReleased(), this checks if the key was released + /// since the last time a GetKey* function was called. + bool GetKeyUp(const int p_Key) + { + const bool t_IsPressed = IsPressed(p_Key); + + const auto t_PreviousState = m_PreviousKeyState.find(p_Key); + const bool t_PreviousValue = + t_PreviousState == m_PreviousKeyState.end() + ? false + : t_PreviousState->second; + + const bool t_Up = !t_IsPressed && t_PreviousValue; + m_PreviousKeyState[p_Key] = t_Up; + + return t_Up; + } + +private: + /// @brief Get the key's current state. + /// Note that unlike GetKey(), this does not store the result for use in + /// the next key state check. + bool IsPressed(const int p_Key) const + { + auto t_CurrentlyPressed = m_CurrentlyPressed.find(p_Key); + if (t_CurrentlyPressed == m_CurrentlyPressed.end()) + { + return false; + } + + return t_CurrentlyPressed->second; + } + + /// @brief Was this key pressed since the last input update? + /// Note that unlike GetKeyDown(), this function will return the same value + /// until the next keyboard state update. + bool WasJustPressed(const int p_Key) const + { + return !WasPressedLastUpdate(p_Key) && IsPressed(p_Key); + } + + /// @brief Was this key released since the last input update? + /// Note that unlike GetKeyUp(), this function will return the same value + /// until the next keyboard state update. + bool WasJustReleased(const int p_Key) const + { + return WasPressedLastUpdate(p_Key) && !IsPressed(p_Key); + } + + /// @brief Was this key pressed the previous input update? + bool WasPressedLastUpdate(const int p_Key) const + { + auto t_StateLastUpdate = m_PressedLastUpdate.find(p_Key); + if (t_StateLastUpdate == m_PressedLastUpdate.end()) + { + return false; + } + + return t_StateLastUpdate->second; + } + + typedef std::map InputMap_t; + InputMap_t m_CurrentlyPressed; + InputMap_t m_PressedLastUpdate; + // The Windows GetKey* functions only update the key state when a GetKey* + // function gets called. To make the Linux input work the same way, this + // map is used. + InputMap_t m_PreviousKeyState; +}; + +static ClientInput g_Input; + +bool SDKClientPlatformSpecific::PlatformSpecificInitialization(void) +{ + const bool t_NcursesResult = InitializeNcurses(); + const bool t_TermiosResult = InitializeTermios(); + const bool t_SignalResult = SetUpSignalHandlers(); + + return t_NcursesResult && t_TermiosResult && t_SignalResult; +} + +bool SDKClientPlatformSpecific::PlatformSpecificShutdown(void) +{ + // Ncurses. + endwin(); + + return true; +} + +void SDKClientPlatformSpecific::UpdateInput(void) +{ + g_Input.Update(); +} + +/*static*/ bool SDKClientPlatformSpecific::CopyString( + char* const p_Target, + const size_t p_MaxLengthThatWillFitInTarget, + const std::string& p_Source) +{ + if (!p_Target) + { + ClientLog::error( + "Tried to copy a string, but the target was null. The string was \"{}\".", + p_Source.c_str()); + + return false; + } + + if (p_MaxLengthThatWillFitInTarget == 0) + { + ClientLog::error( + "Tried to copy a string, but the target's size is zero. The string was \"{}\".", + p_Source.c_str()); + + return false; + } + + if (p_MaxLengthThatWillFitInTarget <= p_Source.length()) + { + ClientLog::error( + "Tried to copy a string that was longer than {} characters, which makes it too big for its target buffer. The string was \"{}\".", + p_MaxLengthThatWillFitInTarget, + p_Source.c_str()); + + return false; + } + + strcpy(p_Target, p_Source.c_str()); + + return true; +} + +bool SDKClientPlatformSpecific::ResizeWindow( + const short int p_ConsoleWidth, + const short int p_ConsoleHeight, + const short int p_ConsoleScrollback) +{ + // https://apple.stackexchange.com/questions/33736/can-a-terminal-window-be-resized-with-a-terminal-command/47841#47841 + // Use a control sequence to resize the window. + // Seems to be supported by the default terminal used in Gnome, as well + // as Mac OS. + // \\e[ -> ASCII ESC character (number 27, or 0x1B) + // -> control sequence introducer + // 8; -> resize the window + // y;xt -> The first number is the height, the second the width. + // Scrollback can't and doesn't need to be set here for Linux. + printf("\e[8;%d;%dt", p_ConsoleHeight, p_ConsoleWidth); + + ClearConsole(); + + // None of the ncurses functions for resizing the terminal actually + // seem to do anything. + /*if (resizeterm(180, 180) != OK) + { + return false; + }*/ + + return true; +} + +void SDKClientPlatformSpecific::ApplyConsolePosition( + const int p_ConsoleCurrentOffset) +{ + printf("\e[%d;1H", p_ConsoleCurrentOffset); +} + +/*static*/ void SDKClientPlatformSpecific::ClearConsole(void) +{ + // https://stackoverflow.com/questions/4062045/clearing-terminal-in-linux-with-c-code + // Use a control sequence to clear the terminal and move the cursor. + // Seems to be supported by the default terminal used in Gnome, + // as well as Mac OS. + // \\e[ -> ASCII ESC character (number 27, or 0x1B) -> control sequence introducer + // 2 -> the entire screen + // J -> clear the screen + //printf("\e[2J\n"); + + // Move the cursor to row 1 column 1. + //printf("\e[1;1H\n"); + + if (clear() != OK) + { + ClientLog::error("Failed to clear the screen."); + } + + refresh(); +} + +bool SDKClientPlatformSpecific::GetKey(const int p_Key) +{ + return g_Input.GetKey(p_Key); +} + +bool SDKClientPlatformSpecific::GetKeyDown(const int p_Key) +{ + return g_Input.GetKeyDown(p_Key); +} + +bool SDKClientPlatformSpecific::GetKeyUp(const int p_Key) +{ + return g_Input.GetKeyUp(p_Key); +} + +std::string SDKClientPlatformSpecific::GetDocumentsDirectoryPath_UTF8(void) +{ + const char* const t_Xdg = getenv("XDG_DOCUMENTS_DIR"); + + // Backup - the documents folder is usually going to be in $HOME/Documents. + const char* const t_Home = getenv("HOME"); + if (!t_Xdg && !t_Home) + { + return std::string(""); + } + + const std::string t_DocumentsDir = + (!t_Xdg || strlen(t_Xdg) == 0) + ? std::string(t_Home) + std::string("/Documents") + : std::string(t_Xdg); + + return t_DocumentsDir; +} + +std::ifstream SDKClientPlatformSpecific::GetInputFileStream( + std::string p_Path_UTF8) +{ + return std::ifstream(p_Path_UTF8, std::ifstream::binary); +} + +std::ofstream SDKClientPlatformSpecific::GetOutputFileStream( + std::string p_Path_UTF8) +{ + return std::ofstream(p_Path_UTF8, std::ofstream::binary); +} + +bool SDKClientPlatformSpecific::DoesFolderOrFileExist(std::string p_Path_UTF8) +{ + return std::filesystem::exists(p_Path_UTF8); +} + +void SDKClientPlatformSpecific::CreateFolderIfItDoesNotExist( + std::string p_Path_UTF8) +{ + if (!DoesFolderOrFileExist(p_Path_UTF8)) + { + std::filesystem::create_directory(p_Path_UTF8); + } +} diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.hpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.hpp new file mode 100644 index 0000000..4cbbe10 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecific.hpp @@ -0,0 +1,90 @@ +#ifndef _CLIENT_PLATFORM_SPECIFIC_HPP_ +#define _CLIENT_PLATFORM_SPECIFIC_HPP_ + +#include "ClientPlatformSpecificTypes.hpp" + +// size_t +#include +// std::string +#include + +// Set up a Doxygen group. +/** @addtogroup SDKClient + * @{ + */ + +class SDKClientPlatformSpecific +{ +protected: + /// @brief Initialise things only needed for this platform. + bool PlatformSpecificInitialization(void); + + /// @brief Shut down things only needed for this platform. + bool PlatformSpecificShutdown(void); + + /// @brief Update the current keyboard state. + void UpdateInput(void); + + /// @brief Copy the given string into the given target. + static bool CopyString( + char* const p_Target, + const size_t p_MaxLengthThatWillFitInTarget, + const std::string& p_Source); + + /// @brief Resize the window, so the log messages will fit. + /// Make sure not to enter illegal sizes in here or SetConsoleWindowInfo + /// and SetConsoleScreenBufferSize can generate an error. + /// This is not code directly needed for the SDK to function, but its handy + /// for a quick and simple output for this client. + bool ResizeWindow( + const short int p_ConsoleWidth, + const short int p_ConsoleHeight, + const short int p_ConsoleScrollback); + + /// @brief Set the current console position. + /// This handles the platform-specific part of advancing the console + /// position. + void ApplyConsolePosition( + const int p_ConsoleCurrentOffset); + + /// @brief Clear the console window. + static void ClearConsole(void); + + /// @brief Gets key's current state. + /// True is pressed false is not pressed. + bool GetKey(const int p_Key); + + /// @brief Returns true first time it is called when key is pressed. + bool GetKeyDown(const int p_Key); + + /// @brief Returns true first time it is called when key is released. + bool GetKeyUp(const int p_Key); + + /// @brief Get the path to the user's Documents folder. + /// The string should be in UTF-8 format. + std::string GetDocumentsDirectoryPath_UTF8(void); + + /// @brief Get an input stream for the given file. + /// The file's path should be in UTF-8 format. + std::ifstream GetInputFileStream(std::string p_Path_UTF8); + + /// @brief Get an output stream for the given file. + /// The file's path should be in UTF-8 format. + std::ofstream GetOutputFileStream(std::string p_Path_UTF8); + + /// @brief Check if the given folder or file exists. + /// The folder path given should be in UTF-8 format. + bool DoesFolderOrFileExist(std::string p_Path_UTF8); + + /// @brief Create the given folder if it does not exist. + /// The folder path given should be in UTF-8 format. + void CreateFolderIfItDoesNotExist(std::string p_Path_UTF8); + + /// @brief The slash character that is used in the filesystem. + static const std::string s_SlashForFilesystemPath; +}; + +// Close the Doxygen group. +/** @} */ + +#endif diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecificTypes.hpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecificTypes.hpp new file mode 100644 index 0000000..2c994b3 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ClientPlatformSpecificTypes.hpp @@ -0,0 +1,48 @@ +#ifndef _CLIENT_PLATFORM_SPECIFIC_TYPES_HPP_ +#define _CLIENT_PLATFORM_SPECIFIC_TYPES_HPP_ + +#include + +// Set up a Doxygen group. +/** @addtogroup SDKClient + * @{ + */ + +// Virtual key code characters for WasKeyPressed(). +// Based on key codes used with GetAsyncKeyState(). +// Manually checked what values getch() returns for these keys. +// https://docs.microsoft.com/en-us/windows/win32/inputdev/virtual-key-codes +enum MicrosoftVirtualKeyCodes +{ + VK_TAB = 9, // ASCII tab character value + VK_RETURN = 13, // ASCII carriage return character value + VK_ESCAPE = 27, // ASCII ESC character value + VK_DOWN = 258, + VK_UP = 259, + VK_LEFT = 260, + VK_RIGHT = 261, + VK_HOME = 262, + VK_BACK = 263, + VK_F1 = 265, + VK_F2 = 266, + VK_F3 = 267, + VK_F4 = 268, + VK_F5 = 269, + VK_F6 = 270, + VK_F7 = 271, + VK_F8 = 272, + VK_F9 = 273, + VK_F10 = 274, + VK_F11 = 275, + VK_F12 = 276, + VK_DELETE = 330, + VK_INSERT = 331, + VK_NEXT = 338, + VK_PRIOR = 339, + VK_END = 360 +}; + +// Close the Doxygen group. +/** @} */ + +#endif diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile new file mode 100644 index 0000000..7e127a7 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile @@ -0,0 +1,88 @@ +# Copyright 2018 gRPC authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.# +# +# Based on https://hub.docker.com/r/grpc/cxx. + +# To build +# docker build -f ./Dockerfile -t manus-linux . + +# To Run (Linux) +# docker run --net==host --privileged -v /dev:/dev -v /run/udev:/run/udev -i -t manus-linux /bin/bash + +# To Run (Windows) +# docker run -p 5000:5000 -i -t manus-linux /bin/bash + +# FROM ubuntu:jammy as build +FROM ubuntu:focal as build +LABEL description="Visual Studio Manus SDK Build container" + +ENV TZ=Europe/Amsterdam +RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone + +RUN apt-get update && apt-get install -y \ + build-essential \ + # Required to install GRPC + git \ + libtool \ + libzmq3-dev \ + # Required for core integrated + libusb-1.0-0-dev \ + libudev-dev \ + # Only required for building the minimal client + libncurses5-dev \ + # Only required for visual studio debugging + gdb \ + && apt-get clean + +ENV GRPC_RELEASE_TAG="v1.28.1" + +RUN git clone -b ${GRPC_RELEASE_TAG} https://github.com/grpc/grpc /var/local/git/grpc && \ + cd /var/local/git/grpc && \ + git submodule update --init --recursive + +RUN echo "-- installing protobuf" && \ + cd /var/local/git/grpc/third_party/protobuf && \ + ./autogen.sh && ./configure --enable-shared && \ + make -j$(nproc) && make -j$(nproc) check && make install && make clean && ldconfig + +RUN echo "-- installing grpc" && \ + cd /var/local/git/grpc && \ + make -j$(nproc) && make install && make clean && ldconfig + +# configure SSH for communication with Visual Studio +RUN apt-get update && apt-get install -y openssh-server +RUN mkdir -p /var/run/sshd +RUN echo 'PasswordAuthentication yes' >> /etc/ssh/sshd_config && \ + ssh-keygen -A + +# please make sure to change the password +RUN id -u manus >/dev/null 2>&1 || useradd -m -d /home/manus -s /bin/bash -G sudo manus && \ +echo "manus:password" | chpasswd + +# copy dependencies to ~/ManusSDK/ directory +COPY ManusSDK /home/manus/ManusSDK + +# Add read/write permissions for manus devices +RUN apt-get update && apt-get install -y udev +RUN echo "# HIDAPI/libusb" > /etc/udev/rules.d/99-manus.rules && \ + echo "SUBSYSTEMS==\"usb\", ATTRS{idVendor}==\"3325\", MODE:=\"0666\"" >> /etc/udev/rules.d/99-manus.rules && \ + echo "# HIDAPI/hidraw" >> /etc/udev/rules.d/99-manus.rules && \ + echo "KERNEL==\"hidraw*\", ATTRS{idVendor}==\"3325\", MODE:=\"0666\"" >> /etc/udev/rules.d/99-manus.rules + +# change default SSH port to 5000 to not conflict with system ssh in host mode +RUN echo "Port 5000" >> /etc/ssh/sshd_config + +ENTRYPOINT service ssh start && service udev start && bin/bash + +EXPOSE 5000 diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile.Integrated b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile.Integrated new file mode 100644 index 0000000..0a810e8 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Dockerfile.Integrated @@ -0,0 +1,69 @@ +# Copyright 2018 gRPC authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.# +# +# Based on https://hub.docker.com/r/grpc/cxx. + +# To build +# docker build -f ./Dockerfile.Integrated -t manus-linux-integrated . + +# To Run (Linux) +# docker run -p 5000:5000 --privileged -v /dev:/dev -v /run/udev:/run/udev -i -t manus-linux-integrated /bin/bash + +# To Run (Windows) +# docker run -p 5000:5000 -i -t manus-linux-integrated /bin/bash + +# FROM ubuntu:jammy as build +FROM ubuntu:focal as build +LABEL description="Visual Studio Manus SDK Build container" + +ENV TZ=Europe/Amsterdam +RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone + +RUN apt-get update && apt-get install -y \ + build-essential \ + # Required for core integrated + libusb-1.0-0-dev \ + libudev-dev \ + # Only required for building the minimal client + libncurses5-dev \ + # Only required for visual studio debugging + gdb \ + && apt-get clean + +# configure SSH for communication with Visual Studio +RUN apt-get update && apt-get install -y openssh-server +RUN mkdir -p /var/run/sshd +RUN echo 'PasswordAuthentication yes' >> /etc/ssh/sshd_config && \ + ssh-keygen -A + +# please make sure to change the password +RUN id -u manus >/dev/null 2>&1 || useradd -m -d /home/manus -s /bin/bash -G sudo manus && \ +echo "manus:password" | chpasswd + +# copy dependencies to ~/ManusSDK/ directory +COPY ManusSDK /home/manus/ManusSDK + +# Add read/write permissions for manus devices +RUN apt-get update && apt-get install -y udev +RUN echo "# HIDAPI/libusb" > /etc/udev/rules.d/99-manus.rules && \ + echo "SUBSYSTEMS==\"usb\", ATTRS{idVendor}==\"3325\", MODE:=\"0666\"" >> /etc/udev/rules.d/99-manus.rules && \ + echo "# HIDAPI/hidraw" >> /etc/udev/rules.d/99-manus.rules && \ + echo "KERNEL==\"hidraw*\", ATTRS{idVendor}==\"3325\", MODE:=\"0666\"" >> /etc/udev/rules.d/99-manus.rules + +# change default SSH port to 5000 to not conflict with system ssh in host mode +RUN echo "Port 5000" >> /etc/ssh/sshd_config + +ENTRYPOINT service ssh start && service udev start && bin/bash + +EXPOSE 5000 \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/Main.cpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Main.cpp new file mode 100644 index 0000000..3749ad2 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Main.cpp @@ -0,0 +1,52 @@ +// Set up a Doxygen group. +/** @addtogroup Main + * @{ + */ + +#include "ClientLogging.hpp" +#include "SDKClient.hpp" +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + + +ClientReturnCode t_Result; +SDKClient t_SDKClient; + +int init() { + t_SDKClient.Initialize(); + t_SDKClient.Run(); + while (output_map.size() == 0){ + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return 0; +} + +py::dict get_latest_state() { + py::dict py_dict; + for (const auto& pair : output_map) { + py::list py_list; // Initialize a py::list for each vector + for (double value : pair.second) { + py_list.append(value); // Append each value from the vector to the py::list + } + py_dict[py::str(pair.first)] = py_list; // Assign the list to the dict with the string key + } + return py_dict; +} + +int shutdown() { + t_SDKClient.ShutDown(); + std::cout<<"Manus shutdown\n"; + return 0; +} + +PYBIND11_MODULE(ManusServer, m) { + m.def("init", &init); + m.def("get_latest_state", &get_latest_state); + m.def("shutdown", &shutdown); +} \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/Makefile b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Makefile new file mode 100644 index 0000000..a5acc7c --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Makefile @@ -0,0 +1,59 @@ +CXX := g++ + +#CXXFLAGS := -pedantic-errors -Wall -Wextra -Werror -g --std=c++17 +# CXXFLAGS := -g --std=c++17 + +PYBIND11_INCLUDES := $(shell python3 -m pybind11 --includes) +PYBIND11_SUFFIX := $(shell python3-config --extension-suffix) +CXXFLAGS := -shared -g --std=c++17 -fPIC ${PYBIND11_INCLUDES} + +LDFLAGS := -L./ManusSDK/lib -lManusSDK -lncurses -lpthread -Wl,-rpath=./ManusSDK/lib +BUILD := ./ +OBJ_DIR := $(BUILD)/objects +APP_DIR := $(BUILD) + +# TARGET := SDKClient_Linux.out +TARGET := ManusServer$(PYBIND11_SUFFIX) + +INCLUDE := -I./ManusSDK/include +SRC := \ + $(wildcard *.cpp) + +OBJECTS := $(SRC:%.cpp=$(OBJ_DIR)/%.o) +DEPENDENCIES \ + := $(OBJECTS:.o=.d) + +all: build $(APP_DIR)/$(TARGET) + +$(OBJ_DIR)/%.o: %.cpp + @mkdir -p $(@D) + $(CXX) $(CXXFLAGS) $(INCLUDE) -c $< -MMD -o $@ + +$(APP_DIR)/$(TARGET): $(OBJECTS) + @mkdir -p $(@D) + $(CXX) $(CXXFLAGS) -o $(APP_DIR)/$(TARGET) $^ $(LDFLAGS) + +-include $(DEPENDENCIES) + +.PHONY: all build clean debug release info + +build: + @mkdir -p $(APP_DIR) + @mkdir -p $(OBJ_DIR) + +debug: CXXFLAGS += -DDEBUG -g +debug: all + +release: CXXFLAGS += -g +release: all + +clean: + -@rm $(TARGET) + -@rm -rvf $(OBJ_DIR)/* + +info: + @echo "[*] Application dir: ${APP_DIR} " + @echo "[*] Object dir: ${OBJ_DIR} " + @echo "[*] Sources: ${SRC} " + @echo "[*] Objects: ${OBJECTS} " + @echo "[*] Dependencies: ${DEPENDENCIES}" diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDK.h b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDK.h new file mode 100644 index 0000000..173a8e3 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDK.h @@ -0,0 +1,1058 @@ +#ifndef __MANUS_SDK_H__ +#define __MANUS_SDK_H__ + +// Set up a Doxygen group. +/** @addtogroup ManusSDK + * @{ + */ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "ManusSDKTypes.h" +#include "ManusSDKTypeInitializers.h" + +#include + +#ifdef MANUS_SDK_STATIC +#define CORESDK_API +#else +#ifdef _WIN32 + // MANUS_SDK_EXPORTS defines if the functions are exported to or + // imported from a DLL. +#ifdef MANUS_SDK_EXPORTS +#define CORESDK_API __declspec(dllexport) +#else +#define CORESDK_API __declspec(dllimport) +#endif +#elif defined(__linux__) +#define CORESDK_API +#else +#error Unrecognized platform. +#endif +#endif + + /****************************************************************************** + * Wrapper startup and shutdown. + *****************************************************************************/ + + /// @brief Initialize the wrapper. + /// Call this before using the wrapper. + /// @param p_TypeOfSession + /// @param p_Remote If set to true, this is essentially the same as calling the old CoreSdk_Initialize + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_FunctionCalledAtWrongTime if the function was not intended to be called at this time. + CORESDK_API SDKReturnCode CoreSdk_Initialize(SessionType p_TypeOfSession, bool p_Remote); + + /// @brief Shut down the wrapper. + /// This needs to be called last. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_ShutDown(); + + /****************************************************************************** + * Utility functions. + *****************************************************************************/ + + /// @brief Check if the wrapper DLL was built in the debug configuration. + /// @param p_WasBuiltInDebugConfiguration + /// @return SDKReturnCode_Success if successful. + CORESDK_API SDKReturnCode CoreSdk_WasDllBuiltInDebugConfiguration(bool* p_WasBuiltInDebugConfiguration); + + /// @brief Gets the timestamp info (more readable form of timestamp). + /// @param p_Timestamp Timestamp to get info from + /// @param p_Info Info of the timestamp + /// @return SDKReturnCode_Success if successful. + CORESDK_API SDKReturnCode CoreSdk_GetTimestampInfo(ManusTimestamp p_Timestamp, ManusTimestampInfo* p_Info); + + /// @brief Sets the timestamp according to the info (more readable form of timestamp). + /// @param p_Timestamp the Timestamp to set info of + /// @param p_Info Info to get info from + /// @return SDKReturnCode_Success if successful. + CORESDK_API SDKReturnCode CoreSdk_SetTimestampInfo(ManusTimestamp* p_Timestamp, ManusTimestampInfo p_Info); + + + /****************************************************************************** + * Connection handling. + *****************************************************************************/ + + /// @brief Start a background task that looks for hosts running Manus Core. + /// Call this first when looking for hosts to connect to. + /// Underlying function will sleep for p_WaitSeconds to allow servers to reply. + /// @param p_WaitSeconds defines the time the function will sleep for to allow servers to reply, + /// @param p_LoopbackOnly if true it looks for hosts only locally, if false it looks for hosts anywhere in the network, + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_FunctionCalledAtWrongTime if the function was not intended to be called at this time. + CORESDK_API SDKReturnCode CoreSdk_LookForHosts(uint32_t p_WaitSeconds = 1, bool p_LoopbackOnly = false); + + /// @brief Get the number of hosts running Manus Core that were found. + /// This is the second function to call when looking for hosts to connect to. + /// @param p_NumberOfAvailableHostsFound + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_FunctionCalledAtWrongTime if the function was not intended to be called at this time. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableHostsFound(uint32_t* p_NumberOfAvailableHostsFound); + + /// @brief Fill the given array with information on the hosts that were found. + /// This is the third and final function to call when looking for hosts to + /// connect to. + /// @param p_AvailableHostsFound + /// @param p_NumberOfHostsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_FunctionCalledAtWrongTime if the function was not intended to be called at this time + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfHostsThatFitInArray does not match the number of available hosts found in the network, + /// SDKReturnCode_InvalidArgument if the provided p_AvailableHostsFound is a null pointer, + /// SDKReturnCode_InternalError if the number of available hosts found is higher than the defined MAX_NUMBER_OF_HOSTS, + /// SDKReturnCode_UnsupportedStringSizeEncountered if the name of a host is higher than the defined MAX_NUM_CHARS_IN_HOST_NAME, + /// SDKReturnCode_Error if copying the string representing the host name was not successful, + /// SDKReturnCode_NullPointer if invalid host data was found. + CORESDK_API SDKReturnCode CoreSdk_GetAvailableHostsFound(ManusHost* p_AvailableHostsFound, const uint32_t p_NumberOfHostsThatFitInArray); + + + /// @brief Check if the wrapper is connected to ManusCore. + /// @param p_ConnectedToCore + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_GetIsConnectedToCore(bool* p_ConnectedToCore); + + /// @brief Connect to the preset GRPC address. + /// @return SDKReturnCode_Success + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_ConnectGRPC(); + + /// @brief Disconnects from the current host + /// @return SDKReturnCode_Success + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_Disconnect(); + + /// @brief Connect to a host using the given host information. + /// @param p_Host + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_SdkIsTerminating if the sdk is being terminated, + /// SDKReturnCode_FunctionCalledAtWrongTime if the function was not intended to be called at this time, + /// SDKReturnCode_NoCoordinateSystemSet if the user tried to connect without first setting up the coordinate system settings. + CORESDK_API SDKReturnCode CoreSdk_ConnectToHost(ManusHost p_Host); + + /// @brief Initialize a coordinate system of type CoordinateSystemVUH. (View Up Handedness) + /// This has to be called before a connection is made. + /// @param p_CoordinateSystem + /// @param p_UseWorldCoordinates if true all data will be defined with respect to a world coordinate system, + /// if false all data will be defined with respect to a local one, + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_InitializeCoordinateSystemWithVUH(CoordinateSystemVUH p_CoordinateSystem, bool p_UseWorldCoordinates); + + /// @brief Initialize a coordinate system of type CoordinateSystemDirection. + /// This has to be called before a connection is made. + /// @param p_CoordinateSystem + /// @param p_UseWorldCoordinates if true all data will be defined with respect to a world coordinate system, + /// if false all data will be defined with respect to a local one, + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_InitializeCoordinateSystemWithDirection(CoordinateSystemDirection p_CoordinateSystem, bool p_UseWorldCoordinates); + + /// @brief Get the SDK and Core version used and check if they are compatible with each other. + /// @param p_SdkVersion + /// @param p_CoreVersion + /// @param p_AreVersionsCompatible + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_GetVersionsAndCheckCompatibility(ManusVersion* p_SdkVersion, ManusVersion* p_CoreVersion, bool* p_AreVersionsCompatible); + + /// @brief Get the current session Id. + /// @param p_SessionId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_InvalidID if the session id is zero, which means it was not correctly assigned. + CORESDK_API SDKReturnCode CoreSdk_GetSessionId(uint32_t* p_SessionId); + + /// @brief Register the callback function that is called when manus core gets connected to the sdk. + /// @param p_ConnectedCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForOnConnect(ConnectedToCoreCallback_t p_ConnectedCallback); + + /// @brief Register the callback function that is called when manus core gets disconnected from the sdk. + /// @param p_DisconnectedCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForOnDisconnect(DisconnectedFromCoreCallback_t p_DisconnectedCallback); + + /// @brief Register the callback function that is called when something is logged in the SDK. + /// @param p_LoggingCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForOnLog(LoggingCallback_t p_LoggingCallback); + + /// @brief Register the callback function that is called when skeleton data comes in. + /// @param p_SkeletonStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForSkeletonStream(SkeletonStreamCallback_t p_SkeletonStreamCallback); + + /// @brief Register the callback function that is called when tracker data comes in. + /// @param p_TrackerStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForTrackerStream(TrackerStreamCallback_t p_TrackerStreamCallback); + + /// @brief Register the callback function that is called when the landscape data comes in. + /// @param p_LandscapeStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForLandscapeStream(LandscapeStreamCallback_t p_LandscapeStreamCallback); + + /// @brief Register the callback function that is called when core system related data comes in. + /// @param p_SystemStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForSystemStream(SystemStreamCallback_t p_SystemStreamCallback); + + /// @brief Register the callback function that is called when ergonomics data comes in. + /// @param p_ErgonomicsStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForErgonomicsStream(ErgonomicsStreamCallback_t p_ErgonomicsStreamCallback); + + /// @brief Register the callback function that is called when skeleton data comes in. + /// @param p_SkeletonStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForRawSkeletonStream(RawSkeletonStreamCallback_t p_RawSkeletonStreamCallback); + + /// @brief Register the callback function that is called when gesture data comes in. + /// @param p_GestureStreamCallback + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_RegisterCallbackForGestureStream(GestureStreamCallback_t p_GestureStreamCallback); + + /****************************************************************************** + * Basic glove interaction. + *****************************************************************************/ + + /// @brief Try to pair a glove to a dongle + /// @param p_GloveID + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_PairGlove(uint32_t p_GloveID, bool* p_Result); + + /// @brief Try to unpair a glove to a dongle + /// @param p_GloveID + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available. + CORESDK_API SDKReturnCode CoreSdk_UnpairGlove(uint32_t p_GloveID, bool* p_Result); + + /// @brief Vibrate the motor on the given fingers of a haptic glove. + /// The order of the fingers is Thumb, Index, Middle, Ring, Pinky. + /// @param p_DongleId + /// @param p_HandType + /// @param p_Powers strength of the vibration, should be an array of 5 values + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + /// @deprecated Use CoreSdk_VibrateFingersForGlove or CoreSdk_VibrateFingersForSkeleton instead + [[deprecated("Use CoreSdk_VibrateFingersForGlove or CoreSdk_VibrateFingersForSkeleton instead")]] + CORESDK_API SDKReturnCode CoreSdk_VibrateFingers(uint32_t p_DongleId, Side p_HandType, const float* p_Powers); + + /// @brief Vibrate the motor on the given fingers of a haptic glove. + /// The order of the fingers is Thumb, Index, Middle, Ring, Pinky. + /// @param p_GloveId + /// @param p_Powers strength of the vibration, should be an array of 5 values + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_VibrateFingersForGlove(uint32_t p_GloveId, const float* p_Powers); + + /// @brief Vibrate the motor on the fingers of a haptic glove associated to skeleton with given id. + /// The order of the fingers is Thumb, Index, Middle, Ring, Pinky. + /// @param p_SkeletonId + /// @param p_HandType + /// @param p_Powers strength of the vibration, should be an array of 5 values + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_VibrateFingersForSkeleton(uint32_t p_SkeletonId, Side p_HandType, const float* p_Powers); + + /// @brief Get a glove ID for the given hand of the given user identified by its Id. + /// @param p_UserId + /// @param p_HandType + /// @param p_GloveId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidArgument if the provided p_HandType was neither left nor right, + /// SDKReturnCode_InvalidID if no user with the provided p_UserId was found in the landscape. + CORESDK_API SDKReturnCode CoreSdk_GetGloveIdOfUser_UsingUserId(uint32_t p_UserId, Side p_HandType, uint32_t* p_GloveId); + + /// @brief Get the number of gloves that are available. + /// @param p_NumberOfAvailableGloves + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of available gloves found is higher than the defined MAX_NUMBER_OF_GLOVES. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableGloves(uint32_t* p_NumberOfAvailableGloves); + + /// @brief Fill the given array with the IDs of all available gloves. + /// The size of the given array must match the number of available gloves. + /// Note that the number of available gloves can change at any time. + /// @param p_IdsOfAvailableGloves + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of available gloves found, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or is higher than the defined MAX_NUMBER_OF_GLOVES. + CORESDK_API SDKReturnCode CoreSdk_GetIdsOfAvailableGloves(uint32_t* p_IdsOfAvailableGloves, uint32_t p_NumberOfIdsThatFitInArray); + + /// @brief Get glove id's for given dongle id. + /// @param p_DongleId + /// @param p_LeftGloveId + /// @param p_RightGloveId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetGlovesForDongle(uint32_t p_DongleId, uint32_t* p_LeftGloveId, uint32_t* p_RightGloveId); + + /// @brief Get data for the glove with the given glove ID. + /// @param p_GloveId + /// @param p_GloveData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_DataNotAvailable if no glove with the provided p_GloveId was found in the landscape. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetDataForGlove_UsingGloveId(uint32_t p_GloveId, GloveLandscapeData* p_GloveData); + + /// @brief Gets the data for a dongle with the given id. + /// @param p_DongleId + /// @param p_DongleData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_DataNotAvailable if no dongle with the provided p_DongleId was found in the landscape. + CORESDK_API SDKReturnCode CoreSdk_GetDataForDongle(uint32_t p_DongleId, DongleLandscapeData* p_DongleData); + + /// @brief Get the number of available dongles. + /// @param p_NumberOfDongles + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of dongles found is higher than the defined MAX_NUMBER_OF_DONGLES. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfDongles(uint32_t* p_NumberOfDongles); + + /// @brief Fill the given array with the IDs of all available dongles. + /// The size of the given array must match the number of available dongles. + /// Note that the number of available dongles can change at any time. + /// @param p_DongleIds + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of dongles found, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or higher than the defined MAX_NUMBER_OF_HAPTICS_DONGLES. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetDongleIds(uint32_t* p_DongleIds, uint32_t p_NumberOfIdsThatFitInArray); + + /****************************************************************************** + * Haptics module. + *****************************************************************************/ + + /// @brief Get the number of available haptics dongles. + /// @param p_NumberOfHapticsDongles + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of haptic dongles found is higher than the defined MAX_NUMBER_OF_HAPTICS_DONGLES. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfHapticsDongles(uint32_t* p_NumberOfHapticsDongles); + + /// @brief Fill the given array with the IDs of all available haptics dongles. + /// The size of the given array must match the number of available haptics dongles. + /// Note that the number of available haptics dongles can change at any time. + /// @param p_HapticsDongleIds + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of haptic dongles found, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or higher than the defined MAX_NUMBER_OF_HAPTICS_DONGLES. + /// @deprecated Get this through the Landscape instead + [[deprecated("Get this through the Landscape instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetHapticsDongleIds(uint32_t* p_HapticsDongleIds, uint32_t p_NumberOfIdsThatFitInArray); + + /****************************************************************************** + * Users. + *****************************************************************************/ + + /// @brief Get the number of available users. + /// Note that this is reliant on the landscape, and the first second after connecting the client to the core it will not yet have landscape + /// data, so the number of users wil be 0. Since there will always be a default users, just wait until the count is above 0 and then proceed. + /// @param p_NumberOfAvailableUsers + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of available users found is higher than the defined MAX_NUMBER_OF_USERS. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableUsers(uint32_t* p_NumberOfAvailableUsers); + + /// @brief Fill the given array with the IDs of all available users. + /// The size of the given array must match the number of available users. + /// Note that the number of available users can change at any time depending on the landscape. + /// @param p_IdsOfAvailableUsers, The array of ids that will get filled + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is null or higher than the defined MAX_NUMBER_OF_USERS, or + /// if p_IdsOfAvailableUsers is a null array. + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with number of available users found. + CORESDK_API SDKReturnCode CoreSdk_GetIdsOfAvailableUsers(uint32_t* p_IdsOfAvailableUsers, uint32_t p_NumberOfIdsThatFitInArray); + + /****************************************************************************** + * Tracking. + *****************************************************************************/ + + /// @brief Get data for a tracker at a given index in the TrackerStream callback. + /// @param p_TrackerIndex + /// @param p_TrackerData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_TrackerIndex is higher than the total number of trackers received from the tracker stream. + CORESDK_API SDKReturnCode CoreSdk_GetTrackerData(uint32_t p_TrackerIndex, TrackerData* p_TrackerData); + + /// @brief Get the number of available trackers. + /// @param p_NumberOfAvailableTrackers + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of trackers found is higher than the defined MAX_NUMBER_OF_TRACKERS. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableTrackers(uint32_t* p_NumberOfAvailableTrackers); + + /// @brief Fill the given array with the IDs of available trackers. + /// The size of the given array must match the number of available trackers. + /// Note that the number of available trackers can change at any time. + /// @param p_IdsOfAvailableTrackers + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or higher than the defined MAX_NUMBER_OF_TRACKERS, or + /// if p_IdsOfAvailableTrackers is a null array. + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of available trackers found. + CORESDK_API SDKReturnCode CoreSdk_GetIdsOfAvailableTrackers(TrackerId* p_IdsOfAvailableTrackers, uint32_t p_NumberOfIdsThatFitInArray); + + /// @brief Get the number of available trackers for a user with given Id. + /// @param p_NumberOfAvailableTrackers + /// @param p_UserId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of trackers found is higher than the defined MAX_NUMBER_OF_TRACKERS. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableTrackersForUserId(uint32_t* p_NumberOfAvailableTrackers, uint32_t p_UserId); + + /// @brief Fill the given array with the IDs of available trackers for a user with given Id. + /// The size of the given array must match the number of available trackers. + /// Note that the number of available trackers can change at any time. + /// @param p_IdsOfAvailableTrackers + /// @param p_UserId + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or higher than the defined MAX_NUMBER_OF_TRACKERS, or + /// if p_IdsOfAvailableTrackers is a null array. + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of available trackers found. + CORESDK_API SDKReturnCode CoreSdk_GetIdsOfAvailableTrackersForUserId(TrackerId* p_IdsOfAvailableTrackers, uint32_t p_UserId, uint32_t p_NumberOfIdsThatFitInArray); + + /// @brief Get the number of available trackers for a user with given Id. + /// @param p_NumberOfAvailableTrackers + /// @param p_UserIndex + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InternalError if the number of trackers found is higher than the defined MAX_NUMBER_OF_TRACKERS. + CORESDK_API SDKReturnCode CoreSdk_GetNumberOfAvailableTrackersForUserIndex(uint32_t* p_NumberOfAvailableTrackers, uint32_t p_UserIndex); + + /// @brief Fill the given array with the IDs of available trackers for a user with given Id. + /// The size of the given array must match the number of available trackers. + /// Note that the number of available trackers can change at any time. + /// @param p_IdsOfAvailableTrackers + /// @param p_UserIndex + /// @param p_NumberOfIdsThatFitInArray + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidArgument if p_NumberOfIdsThatFitInArray is zero or higher than the defined MAX_NUMBER_OF_TRACKERS, or + /// if p_IdsOfAvailableTrackers is a null array. + /// SDKReturnCode_ArgumentSizeMismatch if p_NumberOfIdsThatFitInArray does not match with the number of available trackers found. + CORESDK_API SDKReturnCode CoreSdk_GetIdsOfAvailableTrackersForUserIndex(TrackerId* p_IdsOfAvailableTrackers, uint32_t p_UserIndex, uint32_t p_NumberOfIdsThatFitInArray); + + + + /// @brief Get data for the tracker with the given tracker Id. + /// @param p_TrackerId + /// @param p_TrackerData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_DataNotAvailable if no tracker with the given p_TrackerId was found in the landscape. + CORESDK_API SDKReturnCode CoreSdk_GetDataForTracker_UsingTrackerId(TrackerId p_TrackerId, TrackerData* p_TrackerData); + + /// @brief Get data for the tracker with the given user id and type. + /// @param p_UserId + /// @param p_TrackerType + /// @param p_TrackerData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_DataNotAvailable if no tracker with the given p_TrackerId and the given p_TrackerType was found in the landscape. + CORESDK_API SDKReturnCode CoreSdk_GetDataForTracker_UsingIdAndType(uint32_t p_UserId, uint32_t p_TrackerType, TrackerData* p_TrackerData); + + /// @brief Send data to Core for a tracker. + /// @param p_TrackerData + /// @param p_NumberOfTrackers amount of trackers for which we send the information + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_NullPointer if the provided p_TrackerData is a null pointer, + /// SDKReturnCode_InvalidArgument if p_NumberOfTrackers is null or higher than the defined MAX_NUMBER_OF_TRACKERS. + CORESDK_API SDKReturnCode CoreSdk_SendDataForTrackers(const TrackerData* p_TrackerData, uint32_t p_NumberOfTrackers); + + /// @brief Sets the offset of a tracker for a specified user + /// @param p_UserId + /// @param p_TrackerOffset + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_SetTrackerOffset(uint32_t p_UserId, const TrackerOffset* p_TrackerOffset); + + /****************************************************************************** + * Gestures. + *****************************************************************************/ + + /// @brief Get the landscape data for all the gestures in the latest landscape. + /// @param p_LandscapeDataArray + /// @param p_ArraySize + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_ArraySize is higher than the total number of gestures in the landscape stream. + CORESDK_API SDKReturnCode CoreSdk_GetGestureLandscapeData(GestureLandscapeData* p_LandscapeDataArray, uint32_t p_ArraySize); + + /// @brief The start data index allows you to get more data if there are more than MAX_GESTURE_DATA_CHUNK_SIZE gestures. + /// @param p_GestureStreamDataIndex + /// @param p_StartDataIndex + /// @param p_GestureProbabilitiesCollection + /// @return SDKReturnCode_Success if successful, or if there is no more data to get + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_ArraySize is higher than the total number of gestures in the landscape stream. + CORESDK_API SDKReturnCode CoreSdk_GetGestureStreamData(uint32_t p_GestureStreamDataIndex, uint32_t p_StartDataIndex, GestureProbabilities* p_GestureProbabilitiesCollection); + + /****************************************************************************** + * Glove Calibration. + *****************************************************************************/ + + /// @brief Start the calibration for a glove with the id that is specified in the arguments. + /// @param p_CalibrationArgs + /// @param p_Result + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationStart(GloveCalibrationArgs p_CalibrationArgs, bool* p_Result); + + /// @brief Stop the calibration for a glove with the id that is specified in the arguments. + /// @param p_CalibrationArgs + /// @param p_Result + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationStop(GloveCalibrationArgs p_CalibrationArgs, bool* p_Result); + + /// @brief Finish the calibration for a glove with the id that is specified in the arguments. + /// @param p_CalibrationArgs + /// @param p_Result + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationFinish(GloveCalibrationArgs p_CalibrationArgs, bool* p_Result); + + + /// @brief Get the number of steps in a glove calibration sequence. + /// @param p_CalibrationArgs + /// @param p_NumberOfSteps + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationGetNumberOfSteps(GloveCalibrationArgs p_CalibrationArgs, uint32_t* p_NumberOfSteps); + + /// @brief Get information data about a specific calibration step for a glove. + /// @param p_CalibrationStepArgs + /// @param p_Data + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationGetStepData(GloveCalibrationStepArgs p_CalibrationStepArgs, GloveCalibrationStepData* p_Data); + + /// @brief Start a specific calibration step for a glove. + /// @param p_CalibrationStepArgs + /// @param p_Result + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core + CORESDK_API SDKReturnCode CoreSdk_GloveCalibrationStartStep(GloveCalibrationStepArgs p_CalibrationStepArgs, bool* p_Result); + + + CORESDK_API SDKReturnCode CoreSdk_GetGloveCalibrationSize(uint32_t p_GloveId, uint32_t* p_Size); + + CORESDK_API SDKReturnCode CoreSdk_GetGloveCalibration(unsigned char* p_CalibrationBytes, uint32_t p_BytesLength); + + CORESDK_API SDKReturnCode CoreSdk_SetGloveCalibration(uint32_t p_GloveId, unsigned char* p_CalibrationBytes, SetGloveCalibrationReturnCode* p_Result); + + /****************************************************************************** + * Skeletal System. + *****************************************************************************/ + + /// @brief Get information about the final animated skeleton with given index. + /// @param p_SkeletonIndex + /// @param p_Info + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_SkeletonIndex is higher than the total number of skeletons received from the skeleton stream. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonInfo(uint32_t p_SkeletonIndex, SkeletonInfo* p_Info); + + /// @brief Get data for the final animated skeleton with given index. + /// The size of the given array must match the nodes count. + /// Note that the nodes count can change at any time. + /// @param p_SkeletonIndex + /// @param p_Nodes + /// @param p_NodeCount + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_SkeletonIndex is higher than the total number of skeletons received from the skeleton stream or + /// if the provided p_NodeCount is different from the node count of that skeleton. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonData(uint32_t p_SkeletonIndex, SkeletonNode* p_Nodes, uint32_t p_NodeCount); + + /// @brief Determine how the raw data should be moving + /// @param p_HandMotion + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + CORESDK_API SDKReturnCode CoreSdk_SetRawSkeletonHandMotion(HandMotion p_HandMotion); + + /// @brief Get the raw skeleton hand motion type + /// @param p_HandMotion + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonHandMotion(HandMotion* p_HandMotion); + + /// @brief Get information about the raw skeleton data with given index. + /// @param p_SkeletonIndex + /// @param p_Info + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_SkeletonIndex is higher than the total number of skeletons received from the skeleton stream. + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonInfo(uint32_t p_SkeletonIndex, RawSkeletonInfo* p_Info); + + /// @brief Get data for the raw skeleton with given index. + /// The size of the given array must match the nodes count. + /// Note that the nodes count can change at any time. + /// @param p_SkeletonIndex + /// @param p_Nodes + /// @param p_NodeCount + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if p_SkeletonIndex is higher than the total number of skeletons received from the skeleton stream or + /// if the provided p_NodeCount is different from the node count of that skeleton. + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonData(uint32_t p_SkeletonIndex, SkeletonNode* p_Nodes, uint32_t p_NodeCount); + + /// @brief Identifies if the glove associated to the skeleton with given id and side is a haptic one. + /// @param p_SkeletonId + /// @param p_HandType + /// @param p_IsHaptics + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_DataNotAvailable if the requested glove is not found or if the skeleton does not have any user associated to it, + /// SDKReturnCode_InvalidArgument if the provided p_HandType is neither Left nor Right. + CORESDK_API SDKReturnCode CoreSdk_DoesSkeletonGloveSupportHaptics(uint32_t p_SkeletonId, Side p_HandType, bool* p_IsHaptics); + /****************************************************************************** + * Skeletal System Setup. + *****************************************************************************/ + + /// @brief Create a new SkeletonSetup with the given information and returns the index on which it is saved. + /// @param p_Skeleton The SkeletonSetup information used by this skeleton. + /// @param p_SkeletonSetupIndex The index which is used by this SkeletonSetup. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_ArgumentSizeMismatch if the maximum number of temporary skeletons per session has been reached, + /// when this happens the user can either load or clear a temporary skeleton in order to create a new skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_CreateSkeletonSetup(SkeletonSetupInfo p_Skeleton, uint32_t* p_SkeletonSetupIndex); + + /// @brief Add a node to a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_Node + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddNodeToSkeletonSetup(uint32_t p_SkeletonSetupIndex, NodeSetup p_Node); + + /// @brief Add a chain to a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_Chain + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddChainToSkeletonSetup(uint32_t p_SkeletonSetupIndex, ChainSetup p_Chain); + + /// @brief Add a collider to a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_Collider + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddColliderToSkeletonSetup(uint32_t p_SkeletonSetupIndex, ColliderSetup p_Collider); + + /// @brief Add a mesh setup to a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_NodeID + /// @param p_MeshSetupIndex + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddMeshSetupToSkeletonSetup(uint32_t p_SkeletonSetupIndex, uint32_t p_NodeID, uint32_t* p_MeshSetupIndex); + + /// @brief Add a Vertex to a mesh setup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_MeshSetupIndex + /// @param p_Vertex + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddVertexToMeshSetup(uint32_t p_SkeletonSetupIndex, uint32_t p_MeshSetupIndex, Vertex p_Vertex); + + + /// @brief Add a Triangle to a mesh setup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_MeshSetupIndex + /// @param p_Triangle + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AddTriangleToMeshSetup(uint32_t p_SkeletonSetupIndex, uint32_t p_MeshSetupIndex, Triangle p_Triangle); + + /// @brief Overwrite an existing SkeletonSetup at a given index. + /// This will remove all chains and nodes from the current skeleton at this index. + /// @param p_SkeletonSetupIndex + /// @param p_SkeletonSetup + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup, + /// SDKReturnCode_NullPointer if the skeleton setup was not correctly saved, thus the new skeleton setup is a null pointer. + CORESDK_API SDKReturnCode CoreSdk_OverwriteSkeletonSetup(uint32_t p_SkeletonSetupIndex, SkeletonSetupInfo p_SkeletonSetup); + + /// @brief Overwrite a chain in a SkeletonSetup, the chain to be overwritten is idenfified by p_Chain.id. + /// @param p_SkeletonSetupIndex The SkeletonSetup's index, not to be confused by the Chain's ID. + /// @param p_Chain The chain to replace the matching (via id) chain with. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_OverwriteChainToSkeletonSetup(uint32_t p_SkeletonSetupIndex, ChainSetup p_Chain); + + /// @brief Overwrite a node in a SkeletonSetup, the node to be overwritten is identified by p_Node.id. + /// @param p_SkeletonSetupIndex The SkeletonSetup's index, not to be confused by the Chain's ID. + /// @param p_Node The node to replace the matching (via id) node with. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_OverwriteNodeToSkeletonSetup(uint32_t p_SkeletonSetupIndex, NodeSetup p_Node); + + /// @brief Get the skeleton info (number of nodes and chains) for a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_SkeletonInfo + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupArraySizes(uint32_t p_SkeletonSetupIndex, SkeletonSetupArraySizes* p_SkeletonInfo); + + /// @brief Allocate chains for a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_AllocateChainsForSkeletonSetup(uint32_t p_SkeletonSetupIndex); + CORESDK_API SDKReturnCode CoreSdk_PrepareSkeletonSetup(uint32_t p_SkeletonSetupIndex); + + /// @brief Get setup info for a SkeletonSetup at a given index. + /// @param p_SkeletonSetupIndex + /// @param p_SDK + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupInfo(uint32_t p_SkeletonSetupIndex, SkeletonSetupInfo* p_SDK); + + /// @brief Get setup chains for a SkeletonSetup at a given index. + /// The size of the given array must match the chains count. + /// Note that the chains count can change at any time. + /// @param p_SkeletonSetupIndex + /// @param p_SDK + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_NullPointer if the provided p_SDK (used to store the setup chains) is a null pointer, it should be initialized to + /// an array with size equal to the chains count, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + /// @deprecated Use GetSkeletonSetupChainsArray() instead + [[deprecated ("Use GetSkeletonSetupChainsArray() instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupChains(uint32_t p_SkeletonSetupIndex, ChainSetup* p_SDK); + + /// @brief Get setup chains for a SkeletonSetup at a given index. + /// The size of the given array must match the chains count. + /// Note that the chains count can change at any time. + /// @param p_SkeletonSetupIndex + /// @param p_ChainSetupArray + /// @param p_ArraySize + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_NullPointer if the provided p_ChainSetupArray (used to store the setup chains) is a null pointer, it should be initialized to + /// an array with size equal to the chains count, + /// SDKReturnCode_ArgumentSizeMismatch if the provided p_ArraySize is not equal to the chains count, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupChainsArray(uint32_t p_SkeletonSetupIndex, ChainSetup* p_ChainSetupArray, uint32_t p_ArraySize); + + /// @brief Get setup nodes for a SkeletonSetup at a given index. + /// The size of the given array must match the nodes count. + /// Note that the nodes count can change at any time. + /// @param p_SkeletonSetupIndex + /// @param p_SDK + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_NullPointer if the provided p_SDK (used to store the setup nodes) is a null pointer, it should be initialized to + /// an array with size equal to the nodes count, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + /// @deprecated Use GetSkeletonSetupNodesArray() instead + [[deprecated ("Use GetSkeletonSetupNodesArray() instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupNodes(uint32_t p_SkeletonSetupIndex, NodeSetup* p_SDK); + + /// @brief Get setup nodes for a SkeletonSetup at a given index. + /// The size of the given array must match the nodes count. + /// Note that the nodes count can change at any time. + /// @param p_SkeletonSetupIndex + /// @param p_NodeSetupArray + /// @param p_ArraySize + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_NullPointer if the provided p_SDK (used to store the setup nodes) is a null pointer, it should be initialized to + /// an array with size equal to the nodes count, + /// SDKReturnCode_ArgumentSizeMismatch if the provided p_ArraySize is not equal to the nodes count, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupNodesArray(uint32_t p_SkeletonSetupIndex, NodeSetup* p_NodeSetupArray, uint32_t p_ArraySize); + + /// @brief Get setup colliders for a SkeletonSetup at a given index. + /// The size of the given array must match the colliders count. + /// Note that the colliders count can change at any time. + /// @param p_SkeletonSetupIndex + /// @param p_SDK + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_NullPointer if the provided p_SDK (used to store the setup colliders) is a null pointer, it should be initialized to + /// an array with size equal to the colliders count, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + /// @deprecated + [[deprecated]] + CORESDK_API SDKReturnCode CoreSdk_GetSkeletonSetupColliders(uint32_t p_SkeletonSetupIndex, ColliderSetup* p_SDK); + + /// @brief Get the amount of nodes for the raw skeleton with given id + /// @param p_GloveId + /// @param p_NodeCount + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided glove id does not match any skeleton in core + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonNodeCount(uint32_t p_GloveId, uint32_t& p_NodeCount); + + /// @brief Get the information for nodes of the raw skeleton with given id + /// The size of the given array p_NodeInfo must match the node count retrieved from CoreSdk_GetRawSkeletonNodeCount. + /// @param p_GloveId + /// @param p_NodeInfo + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided glove id does not match any skeleton in core + /// @deprecated Use CoreSdk_GetRawSkeletonNodeInfoArray instead + [[deprecated("Use CoreSdk_GetRawSkeletonNodeInfoArray instead")]] + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonNodeInfo(uint32_t p_GloveId, NodeInfo* p_NodeInfo); + + + /// @brief Get the information for nodes of the raw skeleton with given id + /// The size of the given array p_NodeInfo and p_NodeCount must match the node count retrieved from CoreSdk_GetRawSkeletonNodeCount. + /// @param p_GloveId + /// @param p_NodeInfoArray + /// @param p_ArraySize + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_InvalidID if the provided glove id does not match any skeleton in core, + /// SDKReturnCode_ArgumentSizeMismatch if p_ArraySize does not match node count retrieved from CoreSdk_GetRawSkeletonNodeCount. + CORESDK_API SDKReturnCode CoreSdk_GetRawSkeletonNodeInfoArray(uint32_t p_GloveId, NodeInfo* p_NodeInfoArray, uint32_t p_ArraySize); + + /// @brief Sends a skeleton setup to core to become a skeleton upon which data is applied. + /// Returns the skeleton ID of the loaded skeleton, this ID is used to identify the skeleton in the SkeletonStreamCallback. + /// Loading a skeleton into core for retargetting removes it from the temporary skeletons array, thus causing + /// the p_SkeletonSetupIndex to no longer be valid after this call. + /// @param p_SkeletonSetupIndex The SkeletonSetup's index, will become invalid after this function. + /// @param p_SkeletonId This is the ID of the loaded skeleton. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_LoadSkeleton(uint32_t p_SkeletonSetupIndex, uint32_t* p_SkeletonId); + + /// @brief Unload a skeleton with a certain ID from Manus Core, so data will no longer be generated for it. + /// @param p_SkeletonId The Skeleton's ID which is assigned by Manus Core. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_UnloadSkeleton(uint32_t p_SkeletonId); + + /// @brief Save a SkeletonSetup at a given index and sessionId into Manus Core for use in the Dev Tools. + /// p_IsSkeletonModified is intended to be used mainly by the Dev Tools, it is set to true after saving any modification to the skeleton, + /// this triggers the OnSyStemCallback which is used in the SDK to be notified about a change to its temporary skeletons. + /// @param p_SkeletonSetupIndex + /// @param p_SessionId + /// @param p_IsSkeletonModified + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_SaveTemporarySkeleton(uint32_t p_SkeletonSetupIndex, uint32_t p_SessionId, bool p_IsSkeletonModified); + + /// @brief Send a temporary skeleton to Core to compress it (convert it into an array of bytes) and then get the compressed data back in the SDK, + /// the skeleton compressed data are stored internally. It returns the length of the bytes array for correct memory allocation. + /// This function must be called before CoreSdk_GetCompressedTemporarySkeletonData. + /// @param p_SkeletonSetupIndex + /// @param p_SessionId + /// @param p_TemporarySkeletonLengthInBytes + /// @return + CORESDK_API SDKReturnCode CoreSdk_CompressTemporarySkeletonAndGetSize(uint32_t p_SkeletonSetupIndex, uint32_t p_SessionId, uint32_t* p_TemporarySkeletonLengthInBytes); + + /// @brief Get the compressed temporary skeleton data which are stored internally when calling function CoreSdk_CompressTemporarySkeletonAndGetSize. + /// The size of p_TemporarySkeletonData must match with p_TemporarySkeletonLengthInBytes. + /// @param p_TemporarySkeletonData + /// @param p_TemporarySkeletonLengthInBytes + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_GetCompressedTemporarySkeletonData(unsigned char* p_TemporarySkeletonData, uint32_t p_TemporarySkeletonLengthInBytes); + + /// @brief Get a SkeletonSetup at a given index and sessionId from Manus Core. + /// This function does NOT return a loaded skeleton! + /// @param p_SkeletonSetupIndex + /// @param p_SessionId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + CORESDK_API SDKReturnCode CoreSdk_GetTemporarySkeleton(uint32_t p_SkeletonSetupIndex, uint32_t p_SessionId); + + /// @brief Send the compressed data (an array of bytes) retrieved from a file to Core to be decompressed and converted back into a temporary skeleton. + /// The received temporary skeleton is saved internally with the given index and to the given session id. + /// The size of p_TemporarySkeletonData must match with p_TemporarySkeletonLengthInBytes. + /// @param p_SkeletonSetupIndex + /// @param p_SessionId + /// @param p_TemporarySkeletonData + /// @param p_TemporarySkeletonLengthInBytes + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + CORESDK_API SDKReturnCode CoreSdk_GetTemporarySkeletonFromCompressedData(uint32_t p_SkeletonSetupIndex, uint32_t p_SessionId, unsigned char* p_TemporarySkeletonData, uint32_t p_TemporarySkeletonLengthInBytes); + + /// @brief Clear a SkeletonSetup at a given index and sessionId in Manus Core and the SDK. + /// The given setup index will no longer be valid after this call. + /// @param p_SkeletonSetupIndex + /// @param p_SessionId + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_InvalidID if the provided p_SkeletonSetupIndex does not correspond to any skeleton setup. + CORESDK_API SDKReturnCode CoreSdk_ClearTemporarySkeleton(uint32_t p_SkeletonSetupIndex, uint32_t p_SessionId); + + /// @brief Clear all temporary skeletons associated to the current session both in the sdk and core. + /// All skeleton setups will no longer be valid after this call. + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_NotConnected if there is no connection to core, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + CORESDK_API SDKReturnCode CoreSdk_ClearAllTemporarySkeletons(); + + /// @brief Get temporary skeleton counts for all sessions connected to Core. + /// @param p_temporarySkeletonSessionsData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_GetTemporarySkeletonCountForAllSessions(TemporarySkeletonCountForAllSessions* p_TemporarySkeletonCountForAllSessions); + + /// @brief Get information (name and index) for the SkeletonSetups of the specified session. + /// @param p_SessionId + /// @param p_temporarySkeletonSessionsData + /// @return SDKReturnCode_Success if successful, + /// SDKReturnCode_SdkNotAvailable if the Core SDK is not available, + /// SDKReturnCode_StubNullPointer if the stub has been reset but someone is trying to use it anyway. This usually happens after a shutdown of the SDK, + /// SDKReturnCode_NotConnected if there is no connection to core. + CORESDK_API SDKReturnCode CoreSdk_GetTemporarySkeletonsInfoForSession(uint32_t p_SessionId, TemporarySkeletonsInfoForSession* p_TemporarySkeletonsInfoForSession); + +#ifdef __cplusplus +} // extern "C" +#endif + +// Close the Doxygen group. +/** @} */ + +#endif // #ifndef H_MANUS_SDK diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypeInitializers.h b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypeInitializers.h new file mode 100644 index 0000000..939bfd5 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypeInitializers.h @@ -0,0 +1,397 @@ +#ifndef __MANUS_SDK_TYPE_INITIALIZERS_H__ +#define __MANUS_SDK_TYPE_INITIALIZERS_H__ + +// Set up a Doxygen group. +/** @addtogroup ManusSDKTypeInitializers + * @{ + */ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "ManusSDKTypes.h" + +#include + +#ifdef MANUS_SDK_STATIC + #define CORESDK_API +#else + #ifdef _WIN32 + // MANUS_SDK_EXPORTS defines if the functions are exported to or + // imported from a DLL. + #ifdef MANUS_SDK_EXPORTS + #define CORESDK_API __declspec(dllexport) + #else + #define CORESDK_API __declspec(dllimport) + #endif + #elif defined(__linux__) + #define CORESDK_API + #else + #error Unrecognized platform. + #endif +#endif + + ///@brief All these initializers take a preallocated instance of their related structure and set default values. + + /// @brief Initializer for a ManusVec3 struct + /// @param p_Val + CORESDK_API void ManusVec3_Init(ManusVec3* p_Val); + + /// @brief Initializer for a ManusVec2 struct + /// @param p_Val + CORESDK_API void ManusVec2_Init(ManusVec2* p_Val); + + /// @brief Initializer for a ManusQuaternion struct + /// @param p_Val + CORESDK_API void ManusQuaternion_Init(ManusQuaternion* p_Val); + + /// @brief Initializer for a ManusTransform struct + /// @param p_Val + CORESDK_API void ManusTransform_Init(ManusTransform* p_Val); + + /// @brief Initializer for a Color struct + /// @param p_Val + CORESDK_API void Color_Init(Color* p_Val); + + /// @brief Initializer for a ManusTimestampInfo struct + /// @param p_Val + CORESDK_API void ManusTimestampInfo_Init(ManusTimestampInfo* p_Val); + + /// @brief Initializer for a ManusTimestamp struct + /// @param p_Val + CORESDK_API void ManusTimestamp_Init(ManusTimestamp* p_Val); + + /// @brief Initializer for a IMUCalibrationInfo struct + /// @param p_Val + CORESDK_API void IMUCalibrationInfo_Init(IMUCalibrationInfo* p_Val); + + /// @brief Initializer for a Version struct + /// @param p_Val + CORESDK_API void Version_Init(Version* p_Val); + + /// @brief Initializer for a FirmwareVersion struct + /// @param p_Val + CORESDK_API void FirmwareVersion_Init(FirmwareVersion* p_Val); + + /// @brief Initializer for a ManusVersion struct + /// @param p_Val + CORESDK_API void ManusVersion_Init(ManusVersion* p_Val); + + /// @brief Initializer for a TrackerId struct + /// @param p_Val + CORESDK_API void TrackerId_Init(TrackerId* p_Val); + + /// @brief Initializer for a TrackerData struct + /// @param p_Val + CORESDK_API void TrackerData_Init(TrackerData* p_Val); + + /// @brief Initializer for a ManusHost struct + /// @param p_Val + CORESDK_API void ManusHost_Init(ManusHost* p_Val); + + /// @brief Initializer for a SkeletonNode struct + /// @param p_Val + CORESDK_API void SkeletonNode_Init(SkeletonNode* p_Val); + + /// @brief Initializer for a SkeletonInfo struct + /// @param p_Val + CORESDK_API void SkeletonInfo_Init(SkeletonInfo* p_Val); + + /// @brief Initializer for a RawSkeletonInfo struct + /// @param p_Val + CORESDK_API void RawSkeletonInfo_Init(RawSkeletonInfo* p_Val); + + /// @brief Initializer for a SkeletonStreamInfo struct + /// @param p_Val + CORESDK_API void SkeletonStreamInfo_Init(SkeletonStreamInfo* p_Val); + + /// @brief Initializer for a ErgonomicsData struct + /// @param p_Val + CORESDK_API void ErgonomicsData_Init(ErgonomicsData* p_Val); + + /// @brief Initializer for a ErgonomicsStream struct + /// @param p_Val + CORESDK_API void ErgonomicsStream_Init(ErgonomicsStream* p_Val); + + /// @brief Initializer for a DongleLandscapeData struct + /// @param p_Val + CORESDK_API void DongleLandscapeData_Init(DongleLandscapeData* p_Val); + + /// @brief Initializer for a GloveLandscapeData struct + /// @param p_Val + CORESDK_API void GloveLandscapeData_Init(GloveLandscapeData* p_Val); + + /// @brief Initializer for a Measurement struct + /// @param p_Val + CORESDK_API void Measurement_Init(Measurement* p_Val); + + /// @brief Initializer for a TrackerOffset struct + /// @param p_Val + CORESDK_API void TrackerOffset_Init(TrackerOffset* p_Val); + + /// @brief Initializer for a ExtraTrackerOffset struct + /// @param p_Val + CORESDK_API void ExtraTrackerOffset_Init(ExtraTrackerOffset* p_Val); + + /// @brief Initializer for a TrackerLandscapeData struct + /// @param p_Val + CORESDK_API void TrackerLandscapeData_Init(TrackerLandscapeData* p_Val); + + /// @brief Initializer for a UserProfileLandscapeData struct + /// @param p_Val + CORESDK_API void UserProfileLandscapeData_Init(UserProfileLandscapeData* p_Val); + + /// @brief Initializer for a UserLandscapeData struct + /// @param p_Val + CORESDK_API void UserLandscapeData_Init(UserLandscapeData* p_Val); + + /// @brief Initializer for a SkeletonLandscapeData struct + /// @param p_Val + CORESDK_API void SkeletonLandscapeData_Init(SkeletonLandscapeData* p_Val); + + /// @brief Initializer for a DeviceLandscape struct + /// @param p_Val + CORESDK_API void DeviceLandscape_Init(DeviceLandscape* p_Val); + + /// @brief Initializer for a UserLandscape struct + /// @param p_Val + CORESDK_API void UserLandscape_Init(UserLandscape* p_Val); + + /// @brief Initializer for a SkeletonLandscape struct + /// @param p_Val + CORESDK_API void SkeletonLandscape_Init(SkeletonLandscape* p_Val); + + /// @brief Initializer for a TrackerLandscape struct + /// @param p_Val + CORESDK_API void TrackerLandscape_Init(TrackerLandscape* p_Val); + + /// @brief Initializer for a LicenseInfo struct + /// @param p_Val + CORESDK_API void LicenseInfo_Init(LicenseInfo* p_Val); + + /// @brief Initializer for a SettingsLandscape struct + /// @param p_Val + CORESDK_API void SettingsLandscape_Init(SettingsLandscape* p_Val); + + /// @brief Initializer for a TimecodeInterface struct + /// @param p_Val + CORESDK_API void TimecodeInterface_Init(TimecodeInterface* p_Val); + + /// @brief Initializer for a TimeLandscape struct + /// @param p_Val + CORESDK_API void TimeLandscape_Init(TimeLandscape* p_Val); + + /// @brief Initializer for a GestureLandscapeData struct + /// @param p_Val + CORESDK_API void GestureLandscapeData_Init(GestureLandscapeData* p_Val); + + /// @brief Initializer for a NetDeviceLandscapeData struct + /// @param p_Val + CORESDK_API void NetDeviceLandscapeData_Init(NetDeviceLandscapeData* p_Val); + + /// @brief Initializer for a NetDevicesLandscape struct + /// @param p_Val + CORESDK_API void NetDevicesLandscape_Init(NetDevicesLandscape* p_Val); + + /// @brief Initializer for a Landscape struct + /// @param p_Val + CORESDK_API void Landscape_Init(Landscape* p_Val); + + /// @brief Initializer for a SkeletonSetupArraySizes struct + /// @param p_Val + CORESDK_API void SkeletonSetupArraySizes_Init(SkeletonSetupArraySizes* p_Val); + + /// @brief Initializer for a NodeSettingsIK struct + /// @param p_Val + CORESDK_API void NodeSettingsIK_Init(NodeSettingsIK* p_Val); + + /// @brief Initializer for a NodeSettingsFoot struct + /// @param p_Val + CORESDK_API void NodeSettingsFoot_Init(NodeSettingsFoot* p_Val); + + /// @brief Initializer for a NodeSettingsRotationOffset struct + /// @param p_Val + CORESDK_API void NodeSettingsRotationOffset_Init(NodeSettingsRotationOffset* p_Val); + + /// @brief Initializer for a NodeSettingsLeaf struct + /// @param p_Val + CORESDK_API void NodeSettingsLeaf_Init(NodeSettingsLeaf* p_Val); + + /// @brief Initializer for a NodeSettings struct + /// @param p_Val + CORESDK_API void NodeSettings_Init(NodeSettings* p_Val); + + /// @brief Initializer for a NodeSetup struct + /// @param p_Val + CORESDK_API void NodeSetup_Init(NodeSetup* p_Val); + + /// @brief Initializer for a ChainSettingsPelvis struct + /// @param p_Val + CORESDK_API void ChainSettingsPelvis_Init(ChainSettingsPelvis* p_Val); + + /// @brief Initializer for a ChainSettingsLeg struct + /// @param p_Val + CORESDK_API void ChainSettingsLeg_Init(ChainSettingsLeg* p_Val); + + /// @brief Initializer for a ChainSettingsSpine struct + /// @param p_Val + CORESDK_API void ChainSettingsSpine_Init(ChainSettingsSpine* p_Val); + + /// @brief Initializer for a ChainSettingsNeck struct + /// @param p_Val + CORESDK_API void ChainSettingsNeck_Init(ChainSettingsNeck* p_Val); + + /// @brief Initializer for a ChainSettingsHead struct + /// @param p_Val + CORESDK_API void ChainSettingsHead_Init(ChainSettingsHead* p_Val); + + /// @brief Initializer for a ChainSettingsArm struct + /// @param p_Val + CORESDK_API void ChainSettingsArm_Init(ChainSettingsArm* p_Val); + + /// @brief Initializer for a ChainSettingsShoulder struct + /// @param p_Val + CORESDK_API void ChainSettingsShoulder_Init(ChainSettingsShoulder* p_Val); + + /// @brief Initializer for a ChainSettingsFinger struct + /// @param p_Val + CORESDK_API void ChainSettingsFinger_Init(ChainSettingsFinger* p_Val); + + /// @brief Initializer for a ChainSettingsHand struct + /// @param p_Val + CORESDK_API void ChainSettingsHand_Init(ChainSettingsHand* p_Val); + + /// @brief Initializer for a ChainSettingsFoot struct + /// @param p_Val + CORESDK_API void ChainSettingsFoot_Init(ChainSettingsFoot* p_Val); + + /// @brief Initializer for a ChainSettingsToe struct + /// @param p_Val + CORESDK_API void ChainSettingsToe_Init(ChainSettingsToe* p_Val); + + /// @brief Initializer for a ChainSettings struct + /// @param p_Val + CORESDK_API void ChainSettings_Init(ChainSettings* p_Val); + + /// @brief Initializer for a ChainSetup struct + /// @param p_Val + CORESDK_API void ChainSetup_Init(ChainSetup* p_Val); + + /// @brief Initializer for a SphereColliderSetup struct + /// @param p_Val + CORESDK_API void SphereColliderSetup_Init(SphereColliderSetup* p_Val); + + /// @brief Initializer for a CapsuleColliderSetup struct + /// @param p_Val + CORESDK_API void CapsuleColliderSetup_Init(CapsuleColliderSetup* p_Val); + + /// @brief Initializer for a BoxColliderSetup struct + /// @param p_Val + CORESDK_API void BoxColliderSetup_Init(BoxColliderSetup* p_Val); + + /// @brief Initializer for a ColliderSetup struct + /// @param p_Val + CORESDK_API void ColliderSetup_Init(ColliderSetup* p_Val); + + /// @brief Initializer for a Vertex Weight struct + /// @param p_Val + CORESDK_API void Weight_Init(Weight* p_Val); + + /// @brief Initializer for a Vertex struct + /// @param p_Val + CORESDK_API void Vertex_Init(Vertex* p_Val); + + /// @brief Initializer for a Triangle struct + /// @param p_Val + CORESDK_API void Triangle_Init(Triangle* p_Val); + + /// @brief Initializer for a SkeletonTargetUserData struct + /// @param p_Val + CORESDK_API void SkeletonTargetUserData_Init(SkeletonTargetUserData* p_Val); + + /// @brief Initializer for a SkeletonTargetAnimationData struct + /// @param p_Val + CORESDK_API void SkeletonTargetAnimationData_Init(SkeletonTargetAnimationData* p_Val); + + /// @brief Initializer for a SkeletonTargetGloveData struct + /// @param p_Val + CORESDK_API void SkeletonTargetGloveData_Init(SkeletonTargetGloveData* p_Val); + + /// @brief Initializer for a SkeletonSettings struct + /// @param p_Val + CORESDK_API void SkeletonSettings_Init(SkeletonSettings* p_Val); + + /// @brief Initializer for a SkeletonSetupInfo struct + /// @param p_Val + CORESDK_API void SkeletonSetupInfo_Init(SkeletonSetupInfo* p_Val); + + /// @brief Initializer for a TemporarySkeletonInfo struct + /// @param p_Val + CORESDK_API void TemporarySkeletonInfo_Init(TemporarySkeletonInfo* p_Val); + + /// @brief Initializer for a TemporarySkeletonsForSession struct + /// @param p_Val + CORESDK_API void TemporarySkeletonsInfoForSession_Init(TemporarySkeletonsInfoForSession* p_Val); + + /// @brief Initializer for a TemporarySkeletonCountForSession struct + /// @param p_Val + CORESDK_API void TemporarySkeletonCountForSession_Init(TemporarySkeletonCountForSession* p_Val); + + /// @brief Initializer for a TemporarySkeletonCountForAllSessions struct + /// @param p_Val + CORESDK_API void TemporarySkeletonCountForAllSessions_Init(TemporarySkeletonCountForAllSessions* p_Val); + + /// @brief Initializer for a TemporarySkeletonSessionsData struct + /// @param p_Val + CORESDK_API void TemporarySkeletonSessionsData_Init(TemporarySkeletonSessionsData* p_Val); + + /// @brief Initializer for a SystemMessage struct + /// @param p_Val + CORESDK_API void SystemMessage_Init(SystemMessage* p_Val); + + /// @brief Initializer for a CoordinateSystemVUH struct + /// @param p_Val + CORESDK_API void CoordinateSystemVUH_Init(CoordinateSystemVUH* p_Val); + + /// @brief Initializer for a CoordinateSystemDirection struct + /// @param p_Val + CORESDK_API void CoordinateSystemDirection_Init(CoordinateSystemDirection* p_Val); + + /// @brief Initializer for a NodeInfo struct + /// @param p_Val + CORESDK_API void NodeInfo_Init(NodeInfo* p_Val); + + /// @brief Initializer for a GestureStreamInfo struct + /// @param p_Val + CORESDK_API void GestureStreamInfo_Init(GestureStreamInfo* p_Val); + + /// @brief Initializer for a GestureProbabilities struct + /// @param p_Val + CORESDK_API void GestureProbabilities_Init(GestureProbabilities* p_Val); + + /// @brief Initializer for a GestureProbability struct + /// @param p_Val + CORESDK_API void GestureProbability_Init(GestureProbability* p_Val); + + /// @brief Initializer for a GloveCalibrationArgs struct + /// @param p_Val + CORESDK_API void GloveCalibrationArgs_Init(GloveCalibrationArgs* p_Val); + + /// @brief Initializer for a GloveCalibrationStepArgs struct + /// @param p_Val + CORESDK_API void GloveCalibrationStepArgs_Init(GloveCalibrationStepArgs* p_Val); + + /// @brief Initializer for a GloveCalibrationStepData struct + /// @param p_Val + CORESDK_API void GloveCalibrationStepData_Init(GloveCalibrationStepData* p_Val); + +#ifdef __cplusplus +} // extern "C" +#endif + +// Close the Doxygen group. +/** @} */ + +#endif // #ifndef MANUS_SDK_TYPE_INITIALIZERS_H diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypes.h b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypes.h new file mode 100644 index 0000000..7c45344 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/include/ManusSDKTypes.h @@ -0,0 +1,1717 @@ +#ifndef __MANUS_SDK_TYPES_H__ +#define __MANUS_SDK_TYPES_H__ + +// Set up a Doxygen group. +/** @addtogroup ManusSdkTypes + * @{ + */ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/****************************************************************************** + * Preprocessor defines. + *****************************************************************************/ + +/// @brief Used to descriptively refer to the number of fingers on a hand. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define NUM_FINGERS_ON_HAND 5 + +/// @brief Used to descriptively refer to the number of flex sensor segments. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define NUM_FLEX_SEGMENTS_PER_FINGER 2 + +/// @brief Used to descriptively refer to the maximum IMUs count on a glove. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_IMUS_ON_GLOVE (NUM_FINGERS_ON_HAND + 1) + +/// @brief Used to descriptively refer to the maximum number of Polygon users. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_USERS 16 // we should never reach that + +/// @brief Used to descriptively refer to the maximum user name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_USERNAME 64 + +/// @brief Used to descriptively refer to the maximum body measurement name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_MEASUREMENT 64 + +/// Host name length is based on a post made here: +/// https://community.cisco.com/t5/other-network-architecture/maximum-length-hostname/td-p/529327 +/// Which in turn was based on: https://www.ietf.org/rfc/rfc1035.txt +/// @brief Used to descriptively refer to the maximum host name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_HOST_NAME 256 + +/// @brief Used to descriptively refer to the maximum IP address length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +/// It is based on the length of an IPv6 address. Example: +/// "2001:0db8:0000:0000:0000:8a2e:0370:7334". +#define MAX_NUM_CHARS_IN_IP_ADDRESS 40 + +/// @brief Used to descriptively refer to the maximum tracker name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_TRACKER_ID 32 + +/// @brief Used to descriptively refer to the maximum tracker manufacturer length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_TRACKER_MANUFACTURER 32 + +/// @brief Used to descriptively refer to the maximum tracker manufacturer length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_TRACKER_PRODUCTNAME 32 + +/// @brief Used to descriptively refer to the maximum target name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_TARGET_ID 32 + +/// @brief Used to descriptively refer to the maximum version string length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_VERSION 16 + +/// @brief Used to descriptively refer to the value given to glove and dongle IDs when they are uninitialized. +#define UNINITIALIZED_ID 0 + +/// @brief Used to descriptively refer to the maximum number of hosts that we support when finding manus core. +#define MAX_NUMBER_OF_HOSTS 100 + +/// @brief Used to descriptively refer to the max number of supported dongles. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_DONGLES 16 // we only have 35 radio channels available. if we got 16 dongles, expect wireless issues! and no 16 usb ports is also not likely. + +/// @brief Used to descriptively refer to the maximum license type string length. +#define MAX_NUM_CHARS_IN_LICENSE_TYPE 64 + +/// @brief Used to descriptively refer to the maximum calibration step title length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_CALIBRATION_TITLE 64 + +/// @brief Used to descriptively refer to the maximum calibration step description length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_CALIBRATION_DESCRIPTION 256 + +#define MAX_NUM_CHARS_IN_CALIBRATION_FILE 65536 + +/// @brief Constants for the maximum number of devices considered plausible. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_GLOVES (MAX_NUMBER_OF_DONGLES* 2) +#define MAX_NUMBER_OF_HAPTICS_DONGLES MAX_NUMBER_OF_DONGLES + +/// @brief Constants for the maximum number of skeletons. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_SKELETONS 32 + +/// @brief Constants for the maximum number of users. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_USERS MAX_USERS + +/// @brief Constants for the maximum number of trackers for each polygon skeleton. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define NUMBER_OF_TRACKERS_PER_POLYGON_SKELETON 8 + +/// @brief Constants for the maximum number of trackers. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_TRACKERS (MAX_NUMBER_OF_USERS * NUMBER_OF_TRACKERS_PER_POLYGON_SKELETON) + +/// @brief Used to descriptively refer to the maximum node name length. +/// Used with arrays to make them more descriptive than simply using the +/// number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_NODE_NAME 256 // this is for a UTF8 string , NOT an ASCII CHAR array (same base type though) + + +/// @brief Used to descriptively refer to the max number of chain nodes. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_CHAIN_LENGTH 32 + +/// @brief Used to descriptively refer to the max number of fingers we support per hand in chains (not the same as a glove, as we can retarget alien hands with more or less fingers). +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_FINGER_IDS 10 + +/// @brief Used to descriptively refer to the max number of toes we support per foot in chains ( we can retarget alien feet with more or less toes). +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_TOE_IDS 10 + +/// @brief Used to descriptively refer to the max length of a system error message string. +/// Used with arrays to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_SYSTEM_ERROR_MESSAGE 256 + +/// @brief Used to descriptively refer to the max length of a debugging id string. +/// Used with arrays to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_CHAR_DEBUGGING_ID 64 + +/// @brief Used to descriptively refer to the max number of ergonomics data. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_ERGONOMICS_DATA MAX_NUMBER_OF_GLOVES + +/// @brief Used to descriptively refer to the max number of sessions connected to Core. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_SESSIONS 8 // this is not the real limit for Core but just for the SDKClient to handle + +/// @brief Used to descriptively refer to the max number of skeletons for each session. +/// Used with arrays and loops to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUMBER_OF_SKELETONS_PER_SESSION 32 + +/// @brief Used to descriptively refer to the max length of a skeleton name string. +/// Used with arrays to make them more descriptive than simply using +/// the number, and to make changing the number easier and safer. +#define MAX_NUM_CHARS_IN_SKELETON_NAME 256 // we already encountered 34 char names in unreal, but its utf8 so enbiggen even more! + +/// @brief Used to descriptively refer to the max length of a timecode interface string. +#define MAX_NUM_CHARS_IN_TIMECODE_INTERFACE_STRINGS 64 + +/// @brief Used to descriptively refer to the max amount of timecode interfaces. +#define MAX_NUMBER_OF_AUDIO_INTERFACES 32 + +/// @brief Used to descriptively refer to the maximum number of bone weights per vertex. +#define MAX_BONE_WEIGHTS_PER_VERTEX 4 + +/// @brief Used to descriptively refer to the maximum number of nodes per each estimation skeleton +#define MAX_NUMBER_OF_NODES_PER_ESTIMATION_SKELETON 40 + +/// @brief Used to define the max amount of gestures in one chunk +#define MAX_GESTURE_DATA_CHUNK_SIZE 64 + +#define MAX_NUM_CHARS_IN_OPENXR_FEATURE_STRING 64 + +/// @brief Used to define the max amount of net devices +#define MAX_NUMBER_OF_NETDEVICES MAX_NUMBER_OF_DONGLES + +/// @brief Matchmaker requirements. +#define BROADCAST_ADDRESS "255.255.255.255" +#define BROADCAST_PORT "62687" +#define SECONDS_TO_FIND_HOSTS 2 +#define SECONDS_TO_FIND_LOCAL_HOSTS 2 +#define DEFAULT_BUFFER_SIZE 512 + +/****************************************************************************** + * Enums. + *****************************************************************************/ + +/// @brief The return values that can be given by SDK wrapper functions. +typedef enum SDKReturnCode +{ + /// No issues occurred. + SDKReturnCode_Success, + + /// Something went wrong, but no specific reason can be given. + SDKReturnCode_Error, + + /// One of the arguments given had an invalid value. + SDKReturnCode_InvalidArgument, + + /// The size of an argument given (e.g. an array) does not match the size + /// of the data that it is intended to hold. + SDKReturnCode_ArgumentSizeMismatch, + + /// A string of an unsupported size was encountered. + SDKReturnCode_UnsupportedStringSizeEncountered, + + /// The Core SDK is not available. + SDKReturnCode_SdkNotAvailable, + + /// The network host finder is not available. + SDKReturnCode_HostFinderNotAvailable, + + /// The data requested is not available. + SDKReturnCode_DataNotAvailable, + + /// Failed to allocate memory for something. + SDKReturnCode_MemoryError, + + /// Something went wrong in the SDK internally. + SDKReturnCode_InternalError, + + /// The function was not intended to be called at this time. + SDKReturnCode_FunctionCalledAtWrongTime, + + /// No connection to Core was made. + SDKReturnCode_NotConnected, + + /// The connection with Core timed out. + SDKReturnCode_ConnectionTimeout, + + /// using an uninitialized ID is bad. + SDKReturnCode_InvalidID, + + /// memory unallocated or just a null pointer passed where it wasn't supposed to be! + SDKReturnCode_NullPointer, + + /// null sequence type for polygon calibration + SDKReturnCode_InvalidSequence, + + /// don't forget to set the coordinate system type or there will be trouble + SDKReturnCode_NoCoordinateSystemSet, + + /// if everything is being terminated. don't restart + SDKReturnCode_SdkIsTerminating, + + /// the stub has been reset but someone is tryign to use it anyway. usually after a shutdown of the SDK. + SDKReturnCode_StubNullPointer, + + /// Skeleton could not be loaded. usually when using more then the max skeletons per session (32). + SDKReturnCode_SkeletonNotLoaded, + + /// Function not available for this version of the SDK + SDKReturnCode_FunctionNotAvailable, + + SDKReturnCode_MAX_SIZE +} SDKReturnCode; + +/// @brief Used to tell what severity the log is +typedef enum LogSeverity +{ + LogSeverity_Debug, + LogSeverity_Info, + LogSeverity_Warn, + LogSeverity_Error +} LogSeverity; + + +/// @brief Used to tell what client is using the wrapper. +/// This makes the session easier to identify in the landscape. +typedef enum SessionType +{ + SessionType_Unknown, + SessionType_UnityPlugin, + SessionType_UnrealPlugin, + SessionType_CoreSDK, + SessionType_Xsens, + SessionType_Optitrack, + SessionType_MotionBuilder, + SessionType_VRED, + SessionType_OpenXR, + SessionType_Qualisys, + SessionType_Vicon, + SessionType_Nokov, + SessionType_IcIdo, + SessionType_Siemens +} SessionType; + +/// @brief Describes the different types of trackers that can be used. +typedef enum TrackerType +{ + TrackerType_Unknown, + TrackerType_Head, + TrackerType_Waist, + TrackerType_LeftHand, + TrackerType_RightHand, + TrackerType_LeftFoot, + TrackerType_RightFoot, + TrackerType_LeftUpperArm, + TrackerType_RightUpperArm, + TrackerType_LeftUpperLeg, + TrackerType_RightUpperLeg, + TrackerType_Controller, + TrackerType_Camera, + + TrackerType_MAX_SIZE +} TrackerType; + +/// @brief Describes the tracking quality. +typedef enum TrackingQuality +{ + TrackingQuality_Untrackable, + TrackingQuality_BadTracking, + TrackingQuality_Trackable, +} TrackerQuality; + +/// @brief Describes the different types of tracker systems. +typedef enum TrackerSystemType +{ + TrackerSystemType_Unknown, + TrackerSystemType_Antilatency, + TrackerSystemType_ART, + TrackerSystemType_OpenVR, + TrackerSystemType_Optitrack, + TrackerSystemType_Vicon, + TrackerSystemType_OpenXR, +} TrackerSystemType; + +/// @brief Describes the paired state of the device. +typedef enum DevicePairedState +{ + DevicePairedState_Unknown, + DevicePairedState_Paired, + DevicePairedState_Unpaired, + DevicePairedState_Pairing, +} DevicePairedState; + +/// @brief Describes the different types of device classes. +typedef enum DeviceClassType +{ + DeviceClassType_Unknown, + DeviceClassType_Dongle, + DeviceClassType_Glove, + DeviceClassType_Glongle +} DeviceClassType; + +/// @brief Describes the different types of Manus devices. +typedef enum DeviceFamilyType +{ + DeviceFamilyType_Unknown, + DeviceFamilyType_Prime1, + DeviceFamilyType_Prime2, + DeviceFamilyType_PrimeX, // TODO obsolete? + DeviceFamilyType_Metaglove, + DeviceFamilyType_Prime3, + DeviceFamilyType_Virtual, + DeviceFamilyType_MetaglovePro, +} DeviceFamilyType; + +/// @brief Describes the different types of profile used during the calibration. +typedef enum ProfileType +{ + ProfileType_Hands, + ProfileType_FullBody, + + ProfileType_MAX_SIZE +} ProfileType; + +/// @brief The different types of body measurements used for the polygon calibration. +typedef enum MeasurementType +{ + MeasurementType_Unknown, + MeasurementType_PlayerHeight, + MeasurementType_SpineLength, + MeasurementType_NeckLength, + MeasurementType_UpperArmLength, + MeasurementType_LowerArmLength, + MeasurementType_ArmLength, + MeasurementType_ArmSpan, + MeasurementType_UpperLegLength, + MeasurementType_LowerLegLength, + MeasurementType_LegLength, + MeasurementType_HandLength, + MeasurementType_FootLength, + MeasurementType_HipWidth, + MeasurementType_ShoulderWidth, + MeasurementType_ShoulderHeight, + MeasurementType_HeadLength, + MeasurementType_Thickness, + MeasurementType_ArmRatio, + MeasurementType_LegRatio, + + MeasurementType_MAX_SIZE // Warning, this value is used to define the UserProfile.Measurement[SIZE] +} MeasurementType; + +/// @brief Describes the different types of tracker offsets. +typedef enum TrackerOffsetType +{ + TrackerOffsetType_Unknown, + TrackerOffsetType_HeadTrackerToHead, + TrackerOffsetType_HeadTrackerToTopOfHead, + + TrackerOffsetType_LeftHandTrackerToWrist, + TrackerOffsetType_RightHandTrackerToWrist, + TrackerOffsetType_LeftFootTrackerToAnkle, + TrackerOffsetType_RightFootTrackerToAnkle, + + TrackerOffsetType_HipTrackerToHip, + TrackerOffsetType_HipTrackerToLeftLeg, + TrackerOffsetType_HipTrackerToRightLeg, + + TrackerOffsetType_LeftUpperArmTrackerToElbow, + TrackerOffsetType_RightUpperArmTrackerToElbow, + TrackerOffsetType_LeftUpperArmTrackerToShoulder, + TrackerOffsetType_RightUpperArmTrackerToShoulder, + + TrackerOffsetType_MAX_SIZE // Warning, this value is used to define the UserProfile.TrackerOffset[SIZE] +} TrackerOffsetType; + +/// @brief Describes the different types of extra tracker offsets. +typedef enum ExtraTrackerOffsetType +{ + ExtraTrackerOffsetType_Unknown, + ExtraTrackerOffsetType_HeadForward, + ExtraTrackerOffsetType_HipForward, + ExtraTrackerOffsetType_HipHeight, + ExtraTrackerOffsetType_MAX_SIZE // Warning, this value is used to define the UserProfile.TrackerOffset[SIZE] +} ExtraTrackerOffsetType; + +/// @brief Describes the different types of body measurement units. +typedef enum MeasurementUnit +{ + MeasurementUnit_Meters, + MeasurementUnit_Percentage, +} MeasurementUnit; + +/// @brief Describes the different types of body measurement categories used for the polygon calibration. +typedef enum MeasurementCategory +{ + MeasurementCategory_Misc, + MeasurementCategory_Generic, + MeasurementCategory_Arms, + MeasurementCategory_Legs, + MeasurementCategory_Body, +} MeasurementCategory; + +/// @brief Describes the different possibilities for the update status. +typedef enum UpdateStatusEnum +{ + UpdateStatusEnum_Unknown, + UpdateStatusEnum_NoUpdateAvailable, + UpdateStatusEnum_UpdateAvailable, + UpdateStatusEnum_MandatoryUpdateAvailable, + UpdateStatusEnum_Updating, +} UpdateStatusEnum; + +/// @brief Describes the different skeleton types. +typedef enum SkeletonType +{ + SkeletonType_Invalid, + SkeletonType_Hand, + SkeletonType_Body, + SkeletonType_Both +} SkeletonType; + +/// @brief Describes the possible data that can be used for the skeleton animation. +typedef enum SkeletonTargetType +{ + SkeletonTargetType_Invalid, + SkeletonTargetType_UserData, + SkeletonTargetType_UserIndexData, + SkeletonTargetType_AnimationData, + SkeletonTargetType_GloveData +} SkeletonTargetType; + +/// @brief Describes the possible nodes types used when setting up the skeleton. +typedef enum NodeType +{ + NodeType_Invalid, + NodeType_Joint, + NodeType_Mesh, + NodeType_Leaf, + NodeType_Collider +} NodeType; + +/// @brief Describes the settings that can be applied to a node, it is defined as a flag so that more than one setting can be set. +typedef enum NodeSettingsFlag +{ + NodeSettingsFlag_None = 0, + NodeSettingsFlag_IK = 1 << 0, + NodeSettingsFlag_Foot = 1 << 1, + NodeSettingsFlag_RotationOffset = 1 << 2, + NodeSettingsFlag_Leaf = 1 << 3, +} NodeSettingsFlag; + +/// @brief Describes the possible chain types used when setting up the skeleton. +typedef enum ChainType +{ + ChainType_Invalid, + ChainType_Arm, + ChainType_Leg, + ChainType_Neck, + ChainType_Spine, + ChainType_FingerThumb, + ChainType_FingerIndex, + ChainType_FingerMiddle, + ChainType_FingerRing, + ChainType_FingerPinky, + ChainType_Pelvis, + ChainType_Head, + ChainType_Shoulder, + ChainType_Hand, + ChainType_Foot, + ChainType_Toe +} ChainType; + +/// @brief Describes the possible collider types. +typedef enum CollisionType +{ + CollisionType_None, + CollisionType_Discrete, + CollisionType_Continuous, +} CollisionType; + +/// @brief Describes the possible collider types. +typedef enum ColliderType +{ + ColliderType_Invalid, + ColliderType_Sphere, + ColliderType_Capsule, + ColliderType_Box, +} ColliderType; + +/// @brief Describes the possible chain side. +typedef enum Side +{ + Side_Invalid, + Side_Left, + Side_Right, + Side_Center +} Side; + +/// @brief Describes which sensor data the hand motion is based on. +typedef enum HandMotion +{ + HandMotion_None, + HandMotion_IMU, + HandMotion_Tracker, + HandMotion_Tracker_RotationOnly, + HandMotion_Auto +} HandMotion; + +/// @brief Describes the direction of the coordinate system axis in 3d space. +typedef enum AxisDirection +{ + AxisDirection_Invalid, + AxisDirection_Backward, + AxisDirection_Left, + AxisDirection_Down, + AxisDirection_Up, + AxisDirection_Right, + AxisDirection_Forward +} AxisDirection; + +/// @brief Describes the view of the coordinate system axis. +/// Consider yourself sitting in front of your computer screen. +/// From Viewer means it goes into the screen, so away from you. +/// To Viewer means the axis goes from the screen towards you. +typedef enum AxisView +{ + AxisView_Invalid, + + AxisView_ZFromViewer, + AxisView_YFromViewer, + AxisView_XFromViewer, + + AxisView_XToViewer, + AxisView_YToViewer, + AxisView_ZToViewer +} AxisView; + +/// @brief Describes the polarity of the coordinate system axis. +typedef enum AxisPolarity +{ + AxisPolarity_Invalid, + + AxisPolarity_NegativeZ, + AxisPolarity_NegativeY, + AxisPolarity_NegativeX, + + AxisPolarity_PositiveX, + AxisPolarity_PositiveY, + AxisPolarity_PositiveZ +} AxisPolarity; + +/// @brief Describes the possible types for system messages received from core. +typedef enum SystemMessageType +{ + SystemMessageType_Unknown, + SystemMessageType_LibDebugReplugDongle, + SystemMessageType_LibDebugRxStall, + SystemMessageType_LibDebugTxStall, + + SystemMessageType_TrackerError, + SystemMessageType_TrackerOk, + SystemMessageType_TrackerSystemOutOfDate, + + SystemMessageType_GloveSanityErrorPSOCInit, + SystemMessageType_GloveSanityErrorQCBatV, + SystemMessageType_GloveSanityErrorQCLRACalib, + SystemMessageType_GloveSanityErrorQCFlexInit, + SystemMessageType_GloveSanityErrorQCIMUInit, + SystemMessageType_GloveSanityErrorQCIMUCalib, + SystemMessageType_GloveSanityErrorQCID, + SystemMessageType_GloveSanityErrorQCInterCPU, + + SystemMessageType_SessionConnectionVersionMismatch, + + SystemMessageType_TemporarySkeletonModified, + + SystemMessageType_SessionRefusedDueToLicenseIssue, + + SystemMessageType_LaunchDevTools +} SystemMessageType; + +/// @brief Describes the possible types for the ergonomics data. +typedef enum ErgonomicsDataType +{ +// ErgonomicsDataType_Invalid, + + ErgonomicsDataType_LeftFingerThumbMCPSpread, + ErgonomicsDataType_LeftFingerThumbMCPStretch, + ErgonomicsDataType_LeftFingerThumbPIPStretch, + ErgonomicsDataType_LeftFingerThumbDIPStretch, + + ErgonomicsDataType_LeftFingerIndexMCPSpread, + ErgonomicsDataType_LeftFingerIndexMCPStretch, + ErgonomicsDataType_LeftFingerIndexPIPStretch, + ErgonomicsDataType_LeftFingerIndexDIPStretch, + + ErgonomicsDataType_LeftFingerMiddleMCPSpread, + ErgonomicsDataType_LeftFingerMiddleMCPStretch, + ErgonomicsDataType_LeftFingerMiddlePIPStretch, + ErgonomicsDataType_LeftFingerMiddleDIPStretch, + + ErgonomicsDataType_LeftFingerRingMCPSpread, + ErgonomicsDataType_LeftFingerRingMCPStretch, + ErgonomicsDataType_LeftFingerRingPIPStretch, + ErgonomicsDataType_LeftFingerRingDIPStretch, + + ErgonomicsDataType_LeftFingerPinkyMCPSpread, + ErgonomicsDataType_LeftFingerPinkyMCPStretch, + ErgonomicsDataType_LeftFingerPinkyPIPStretch, + ErgonomicsDataType_LeftFingerPinkyDIPStretch, + + + ErgonomicsDataType_RightFingerThumbMCPSpread, + ErgonomicsDataType_RightFingerThumbMCPStretch, + ErgonomicsDataType_RightFingerThumbPIPStretch, + ErgonomicsDataType_RightFingerThumbDIPStretch, + + ErgonomicsDataType_RightFingerIndexMCPSpread, + ErgonomicsDataType_RightFingerIndexMCPStretch, + ErgonomicsDataType_RightFingerIndexPIPStretch, + ErgonomicsDataType_RightFingerIndexDIPStretch, + + ErgonomicsDataType_RightFingerMiddleMCPSpread, + ErgonomicsDataType_RightFingerMiddleMCPStretch, + ErgonomicsDataType_RightFingerMiddlePIPStretch, + ErgonomicsDataType_RightFingerMiddleDIPStretch, + + ErgonomicsDataType_RightFingerRingMCPSpread, + ErgonomicsDataType_RightFingerRingMCPStretch, + ErgonomicsDataType_RightFingerRingPIPStretch, + ErgonomicsDataType_RightFingerRingDIPStretch, + + ErgonomicsDataType_RightFingerPinkyMCPSpread, + ErgonomicsDataType_RightFingerPinkyMCPStretch, + ErgonomicsDataType_RightFingerPinkyPIPStretch, + ErgonomicsDataType_RightFingerPinkyDIPStretch, + + ErgonomicsDataType_MAX_SIZE +} ErgonomicsDataType; + +/// @brief Describes the possible Manus license types. +typedef enum LicenseType +{ + LicenseType_Undefined, + LicenseType_Polygon, + LicenseType_CoreXO, + LicenseType_CorePro, + LicenseType_CoreXOPro, + + LicenseType_CoreX, + LicenseType_CoreO, + LicenseType_CoreQ, + LicenseType_CoreXPro, + LicenseType_CoreOPro, + LicenseType_CoreQPro, + LicenseType_CoreXOQPro, + LicenseType_CoreXR, + LicenseType_CorePrimeThree, + LicenseType_Feature, + +} LicenseType; + +/// @brief The possible FPS rates +typedef enum TimecodeFPS +{ + TimecodeFPS_Undefined, + TimecodeFPS_23_976, + TimecodeFPS_24, + TimecodeFPS_25, + TimecodeFPS_29_97, + TimecodeFPS_30, + TimecodeFPS_48, + TimecodeFPS_50, + TimecodeFPS_59_94, + TimecodeFPS_60, + TimecodeFPS_29_97DF, + TimecodeFPS_59_94DF, +} TimecodeFPS; + +typedef enum FingerJointType +{ + FingerJointType_Invalid, + FingerJointType_Metacarpal, + FingerJointType_Proximal, + FingerJointType_Intermediate, + FingerJointType_Distal, //thumb doesn't have it + FingerJointType_Tip +}FingerJointType; + +typedef enum SetGloveCalibrationReturnCode +{ + SetGloveCalibrationReturnCode_Error, + SetGloveCalibrationReturnCode_Success, + SetGloveCalibrationReturnCode_VersionError, + SetGloveCalibrationReturnCode_WrongSideError, + SetGloveCalibrationReturnCode_GloveNotFoundError, + SetGloveCalibrationReturnCode_UserServiceError, + SetGloveCalibrationReturnCode_DeserializationError, + + SetGloveCalibrationReturnCode_MAX_SIZE +} SetGloveCalibrationReturnCode; + +/****************************************************************************** + * Structs. + *****************************************************************************/ + +/// @brief A 3D vector, used for translations. +typedef struct ManusVec3 +{ + float x; //default = 0.0f; + float y; //default = 0.0f; + float z; //default = 0.0f; +} ManusVec3; + +/// @brief A 2D vector, used for translations. +typedef struct ManusVec2 +{ + float x; //default = 0.0f; + float y; //default = 0.0f; +} ManusVec2; + +/// @brief A quaternion, used for rotations. +typedef struct ManusQuaternion +{ + float w; //default = 1.0f; + float x; //default = 0.0f; + float y; //default = 0.0f; + float z; //default = 0.0f; +} ManusQuaternion; + +/// @brief Transform containing position, rotation and scaling. +typedef struct ManusTransform +{ + ManusVec3 position; + ManusQuaternion rotation; + ManusVec3 scale; +} ManusTransform; + +/// @brief Color containing red, green, blue and alpha. +typedef struct Color +{ + float r; //default = 0.0f; + float g; //default = 0.0f; + float b; //default = 0.0f; + float a; //default = 0.0f; +}Color; + +/// @brief A Timestamp +typedef struct ManusTimestampInfo +{ + uint16_t fraction; //is either frame in timecode or miliseconds in non timecode + uint8_t second; + uint8_t minute; + uint8_t hour; + uint8_t day; //is 0 in timecode + uint8_t month; //is 0 in timecode + uint32_t year; //is 0 in timecode + bool timecode; +} ManusTimestampInfo; + +/// @brief A compressed timestamp +typedef struct ManusTimestamp +{ + uint64_t time; +} ManusTimestamp; + +/// @brief Information regarding IMU sensors used for calibration +typedef struct IMUCalibrationInfo +{ + uint32_t mag; // Magnometer calibration level 0-3 + uint32_t acc; // Accelerometer caibraton level 0-3 + uint32_t gyr; // Gyroscope calibration level 0-3 + uint32_t sys; // System accuracy +} IMUCalibrationInfo; + +/// @brief Used to describe hardware, firmware or ManusCore version. +typedef struct Version +{ + uint32_t major; + uint32_t minor; + uint32_t patch; + char label[MAX_NUM_CHARS_IN_VERSION]; + char sha[MAX_NUM_CHARS_IN_VERSION]; + char tag[MAX_NUM_CHARS_IN_VERSION]; +} Version; + +typedef struct FirmwareVersion +{ + int32_t version; + ManusTimestamp timestamp; +} FirmwareVersion; + +/// @brief Stores a single version string. +typedef struct ManusVersion +{ + char versionInfo[MAX_NUM_CHARS_IN_VERSION]; +} ManusVersion; + +/// @brief Contains information for connecting to a host running Manus Core. +/// Note that if one of these values is blank, the other will be used when +/// connecting. +typedef struct ManusHost +{ + char hostName[MAX_NUM_CHARS_IN_HOST_NAME]; // TODO utf8 compliant? + char ipAddress[MAX_NUM_CHARS_IN_IP_ADDRESS]; + Version manusCoreVersion; +} ManusHost; + +/****************************************************************************** + * Tracking + *****************************************************************************/ + + + +/// @brief Stores the name of a tracker. +typedef struct TrackerId +{ + char id[MAX_NUM_CHARS_IN_TRACKER_ID]; // todo. make this UTF8 compliant ? +} TrackerId; + +/// @brief Stores all the tracker data that can be sent or received. +typedef struct TrackerData +{ + ManusTimestamp lastUpdateTime; //default = 0; + + TrackerId trackerId; + + uint32_t userId; //default = 0; + + bool isHmd; //default = false; + TrackerType trackerType; //default = TrackerType::TrackerType_Unknown; + + ManusQuaternion rotation; + ManusVec3 position; + + TrackingQuality quality; //default = TrackingQuality::TrackingQuality_Untrackable; +} TrackerData; + + +/// @brief Stores the information sent by the tracker stream. +typedef struct TrackerStreamInfo +{ + ManusTimestamp publishTime; //default = 0; // DateTime.UtcNow. + uint32_t trackerCount; //default = 0; +} TrackerStreamInfo; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Tracking +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Gesture Stream +// ------------------------------------------------------------------------------------------------------------------------------ + +typedef struct GestureProbability +{ + uint32_t id; + float percent; +} GestureProbability; + +typedef struct GestureProbabilities +{ + uint32_t id; //default = 0; + bool isUserID; //default = false; + uint32_t totalGestureCount; //default = 0; + GestureProbability gestureData[MAX_GESTURE_DATA_CHUNK_SIZE]; //default = { 0 }; + uint32_t gestureCount; //default = 0; +} GestureProbabilities; + +typedef struct GestureStreamInfo +{ + ManusTimestamp publishTime; //default = 0; // DateTime.UtcNow. + uint32_t gestureProbabilitiesCount; //default = 0; +} GestureStreamInfo; + + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Gesture Stream +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Skeleton Stream +// ------------------------------------------------------------------------------------------------------------------------------ + +/// @brief Stores the information regarding each skeleton node. +/// The transform is defined as a local or global transform depending on the coordinate system set when initializing the SDK. +/// See functions CoreSdk_InitializeCoordinateSystemWithVUH and CoreSdk_InitializeCoordinateSystemWithDirection. +typedef struct SkeletonNode +{ + uint32_t id; + ManusTransform transform; +} SkeletonNode; + +/// @brief Stores the information regarding the skeletons that have been added to Manus Core. +typedef struct SkeletonInfo +{ + uint32_t id; //default = 0; + uint32_t nodesCount; //default = 0; + ManusTimestamp publishTime; //default = 0; // DateTime.UtcNow. +} SkeletonInfo; + +/// @brief Stores the information regarding the skeletons coming from the estimation system in core +typedef struct RawSkeletonInfo +{ + uint32_t gloveId; //default = 0; + uint32_t nodesCount; //default = 0; + ManusTimestamp publishTime; //default = 0; // DateTime.UtcNow. +} RawSkeletonInfo; + +/// @brief Stores the information sent by the skeleton stream. +typedef struct SkeletonStreamInfo +{ + ManusTimestamp publishTime; //default = 0; // DateTime.UtcNow. + uint32_t skeletonsCount; //default = 0; +} SkeletonStreamInfo; + + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Skeleton Stream +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Ergonomics +// ------------------------------------------------------------------------------------------------------------------------------ + +/// @brief Stores the received ergonomics data. +typedef struct ErgonomicsData +{ + uint32_t id; //default = 0; + bool isUserID; //default = false; + float data[ErgonomicsDataType_MAX_SIZE]; //default = { 0 }; +} ErgonomicsData; + +/// @brief Stores the information sent by the ergonomics stream. +typedef struct ErgonomicsStream +{ + ManusTimestamp publishTime; //default = 0; + ErgonomicsData data[MAX_NUMBER_OF_ERGONOMICS_DATA]; + uint32_t dataCount; //default = 0; +} ErgonomicsStream; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Ergonomics +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Landscape +// ------------------------------------------------------------------------------------------------------------------------------ + +/// @brief Stores all the received dongle data. +typedef struct DongleLandscapeData +{ + uint32_t id; + DeviceClassType classType; + DeviceFamilyType familyType; + bool isHaptics; + + Version hardwareVersion; + Version firmwareVersion; + ManusTimestamp firmwareTimestamp; + + uint32_t chargingState; + + int32_t channel; + + UpdateStatusEnum updateStatus; + + char licenseType[MAX_NUM_CHARS_IN_LICENSE_TYPE]; + + ManusTimestamp lastSeen; // this may be used in playback/recording TBD + + uint32_t leftGloveID; + uint32_t rightGloveID; + + LicenseType licenseLevel; //default = LicenseType::LicenseType_Undefined; + ManusTimestamp licenseExpiration; + + uint32_t netDeviceID; +} DongleLandscapeData; + +/// @brief Stores all the received glove data. +typedef struct GloveLandscapeData +{ + uint32_t id; + DeviceClassType classType; + DeviceFamilyType familyType; + Side side; + bool isHaptics; + + DevicePairedState pairedState; + uint32_t dongleID; + + Version hardwareVersion; + Version firmwareVersion; + ManusTimestamp firmwareTimestamp; + + UpdateStatusEnum updateStatus; + + uint32_t batteryPercentage; + int32_t transmissionStrength; + + IMUCalibrationInfo iMUCalibrationInfo[MAX_NUM_IMUS_ON_GLOVE]; + + ManusTimestamp lastSeen; // this may be used in playback/recording TBD + bool excluded; + + uint32_t netDeviceID; +} GloveLandscapeData; + +/// @brief Stores informations regarding the lengths of different parts of the body. +typedef struct Measurement +{ + MeasurementType entryType; //default = MeasurementType::MeasurementType_Unknown; + float value; //default = 0.0f; + + MeasurementUnit unit; //default = MeasurementUnit_Meters; + MeasurementCategory category; //default = MeasurementCategory_Misc; + char displayName[MAX_NUM_CHARS_IN_MEASUREMENT]; +} Measurement; + +/// @brief Stores the local offsets to the trackers. +typedef struct TrackerOffset +{ + TrackerOffsetType entryType; + ManusVec3 translation; + ManusQuaternion rotation; +} TrackerOffset; + +/// @brief Stores possible extra offsets to the trackers. +typedef struct ExtraTrackerOffset +{ + ExtraTrackerOffsetType entryType; //default = ExtraTrackerOffsetType::ExtraTrackerOffsetType_Unknown; + float value; //default = 0.0f; +} ExtraTrackerOffset; + +/// @brief Stores all the received tracker data. +typedef struct TrackerLandscapeData +{ + char id[MAX_NUM_CHARS_IN_TRACKER_ID]; + TrackerType type; //default = TrackerType::TrackerType_Unknown; + TrackerSystemType systemType; //default = TrackerSystemType::TrackerSystemType_Unknown; + uint32_t user; //default = 0; + bool isHMD; //default = false; + char manufacturer[MAX_NUM_CHARS_IN_TRACKER_MANUFACTURER]; // default = "Unknown" + char productName[MAX_NUM_CHARS_IN_TRACKER_PRODUCTNAME]; // default = "Unknown" +} TrackerLandscapeData; + +/// @brief Stores all the received user profile data. +typedef struct UserProfileLandscapeData +{ + ProfileType profileType; //default = ProfileType::ProfileType_Hands; + Measurement measurements[MeasurementType_MAX_SIZE]; + TrackerOffset trackerOffsets[TrackerOffsetType_MAX_SIZE]; + ExtraTrackerOffset extraTrackerOffsets[ExtraTrackerOffsetType_MAX_SIZE]; +} UserProfileLandscapeData; + +/// @brief Stores all the received user data. +typedef struct UserLandscapeData +{ + uint32_t id; //default = 0; + char name[MAX_NUM_CHARS_IN_USERNAME]; + Color color; + uint32_t dongleID; //default = 0; + uint32_t leftGloveID; //default = 0; + uint32_t rightGloveID; //default = 0; + UserProfileLandscapeData profile; + uint32_t userIndex; //default = 0; +} UserLandscapeData; + +/// @brief Stores all the received skeleton data. +typedef struct SkeletonLandscapeData +{ + uint32_t id; //default = 0; + char session[MAX_NUM_CHARS_IN_HOST_NAME]; + uint32_t userId; //default = 0; + SkeletonType type; //default = SkeletonType::SkeletonType_Invalid; + char rootBoneName[MAX_NUM_CHARS_IN_NODE_NAME]; + bool scaled; //default = false; +} SkeletonLandscapeData; + +/// @brief Stores all the information related to the devices present in the landscape. +typedef struct DeviceLandscape +{ + DongleLandscapeData dongles[MAX_NUMBER_OF_DONGLES]; + uint32_t dongleCount; //default = 0; + GloveLandscapeData gloves[MAX_NUMBER_OF_GLOVES]; + uint32_t gloveCount; //default = 0; +} DeviceLandscape; + +/// @brief Stores all the information related to the users present in the landscape. +typedef struct UserLandscape +{ + UserLandscapeData users[MAX_USERS]; + uint32_t userCount; //default = 0; +} UserLandscape; + +/// @brief Stores all the information related to the skeletons present in the landscape. +typedef struct SkeletonLandscape +{ + SkeletonLandscapeData skeletons[MAX_NUMBER_OF_SKELETONS]; + uint32_t skeletonCount; //default = 0; +} SkeletonLandscape; + +/// @brief Stores all the information related to the trackers present in the landscape. +typedef struct TrackerLandscape +{ + TrackerLandscapeData trackers[MAX_NUMBER_OF_TRACKERS]; + uint32_t trackerCount; //default = 0; +} TrackerLandscape; + +/// @brief Stores the license information. +typedef struct LicenseInfo +{ + bool recording; //default = false; + bool exporting; //default = false; + bool advancedExporting; //default = false; + bool unitySession; //default = false; + bool unrealSession; //default = false; + bool openXRSession; //default = false; + bool sdk; //default = false; + bool raw; //default = false; + bool mobuSession; //default = false; + bool xsensSession; //default = false; + bool optitrackSession; //default = false; + bool qualisysSession; //default = false; + bool viconSession; //default = false; + bool nokovSession; //default = false; + bool icidoSession; //default = false; + bool siemensSession; //default = false; + bool vredSession; //default = false; + +} LicenseInfo; + +/// @brief Stores the landscape settings. +typedef struct SettingsLandscape +{ + Version manusCoreVersion; + LicenseInfo license; + bool playbackMode; //default = false; + bool ignoreSessionTimeOuts; //default = false; + FirmwareVersion firmwareOne; + FirmwareVersion firmwareTwo; + bool recordingMode; //default = false; + bool isNetDevice; // default = false; + bool isConnectedAsNetDevice; // default = false; +}SettingsLandscape; + +typedef struct TimecodeInterface +{ + char name[MAX_NUM_CHARS_IN_TIMECODE_INTERFACE_STRINGS]; + char api[MAX_NUM_CHARS_IN_TIMECODE_INTERFACE_STRINGS]; + int index; //default = -1; +} TimecodeInterface; + +typedef struct TimeLandscape +{ + TimecodeInterface interfaces[MAX_NUMBER_OF_AUDIO_INTERFACES]; + uint32_t interfaceCount; //default = 0; + TimecodeInterface currentInterface; + TimecodeFPS fps; //default = Undefined; + bool fakeTimecode; //default = false; + bool useSyncPulse; //default = false; + bool deviceKeepAlive; //default = false; + bool syncStatus; //default = false; + bool timecodeStatus; //default = false; + int32_t ltcChannel; //default = -1; +} TimeLandscape; + +/// @brief Contains information about a gesture +typedef struct GestureLandscapeData +{ + uint32_t id; //default = 0; + char name[MAX_NUM_CHARS_IN_USERNAME]; +} GestureLandscapeData; + +/// @brief Contains information about a net devices +typedef struct NetDeviceLandscapeData +{ + uint32_t netDeviceID; + char hostname[MAX_NUM_CHARS_IN_HOST_NAME]; + char ip[MAX_NUM_CHARS_IN_IP_ADDRESS]; +} NetDeviceLandscapeData; + +/// @brief Contains information about a single net device +typedef struct NetDevicesLandscape +{ + uint32_t numberOfNetDevices; // default = 0; + NetDeviceLandscapeData netDevices[MAX_NUMBER_OF_NETDEVICES]; +} NetDeviceLandscape; + +/// @brief Stores the landscape data. +typedef struct Landscape +{ + DeviceLandscape gloveDevices; + UserLandscape users; + SkeletonLandscape skeletons; + TrackerLandscape trackers; + SettingsLandscape settings; + NetDevicesLandscape netDevices; + TimeLandscape time; + uint32_t gestureCount; //default = 0; +} Landscape; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Landscape +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Skeleton +// ------------------------------------------------------------------------------------------------------------------------------ +/// @brief Stores the inverse kinematics settings of a node, the ikAim is used to control the IK solve, 1 is default, -1 is inversed. +typedef struct NodeSettingsIK +{ + float ikAim; +} NodeSettingsIK; + +/// @brief Stores the settings for a node of type foot, heightFromGround is used to set the height of the 3d model ankle from ground. +typedef struct NodeSettingsFoot +{ + float heightFromGround; +} NodeSettingsFoot; + +/// @brief Stores the rotation offset of a node, this is used internally for building the skeleton. +typedef struct NodeSettingsRotationOffset +{ + ManusQuaternion value; +} NodeSettingsRotationOffset; + +/// @brief Stores the settings of a node of type leaf, the direction vector is defined with respect to the previous node in the chain. +typedef struct NodeSettingsLeaf +{ + ManusVec3 direction; + float length; +} NodeSettingsLeaf; + +/// @brief Stores all the node settings. +typedef struct NodeSettings +{ + NodeSettingsFlag usedSettings; + NodeSettingsIK ik; + NodeSettingsFoot foot; + NodeSettingsRotationOffset rotationOffset; + NodeSettingsLeaf leaf; +} NodeSettings; + +/// @brief Stores the node setup information. Each node represents a segment of the skeleton that can be animated, nodes combine together to form chains. +/// the parentID is used to identify the node with respect to which the current one will move. +typedef struct NodeSetup +{ + uint32_t id; //default = 0; + char name[MAX_NUM_CHARS_IN_NODE_NAME]; // this is a UTF8 string , NOT an ASCII CHAR array (same base type though) + NodeType type; //default = NodeType::NodeType_Invalid; + ManusTransform transform; + uint32_t parentID; //default = 0; + NodeSettings settings; +} NodeSetup; + +/// @brief Stores all the settings of a chain of type pelvis. +typedef struct ChainSettingsPelvis +{ + float hipHeight; //default = 0.0f; + float hipBendOffset; //default = 0.0f; + float thicknessMultiplier; //default = 1.0f; +} ChainSettingsPelvis; + +/// @brief Stores all the settings of a chain of type leg. +typedef struct ChainSettingsLeg +{ + bool reverseKneeDirection; //default = false; + float kneeRotationOffset; //default = 0.0f; + float footForwardOffset; //default = 0.0f; + float footSideOffset; //default = 0.0f; +} ChainSettingsLeg; + +/// @brief Stores all the settings of a chain of type spine. +typedef struct ChainSettingsSpine +{ + float spineBendOffset; //default = 0.0f; +} ChainSettingsSpine; + +/// @brief Stores all the settings of a chain of type neck. +typedef struct ChainSettingsNeck +{ + float neckBendOffset; //default = 0.0f; +} ChainSettingsNeck; + +/// @brief Stores all the settings of a chain of type head. +typedef struct ChainSettingsHead +{ + float headPitchOffset; //default = 0.0f; + float headYawOffset; //default = 0.0f; + float headTiltOffset; //default = 0.0f; + bool useLeafAtEnd; //default = false; +} ChainSettingsHead; + +/// @brief Stores all the settings of a chain of type arm. +typedef struct ChainSettingsArm +{ + float armLengthMultiplier; //default = 0.0f; + float elbowRotationOffset; //default = 0.0f; + + ManusVec3 armRotationOffset; + + ManusVec3 positionMultiplier; + ManusVec3 positionOffset; +} ChainSettingsArm; + +/// @brief Stores all the settings of a chain of type shoulder. +typedef struct ChainSettingsShoulder +{ + float forwardOffset; //default = 0.0f; + float shrugOffset; //default = 0.0f; + + float forwardMultiplier; //default = 0.0f; + float shrugMultiplier; //default = 0.0f; +} ChainSettingsShoulder; + +/// @brief Stores all the settings of a chain of type finger. +typedef struct ChainSettingsFinger +{ + bool useLeafAtEnd; //default = false; + int32_t metacarpalBoneId; //default = 0; + int32_t handChainId; //default = 0; + float fingerWidth; //default = 0; +} ChainSettingsFinger; + +/// @brief Stores all the settings of a chain of type hand. +typedef struct ChainSettingsHand +{ + int32_t fingerChainIds[MAX_NUM_FINGER_IDS]; + int32_t fingerChainIdsUsed; //default = 0; + HandMotion handMotion; //default = HandMotion::HandMotion_None; +} ChainSettingsHand; + +/// @brief Stores all the settings of a chain of type foot. +typedef struct ChainSettingsFoot +{ + int32_t toeChainIds[MAX_NUM_TOE_IDS]; + int32_t toeChainIdsUsed; //default = 0; +} ChainSettingsFoot; + +/// @brief Stores all the settings of a chain of type toe. +typedef struct ChainSettingsToe +{ + int32_t footChainId; //default = 0; + bool useLeafAtEnd; //default = false; +} ChainSettingsToe; + +/// @brief Stores all chain settings. +typedef struct ChainSettings +{ + ChainType usedSettings; + ChainSettingsPelvis pelvis; + ChainSettingsLeg leg; + ChainSettingsSpine spine; + ChainSettingsNeck neck; + ChainSettingsHead head; + ChainSettingsArm arm; + ChainSettingsShoulder shoulder; + ChainSettingsFinger finger; + ChainSettingsHand hand; + ChainSettingsFoot foot; + ChainSettingsToe toe; +} ChainSettings; + +/// @brief Stores the chain setup information. +typedef struct ChainSetup +{ + uint32_t id; //default = 0; + ChainType type; //default = ChainType::ChainType_Invalid; + ChainType dataType; //default = ChainType::ChainType_Invalid; + uint32_t dataIndex; //default = 0; + uint32_t nodeIdCount; //default = 0; + uint32_t nodeIds[MAX_CHAIN_LENGTH]; + ChainSettings settings; + Side side; //default = Side::Side_Invalid; +} ChainSetup; + +/// @brief Stores all the settings of a collider of type sphere. +typedef struct SphereColliderSetup +{ + float radius; //default = 0; +} SphereColliderSetup; + +/// @brief Stores all the settings of a collider of type capsule. +typedef struct CapsuleColliderSetup +{ + float radius; //default = 0; + float length; //default = 0; +} CapsuleColliderSetup; + +/// @brief Stores all the settings of a collider of type box. +typedef struct BoxColliderSetup +{ + ManusVec3 size; //default = 0,0,0; +} BoxColliderSetup; + +/// @brief Stores the collider setup information. +typedef struct ColliderSetup +{ + uint32_t nodeID; //default = 0; + ManusVec3 localPosition; // default 0,0,0 + ManusVec3 localRotation; // default 0,0,0 + + ColliderType type;//default = ColliderType::ColliderType_Invalid + SphereColliderSetup sphere; + CapsuleColliderSetup capsule; + BoxColliderSetup box; +} ColliderSetup; + +/// @brief Stores the vertex weight information. +typedef struct Weight +{ + uint32_t nodeID; + float weightValue; +} Weight; + +/// @brief Stores the vertex information. +typedef struct Vertex +{ + ManusVec3 position; // default 0,0,0 + uint32_t weightsCount; //default = 0; + Weight weights[MAX_BONE_WEIGHTS_PER_VERTEX]; +} Vertex; + +/// @brief Stores the triangle information. +typedef struct Triangle +{ + int32_t vertexIndex1; + int32_t vertexIndex2; + int32_t vertexIndex3; +} Triangle; + +/// @brief Stores the information regarding the user data used to animate the skeleton. +typedef struct SkeletonTargetUserData +{ + uint32_t userID; //default = 0; +} SkeletonTargetUserData; + +/// @brief Stores the information regarding the user index data used to animate the skeleton. +typedef struct SkeletonTargetUserIndexData +{ + uint32_t userIndex; //default = 0; +} SkeletonTargetUserIndexData; + +/// @brief Stores the information regarding the animation data used to animate the skeleton. +typedef struct SkeletonTargetAnimationData +{ + char id[MAX_NUM_CHARS_IN_TARGET_ID]; +} SkeletonTargetAnimationData; + +/// @brief Stores the information regarding the glove data used to animate the skeleton. +typedef struct SkeletonTargetGloveData +{ + uint32_t gloveID; //default = 0; +} SkeletonTargetGloveData; + +/// @brief Stores all the possible skeleton settings. +typedef struct SkeletonSettings +{ + bool scaleToTarget; //default = false; + bool useEndPointApproximations; //default = false; + CollisionType collisionType; //default = CollisionType::CollisionType_None + + SkeletonTargetType targetType; + SkeletonTargetUserData skeletonTargetUserData; + SkeletonTargetUserIndexData skeletonTargetUserIndexData; + SkeletonTargetAnimationData skeletonTargetAnimationData; + SkeletonTargetGloveData skeletonGloveData; +} SkeletonSettings; + +/// @brief Stores the skeleton setup information. +typedef struct SkeletonSetupInfo +{ + uint32_t id; //default = 0; + SkeletonType type; //default = SkeletonType::SkeletonType_Invalid; + SkeletonSettings settings; + char name[MAX_NUM_CHARS_IN_SKELETON_NAME]; // this is a UTF8 string , NOT an ASCII CHAR array (same base type though) +} SkeletonSetupInfo; + + +/// @brief Stores the amount of nodes and chains in the skeleton setup. +typedef struct SkeletonSetupArraySizes +{ + uint32_t nodesCount; //default = 0; + uint32_t chainsCount; //default = 0; + uint32_t collidersCount; //default = 0; + uint32_t meshCount; //default = 0; +} SkeletonSetupArraySizes; + +/// @brief Stores the temporary skeleton information. +typedef struct TemporarySkeletonInfo +{ + char name[MAX_NUM_CHARS_IN_SKELETON_NAME]; // this is a UTF8 string , NOT an ASCII CHAR array (same base type though) + uint32_t index; //default = UINT32_MAX; +} TemporarySkeletonInfo; + +/// @brief Stores the temporary skeletons available for a specific session. +typedef struct TemporarySkeletonsInfoForSession +{ + uint32_t sessionId; //default = 0; + char sessionName[MAX_NUM_CHARS_IN_HOST_NAME]; + uint32_t skeletonCount; //default = 0; + TemporarySkeletonInfo skeletonInfo[MAX_NUMBER_OF_SKELETONS_PER_SESSION]; +} TemporarySkeletonsInfoForSession; + +/// @brief Stores the temporary skeletons available for a specific session. +typedef struct TemporarySkeletonCountForSession +{ + uint32_t sessionId; //default = 0; + char sessionName[MAX_NUM_CHARS_IN_HOST_NAME]; + uint32_t skeletonCount; //default = 0; +} TemporarySkeletonCountForSession; + +/// @brief Stores the temporary skeleton available for all sessions connected to Core. +typedef struct TemporarySkeletonCountForAllSessions +{ + uint32_t sessionsCount; //default = 0; + TemporarySkeletonCountForSession temporarySkeletonCountForSessions[MAX_NUMBER_OF_SESSIONS]; +} TemporarySkeletonCountForAllSessions; + +/// @brief ONLY USED INTERNALLY +/// @brief Stores the temporary skeleton available for all sessions connected to Core. +typedef struct TemporarySkeletonSessionsData +{ + uint32_t sessionsCount; //default = 0; + TemporarySkeletonsInfoForSession temporarySkeletonsSessions[MAX_NUMBER_OF_SESSIONS]; +} TemporarySkeletonSessionsData; + +/// @brief Stores the data associated to System messages received from Core. +typedef struct SystemMessage +{ + SystemMessageType type; + char infoString[MAX_NUM_CHARS_IN_SYSTEM_ERROR_MESSAGE]; + uint32_t infoUInt; +} SystemMessage; + +typedef struct NodeInfo +{ + uint32_t nodeId; //default = 0; + uint32_t parentId; //default = 0; + ChainType chainType; + Side side; + FingerJointType fingerJointType; +}NodeInfo; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Skeleton +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Glove calibration +// ------------------------------------------------------------------------------------------------------------------------------ + +typedef struct GloveCalibrationArgs { + uint32_t gloveId; //default = 0; +} GloveCalibrationArgs; + +typedef struct GloveCalibrationStepArgs { + uint32_t gloveId; //default = 0; + uint32_t stepIndex; //default = 0; +} GloveCalibrationStepArgs; + +/// @brief Stores the data associated to a single calibration step for a specific glove. +/// if the time value is negative, it means that its a continuous step and the value is estimated +typedef struct GloveCalibrationStepData { + uint32_t index; //default = 0; + char title[MAX_NUM_CHARS_IN_CALIBRATION_TITLE]; + char description[MAX_NUM_CHARS_IN_CALIBRATION_DESCRIPTION]; + float time; //default = 0 +} GloveCalibrationStepData; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Glove calibration +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Coordinate system settings +// ------------------------------------------------------------------------------------------------------------------------------ + +/// @brief Stores the information regarding the coordinate system used by the client, defined as VUH (view, up, handedness). +typedef struct CoordinateSystemVUH +{ + AxisView view; + AxisPolarity up; + Side handedness; + float unitScale; // in meters +} CoordinateSystemVUH; + +/// @brief Stores the information regarding the coordinate system used by the client, defined by each axis direction. +typedef struct CoordinateSystemDirection +{ + AxisDirection x; + AxisDirection y; + AxisDirection z; + float unitScale; // in meters +} CoordinateSystemDirection; + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Coordinate system settings +// ------------------------------------------------------------------------------------------------------------------------------ + +// ------------------------------------------------------------------------------------------------------------------------------ +// Callbacks +// ------------------------------------------------------------------------------------------------------------------------------ + +/// @brief Describes the ConnectedToCoreCallback function type +typedef void(*ConnectedToCoreCallback_t)(const ManusHost* const p_Host); +/// @brief Describes the DisconnectedToCoreCallback function type +typedef void(*DisconnectedFromCoreCallback_t)(const ManusHost* const p_Host); +/// @brief Describes the LoggingCallback function type +typedef void(*LoggingCallback_t)(LogSeverity p_Severity, const char* const p_Log, uint32_t p_Length); +/// @brief Describes the SkeletonStreamCallback function type +typedef void(*SkeletonStreamCallback_t)(const SkeletonStreamInfo* const p_SkeletonInfo); +/// @brief Describes the LandscapeStreamCallback function type +typedef void(*LandscapeStreamCallback_t)(const Landscape* const p_Landscape); +/// @brief Describes the TrackerStreamCallback function type +typedef void(*TrackerStreamCallback_t)(const TrackerStreamInfo* const p_TrackerStreamInfo); +/// @brief Describes the ErgonomicsStreamCallback type +typedef void(*ErgonomicsStreamCallback_t)(const ErgonomicsStream* const p_Ergonomics); +/// @brief Describes the SystemStreamCallback function type +typedef void(*SystemStreamCallback_t)(const SystemMessage* const p_SystemMessage); +/// @brief Describes the RawSkeletonStreamCallback function type +typedef void(*RawSkeletonStreamCallback_t)(const SkeletonStreamInfo* const p_SkeletonInfo); +/// @brief Describes the GestureStreamCallback function type +typedef void(*GestureStreamCallback_t)(const GestureStreamInfo* const p_GestureStream); + +// ------------------------------------------------------------------------------------------------------------------------------ +// end of Callbacks +// ------------------------------------------------------------------------------------------------------------------------------ +#ifdef __cplusplus +} // extern "C" +#endif + +// Close the Doxygen group. +/** @} */ + +#endif // #ifndef H_CORESDKWRAPPERTYPES diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK.so b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK.so new file mode 100644 index 0000000..0a4797d --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b1676a140e00368def349ce55f3990fe0fc4be3fd0a3284ad95885733b7229f +size 80253728 diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK_Integrated.so b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK_Integrated.so new file mode 100644 index 0000000..0a4797d --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib/libManusSDK_Integrated.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b1676a140e00368def349ce55f3990fe0fc4be3fd0a3284ad95885733b7229f +size 80253728 diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusServer.cpython-310-x86_64-linux-gnu.so b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusServer.cpython-310-x86_64-linux-gnu.so new file mode 100755 index 0000000..1f84aec --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/ManusServer.cpython-310-x86_64-linux-gnu.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e24785ebe724751e28605df17755b8f730357ee14f09d89be3e21c3d3d95310b +size 3820896 diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/Readme.md b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Readme.md new file mode 100644 index 0000000..4da2914 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/Readme.md @@ -0,0 +1,4 @@ +# MANUS SDK Client - Linux +The MANUS SDK Client is an example client that demonstrates the most functionality of the MANUS SDK. It handles setting up a connection to the gloves (be it directly or via a MANUS Core instance). And implements all major features available in the MANUS SDK. + +For a full guide on how to get started please refer to the guide on our knowledge center: https://docs.manus-meta.com/latest/Plugins/SDK/Windows/SDK%20Client/ \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.cpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.cpp new file mode 100644 index 0000000..1cb5aad --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.cpp @@ -0,0 +1,3118 @@ +#include "SDKClient.hpp" +#include "ManusSDKTypes.h" +#include "ClientLogging.hpp" +#include +#include +#include +#include +#include +#include + +#include "ClientPlatformSpecific.hpp" + +#define GO_TO_DISPLAY(p_Key,p_Function) if (GetKeyDown(p_Key)) { ClearConsole();\ + m_CurrentInteraction = std::bind(&SDKClient::p_Function, this); return ClientReturnCode::ClientReturnCode_Success;} + +#define GO_TO_MENU_IF_REQUESTED() if (GetKeyDown('Q')) { ClearConsole();\ + m_CurrentInteraction = nullptr; return ClientReturnCode::ClientReturnCode_Success;} + +using ManusSDK::ClientLog; + +std::map> output_map; + +SDKClient* SDKClient::s_Instance = nullptr; + +std::vector idleft={3822396207, 3998055887}; +std::vector idright={3762867141, 831307785}; + +SDKClient::SDKClient() +{ + s_Instance = this; + + // using initializers like these ensure that the data is set to its default values. + ErgonomicsData_Init(&m_LeftGloveErgoData); + ErgonomicsData_Init(&m_RightGloveErgoData); + + TestTimestamp(); +} + +SDKClient::~SDKClient() +{ + s_Instance = nullptr; +} + +/// @brief Initialize the sample console and the SDK. +/// This function attempts to resize the console window and then proceeds to initialize the SDK's interface. +ClientReturnCode SDKClient::Initialize() +{ + // if (!PlatformSpecificInitialization()) + // { + // return ClientReturnCode::ClientReturnCode_FailedPlatformSpecificInitialization; + // } + + const ClientReturnCode t_IntializeResult = InitializeSDK(); + if (t_IntializeResult != ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::error("Failed to initialize the SDK. Are you sure the correct ManusSDKLibary is used?"); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief The main SDKClient loop. +/// This is a simple state machine which switches between different substates. +ClientReturnCode SDKClient::Run() +{ + ConnectingToCore(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief When you are done with the SDK, don't forget to nicely shut it down +/// this will close all connections to the host, close any threads and clean up after itself +/// after this is called it is expected to exit the client program. If not it needs to call initialize again. +ClientReturnCode SDKClient::ShutDown() +{ + const SDKReturnCode t_Result = CoreSdk_ShutDown(); + if (t_Result != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to shut down the SDK wrapper. The value returned was {}.", (int32_t)t_Result); + return ClientReturnCode::ClientReturnCode_FailedToShutDownSDK; + } + + if (!PlatformSpecificShutdown()) + { + return ClientReturnCode::ClientReturnCode_FailedPlatformSpecificShutdown; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief Gets called when the client is connects to manus core +/// Using this callback is optional. +/// In this sample we use this callback to change the client's state and switch to another screen +void SDKClient::OnConnectedCallback(const ManusHost* const p_Host) +{ + ClientLog::print("Connected to manus core."); + + //No need to initialize these as they get filled in the CoreSdk_GetVersionsAndCheckCompatibility + ManusVersion t_SdkVersion; + ManusVersion t_CoreVersion; + bool t_IsCompatible; + + const SDKReturnCode t_Result = CoreSdk_GetVersionsAndCheckCompatibility(&t_SdkVersion, &t_CoreVersion, &t_IsCompatible); + + if (t_Result == SDKReturnCode::SDKReturnCode_Success) + { + const std::string t_Versions = "Sdk version : " + std::string(t_SdkVersion.versionInfo) + ", Core version : " + std::string(t_CoreVersion.versionInfo) + "."; + + if (t_IsCompatible) + { + ClientLog::print("Versions are compatible.{}", t_Versions); + } + else + { + ClientLog::warn("Versions are not compatible with each other.{}", t_Versions); + } + } + else + { + ClientLog::error("Failed to get the versions from the SDK. The value returned was {}.", (int32_t)t_Result); + } + + uint32_t t_SessionId; + const SDKReturnCode t_SessionIdResult = CoreSdk_GetSessionId(&t_SessionId); + if (t_SessionIdResult == SDKReturnCode::SDKReturnCode_Success && t_SessionId != 0) + { + ClientLog::print("Session Id: {}", t_SessionId); + s_Instance->m_SessionId = t_SessionId; + } + else + { + ClientLog::print("Failed to get the Session ID from Core. The value returned was{}.", (int32_t)t_SessionIdResult); + } + + ManusHost t_Host(*p_Host); + s_Instance->m_Host = std::make_unique(t_Host); + + // Set the hand motion mode of the RawSkeletonStream. This is optional and can be set to any of the HandMotion enum values. Default = None + const SDKReturnCode t_HandMotionResult = CoreSdk_SetRawSkeletonHandMotion(HandMotion_Auto); + if (t_HandMotionResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::print("Failed to set the hand motion mode. The value returned was {}.", (int32_t)t_HandMotionResult); + } + + // Only setting state to displaying data on automatic reconnect + if (s_Instance->m_State == ClientState::ClientState_Disconnected) + { + s_Instance->m_State = ClientState::ClientState_DisplayingData; + } +} + +/// @brief Gets called when the client disconnects from manus core. +/// This callback is optional and in the sample changes the client's state. +void SDKClient::OnDisconnectedCallback(const ManusHost* const p_Host) +{ + ClientLog::print("Disconnected from manus core."); + s_Instance->m_TimeSinceLastDisconnect = std::chrono::high_resolution_clock::now(); + ManusHost t_Host(*p_Host); + s_Instance->m_Host = std::make_unique(t_Host); + s_Instance->m_State = ClientState::ClientState_Disconnected; +} + +void SDKClient::OnLogCallback(LogSeverity p_Severity, const char* const p_Log, uint32_t p_Length) +{ + if (!s_Instance)return; + + auto t_Log = new SDKLog(); + t_Log->severity = p_Severity; + t_Log->string = std::string(p_Log); + s_Instance->m_LogMutex.lock(); + s_Instance->m_Logs.push_back(t_Log); + s_Instance->m_LogMutex.unlock(); +} + +/// @brief This gets called when the client is connected to manus core +/// @param p_SkeletonStreamInfo contains the meta data on how much data regarding the skeleton we need to get from the SDK. +void SDKClient::OnSkeletonStreamCallback(const SkeletonStreamInfo* const p_SkeletonStreamInfo) +{ + if (s_Instance) + { + ClientSkeletonCollection* t_NxtClientSkeleton = new ClientSkeletonCollection(); + t_NxtClientSkeleton->skeletons.resize(p_SkeletonStreamInfo->skeletonsCount); + + for (uint32_t i = 0; i < p_SkeletonStreamInfo->skeletonsCount; i++) + { + CoreSdk_GetSkeletonInfo(i, &t_NxtClientSkeleton->skeletons[i].info); + t_NxtClientSkeleton->skeletons[i].nodes.resize(t_NxtClientSkeleton->skeletons[i].info.nodesCount); + t_NxtClientSkeleton->skeletons[i].info.publishTime = p_SkeletonStreamInfo->publishTime; + CoreSdk_GetSkeletonData(i, t_NxtClientSkeleton->skeletons[i].nodes.data(), t_NxtClientSkeleton->skeletons[i].info.nodesCount); + } + s_Instance->m_SkeletonMutex.lock(); + if (s_Instance->m_NextSkeleton != nullptr) delete s_Instance->m_NextSkeleton; + s_Instance->m_NextSkeleton = t_NxtClientSkeleton; + s_Instance->m_SkeletonMutex.unlock(); + } +} + +/// @brief This gets called when the client is connected to manus core. It sends the skeleton data coming from the estimation system, before the retargeting to the client skeleton model. +/// @param p_RawSkeletonStreamInfo contains the meta data on how much data regarding the raw skeleton we need to get from the SDK. +void SDKClient::OnRawSkeletonStreamCallback(const SkeletonStreamInfo* const p_RawSkeletonStreamInfo) +{ + // std::cout<<"OnRawSkeletonStreamCallback"<skeletons.resize(p_RawSkeletonStreamInfo->skeletonsCount); + + for (uint32_t i = 0; i < p_RawSkeletonStreamInfo->skeletonsCount; i++) + { + CoreSdk_GetRawSkeletonInfo(i, &t_NxtClientRawSkeleton->skeletons[i].info); + t_NxtClientRawSkeleton->skeletons[i].nodes.resize(t_NxtClientRawSkeleton->skeletons[i].info.nodesCount); + t_NxtClientRawSkeleton->skeletons[i].info.publishTime = p_RawSkeletonStreamInfo->publishTime; + CoreSdk_GetRawSkeletonData(i, t_NxtClientRawSkeleton->skeletons[i].nodes.data(), t_NxtClientRawSkeleton->skeletons[i].info.nodesCount); + + std::string glove_id = std::to_string(t_NxtClientRawSkeleton->skeletons[i].info.gloveId); + std::vector positions; + std::vector orientations; + for (uint32_t j = 0; j < t_NxtClientRawSkeleton->skeletons[i].info.nodesCount; j++){ + positions.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.position.x); + positions.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.position.y); + positions.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.position.z); + orientations.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.rotation.w); + orientations.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.rotation.x); + orientations.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.rotation.y); + orientations.push_back(t_NxtClientRawSkeleton->skeletons[i].nodes[j].transform.rotation.z); + } + output_map[glove_id +"_position"] = positions; + output_map[glove_id +"_orientation"] = orientations; + } + s_Instance->m_RawSkeletonMutex.lock(); + if (s_Instance->m_NextRawSkeleton != nullptr) delete s_Instance->m_NextRawSkeleton; + s_Instance->m_NextRawSkeleton = t_NxtClientRawSkeleton; + s_Instance->m_RawSkeletonMutex.unlock(); + } +} + +/// @brief This gets called when receiving tracker information from core +/// @param p_TrackerStreamInfo contains the meta data on how much data regarding the trackers we need to get from the SDK. +void SDKClient::OnTrackerStreamCallback(const TrackerStreamInfo* const p_TrackerStreamInfo) +{ + if (s_Instance) + { + TrackerDataCollection* t_TrackerData = new TrackerDataCollection(); + + t_TrackerData->trackerData.resize(p_TrackerStreamInfo->trackerCount); + + for (uint32_t i = 0; i < p_TrackerStreamInfo->trackerCount; i++) + { + CoreSdk_GetTrackerData(i, &t_TrackerData->trackerData[i]); + } + s_Instance->m_TrackerMutex.lock(); + if (s_Instance->m_NextTrackerData != nullptr) delete s_Instance->m_NextTrackerData; + s_Instance->m_NextTrackerData = t_TrackerData; + s_Instance->m_TrackerMutex.unlock(); + } +} + +/// @brief This gets called when receiving gesture data from Manus Core +/// In our sample we only save the first glove's gesture data. +/// Gesture data gets generated and sent when glove data changes, this means that the stream +/// does not always contain ALL of the devices, because some may not have had new data since +/// the last time the gesture data was sent. +/// @param p_GestureStream contains the basic info to retrieve gesture data. +void SDKClient::OnGestureStreamCallback(const GestureStreamInfo* const p_GestureStream) +{ + if (s_Instance) + { + for (uint32_t i = 0; i < p_GestureStream->gestureProbabilitiesCount; i++) + { + GestureProbabilities t_Probs; + CoreSdk_GetGestureStreamData(i, 0, &t_Probs); + if (t_Probs.isUserID)continue; + if (t_Probs.id != s_Instance->m_FirstLeftGloveID && t_Probs.id != s_Instance->m_FirstRightGloveID)continue; + ClientGestures* t_Gest = new ClientGestures(); + t_Gest->info = t_Probs; + t_Gest->probabilities.reserve(t_Gest->info.totalGestureCount); + uint32_t t_BatchCount = (t_Gest->info.totalGestureCount / MAX_GESTURE_DATA_CHUNK_SIZE) + 1; + uint32_t t_ProbabilityIdx = 0; + for (uint32_t b = 0; b < t_BatchCount; b++) + { + for (uint32_t j = 0; j < t_Probs.gestureCount; j++) + { + t_Gest->probabilities.push_back(t_Probs.gestureData[j]); + } + t_ProbabilityIdx += t_Probs.gestureCount; + CoreSdk_GetGestureStreamData(i, t_ProbabilityIdx, &t_Probs); //this will get more data, if needed for the next iteration. + } + + s_Instance->m_GestureMutex.lock(); + if (t_Probs.id == s_Instance->m_FirstLeftGloveID) + { + if (s_Instance->m_NewFirstLeftGloveGestures != nullptr) delete s_Instance->m_NewFirstLeftGloveGestures; + s_Instance->m_NewFirstLeftGloveGestures = t_Gest; + } + else + { + if (s_Instance->m_NewFirstRightGloveGestures != nullptr) delete s_Instance->m_NewFirstRightGloveGestures; + s_Instance->m_NewFirstRightGloveGestures = t_Gest; + } + s_Instance->m_GestureMutex.unlock(); + } + } +} + +/// @brief This gets called when receiving landscape information from core +/// @param p_Landscape contains the new landscape from core. +void SDKClient::OnLandscapeCallback(const Landscape* const p_Landscape) +{ + if (s_Instance == nullptr)return; + + Landscape* t_Landscape = new Landscape(*p_Landscape); + s_Instance->m_LandscapeMutex.lock(); + if (s_Instance->m_NewLandscape != nullptr) delete s_Instance->m_NewLandscape; + s_Instance->m_NewLandscape = t_Landscape; + s_Instance->m_NewGestureLandscapeData.resize(t_Landscape->gestureCount); + CoreSdk_GetGestureLandscapeData(s_Instance->m_NewGestureLandscapeData.data(), (uint32_t)s_Instance->m_NewGestureLandscapeData.size()); + s_Instance->m_LandscapeMutex.unlock(); +} + + +/// @brief This gets called when receiving a system message from Core. +/// @param p_SystemMessage contains the system message received from core. +void SDKClient::OnSystemCallback(const SystemMessage* const p_SystemMessage) +{ + if (s_Instance) + { + s_Instance->m_SystemMessageMutex.lock(); + + switch (p_SystemMessage->type) + { + case SystemMessageType::SystemMessageType_TemporarySkeletonModified: + // if the message was triggered by a temporary skeleton being modified then save the skeleton index, + // this information will be used to get and load the skeleton into core + s_Instance->m_ModifiedSkeletonIndex = p_SystemMessage->infoUInt; + break; + default: + s_Instance->m_SystemMessageCode = p_SystemMessage->type; + s_Instance->m_SystemMessage = p_SystemMessage->infoString; + break; + } + s_Instance->m_SystemMessageMutex.unlock(); + } +} + +/// @brief This gets called when receiving ergonomics data from Manus Core +/// In our sample we only save the first left and first right glove's latests ergonomics data. +/// Ergonomics data gets generated and sent when glove data changes, this means that the stream +/// does not always contain ALL of the devices, because some may not have had new data since +/// the last time the ergonomics data was sent. +/// @param p_Ergo contains the ergonomics data for each glove connected to Core. +void SDKClient::OnErgonomicsCallback(const ErgonomicsStream* const p_Ergo) +{ + if (s_Instance) + { + long int left_glove_id = 0; + long int right_glove_id = 0; + for (uint32_t i = 0; i < p_Ergo->dataCount; i++) + { + if (p_Ergo->data[i].isUserID)continue; + + ErgonomicsData* t_Ergo = nullptr; + if (std::find(idleft.begin(), idleft.end(), p_Ergo->data[i].id) != idleft.end()) + { + t_Ergo = &s_Instance->m_LeftGloveErgoData; + left_glove_id = p_Ergo->data[i].id; + } + if (std::find(idright.begin(), idright.end(), p_Ergo->data[i].id) != idright.end()) + { + t_Ergo = &s_Instance->m_RightGloveErgoData; + right_glove_id = p_Ergo->data[i].id; + } + if (t_Ergo == nullptr)continue; + CoreSdk_GetTimestampInfo(p_Ergo->publishTime, &s_Instance->m_ErgoTimestampInfo); + t_Ergo->id = p_Ergo->data[i].id; + t_Ergo->isUserID = p_Ergo->data[i].isUserID; + // Create a vector to store the angles of the hand + std::vector orientations_left; + std::vector orientations_right; + for (int j = 0; j < ErgonomicsDataType::ErgonomicsDataType_MAX_SIZE; j++) + { + t_Ergo->data[j] = p_Ergo->data[i].data[j]; + double roundedValue = s_Instance->RoundFloatValue(t_Ergo->data[j], 2); + if (t_Ergo == &s_Instance->m_LeftGloveErgoData) + { + orientations_left.push_back(roundedValue); + } + else if (t_Ergo == &s_Instance->m_RightGloveErgoData) + { + orientations_right.push_back(roundedValue); + } + } + if(left_glove_id!=0){ + output_map[std::to_string(left_glove_id)+"_angle"] = orientations_left; + } + if(right_glove_id!=0){ + output_map[std::to_string(right_glove_id)+"_angle"] = orientations_right; + } + } + } +} + +/// @brief Round the given float value so that it has no more than the given number of decimals. +float SDKClient::RoundFloatValue(float p_Value, int p_NumDecimalsToKeep) +{ + // Since C++11, powf is supposed to be declared in . + // Unfortunately, gcc decided to be non-compliant on this for no apparent + // reason, so now we have to do this. + // https://stackoverflow.com/questions/5483930/powf-is-not-a-member-of-std + float t_Power = static_cast(std::pow( + 10.0, + static_cast(p_NumDecimalsToKeep))); + return std::round(p_Value * t_Power) / t_Power; +} + +/// @brief Set the position that the next log message will appear at. +/// Using this allows us to have somewhat of a static, yet flexible layout of logging. +void SDKClient::AdvanceConsolePosition(short int p_Y) +{ + if (p_Y < 0) + { + m_ConsoleCurrentOffset = 0; + } + else + { + m_ConsoleCurrentOffset += p_Y; + } + + ApplyConsolePosition(m_ConsoleCurrentOffset); +} + +/// @brief Initialize the sdk, register the callbacks and set the coordinate system. +/// This needs to be done before any of the other SDK functions can be used. +ClientReturnCode SDKClient::InitializeSDK() +{ + // ClientLog::print("Select what mode you would like to start in (and press enter to submit)"); + // ClientLog::print("[1] Core Integrated - This will run standalone without the need for a MANUS Core connection"); + // ClientLog::print("[2] Core Local - This will connect to a MANUS Core running locally on your machine"); + // ClientLog::print("[3] Core Remote - This will search for a MANUS Core running locally on your network"); + // std::string t_ConnectionTypeInput; + // std::cin >> t_ConnectionTypeInput; + + std::string t_ConnectionTypeInput = "1"; + + switch (t_ConnectionTypeInput[0]) + { + case '1': + m_ConnectionType = ConnectionType::ConnectionType_Integrated; + m_State = ClientState::ClientState_ConnectingToCore; + break; + case '2': + m_ConnectionType = ConnectionType::ConnectionType_Local; + m_State = ClientState::ClientState_LookingForHosts; + break; + case '3': + m_ConnectionType = ConnectionType::ConnectionType_Remote; + m_State = ClientState::ClientState_LookingForHosts; + break; + default: + m_ConnectionType = ConnectionType::ConnectionType_Invalid; + m_State = ClientState::ClientState_Starting; + ClientLog::print("Invalid input, try again"); + return InitializeSDK(); + } + + // Invalid connection type detected + if (m_ConnectionType == ConnectionType::ConnectionType_Invalid + || m_ConnectionType == ConnectionType::ClientState_MAX_CLIENT_STATE_SIZE) + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + + // before we can use the SDK, some internal SDK bits need to be initialized. + // however after initializing, the SDK is not yet connected to a host or doing anything network related just yet. + bool t_Remote = m_ConnectionType != ConnectionType::ConnectionType_Integrated; + const SDKReturnCode t_InitializeResult = CoreSdk_Initialize(m_ClientType, t_Remote); + if (t_InitializeResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to initialize the Manus Core SDK. The value returned was {}.", (int32_t)t_InitializeResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + const ClientReturnCode t_CallBackResults = RegisterAllCallbacks(); + if (t_CallBackResults != ::ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::error("Failed to initialize callbacks."); + return t_CallBackResults; + } + + // after everything is registered and initialized as seen above + // we must also set the coordinate system being used for the data in this client. + // (each client can have their own settings. unreal and unity for instance use different coordinate systems) + // if this is not set, the SDK will not connect to any Manus core host. + // The coordinate system used for this example is z-up, x-positive, right-handed and in meter scale. + CoordinateSystemVUH t_VUH; + CoordinateSystemVUH_Init(&t_VUH); + t_VUH.handedness = Side::Side_Right; + t_VUH.up = AxisPolarity::AxisPolarity_PositiveZ; + t_VUH.view = AxisView::AxisView_XFromViewer; + t_VUH.unitScale = 1.0f; //1.0 is meters, 0.01 is cm, 0.001 is mm. + + // The above specified coordinate system is used to initialize and the coordinate space is specified (world/local). + const SDKReturnCode t_CoordinateResult = CoreSdk_InitializeCoordinateSystemWithVUH(t_VUH, true); + + /* this is an example if you want to use the other coordinate system instead of VUH (view, up, handedness) + CoordinateSystemDirection t_Direction; + t_Direction.x = AxisDirection::AD_Right; + t_Direction.y = AxisDirection::AD_Up; + t_Direction.z = AxisDirection::AD_Forward; + const SDKReturnCode t_InitializeResult = CoreSdk_InitializeCoordinateSystemWithDirection(t_Direction, true); + */ + + if (t_CoordinateResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to initialize the Manus Core SDK coordinate system. The value returned was {}.", (int32_t)t_InitializeResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief Used to restart and initialize the SDK to make sure a new connection can be set up. +/// This function is used by the client to shutdown the SDK and any connections it has. +/// After that it reinitializes the SDK so that it is ready to connect to a new Manus Core. +ClientReturnCode SDKClient::RestartSDK() +{ + const SDKReturnCode t_ShutDownResult = CoreSdk_ShutDown(); + if (t_ShutDownResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to shutdown the SDK. The value returned was {}.", (int32_t)t_ShutDownResult); + return ClientReturnCode::ClientReturnCode_FailedToShutDownSDK; + } + + const ClientReturnCode t_IntializeResult = InitializeSDK(); + if (t_IntializeResult != ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::error("Failed to initialize the SDK functionality. The value returned was {}.", (int32_t)t_IntializeResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief Used to register the callbacks between sdk and core. +/// Callbacks that are registered functions that get called when a certain 'event' happens, such as data coming in from Manus Core. +/// All of these are optional, but depending on what data you require you may or may not need all of them. +ClientReturnCode SDKClient::RegisterAllCallbacks() +{ + // Register the callback for when manus core is connected to the SDK + // it is optional, but helps trigger your client nicely if needed. + // see the function OnConnectedCallback for more details + const SDKReturnCode t_RegisterConnectCallbackResult = CoreSdk_RegisterCallbackForOnConnect(*OnConnectedCallback); + if (t_RegisterConnectCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for after connecting to Manus Core. The value returned was {}.", (int32_t)t_RegisterConnectCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is disconnected to the SDK + // it is optional, but helps trigger your client nicely if needed. + // see OnDisconnectedCallback for more details. + const SDKReturnCode t_RegisterDisconnectCallbackResult = CoreSdk_RegisterCallbackForOnDisconnect(*OnDisconnectedCallback); + if (t_RegisterDisconnectCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for after disconnecting from Manus Core. The value returned was {}.", (int32_t)t_RegisterDisconnectCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for logging from the SDK + // it is optional, but it might help display the logs at the right time/place + const SDKReturnCode t_RegisterLogCallbackResult = CoreSdk_RegisterCallbackForOnLog(*OnLogCallback); + if (t_RegisterLogCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for logging from the SDK. The value returned was {}.", (int32_t)t_RegisterLogCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending Skeleton data + // it is optional, but without it you can not see any resulting skeleton data. + // see OnSkeletonStreamCallback for more details. + const SDKReturnCode t_RegisterSkeletonCallbackResult = CoreSdk_RegisterCallbackForSkeletonStream(*OnSkeletonStreamCallback); + if (t_RegisterSkeletonCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for processing skeletal data from Manus Core. The value returned was {}.", (int32_t)t_RegisterSkeletonCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending landscape data + // it is optional, but this allows for a reactive adjustment of device information. + const SDKReturnCode t_RegisterLandscapeCallbackResult = CoreSdk_RegisterCallbackForLandscapeStream(*OnLandscapeCallback); + if (t_RegisterLandscapeCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback for landscape from Manus Core. The value returned was {}.", (int32_t)t_RegisterLandscapeCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending System messages + // This is usually not used by client applications unless they want to show errors/events from core. + // see OnSystemCallback for more details. + const SDKReturnCode t_RegisterSystemCallbackResult = CoreSdk_RegisterCallbackForSystemStream(*OnSystemCallback); + if (t_RegisterSystemCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for system feedback from Manus Core. The value returned was {}.", (int32_t)t_RegisterSystemCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending Ergonomics data + // it is optional, but helps trigger your client nicely if needed. + // see OnErgonomicsCallback for more details. + const SDKReturnCode t_RegisterErgonomicsCallbackResult = CoreSdk_RegisterCallbackForErgonomicsStream(*OnErgonomicsCallback); + if (t_RegisterErgonomicsCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for ergonomics data from Manus Core. The value returned was {}.", (int32_t)t_RegisterErgonomicsCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending Raw Skeleton data + // it is optional, but without it you can not see any resulting skeleton data. + // see OnSkeletonStreamCallback for more details. + const SDKReturnCode t_RegisterRawSkeletonCallbackResult = CoreSdk_RegisterCallbackForRawSkeletonStream(*OnRawSkeletonStreamCallback); + if (t_RegisterRawSkeletonCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for processing raw skeletal data from Manus Core. The value returned was {}.", (int32_t)t_RegisterRawSkeletonCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending Tracker data + // it is optional, but without it you can not see any resulting tracker data. + // see OnTrackerStreamCallback for more details. + const SDKReturnCode t_RegisterTrackerCallbackResult = CoreSdk_RegisterCallbackForTrackerStream(*OnTrackerStreamCallback); + if (t_RegisterTrackerCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for processing tracker data from Manus Core. The value returned was {}.", (int32_t)t_RegisterTrackerCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + // Register the callback for when manus core is sending Raw Skeleton data + // it is optional, but without it you can not see any resulting skeleton data. + // see OnGestureStreamCallback for more details. + const SDKReturnCode t_RegisterGestureCallbackResult = CoreSdk_RegisterCallbackForGestureStream(*OnGestureStreamCallback); + if (t_RegisterGestureCallbackResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to register callback function for processing gesture data from Manus Core. The value returned was {}.", (int32_t)t_RegisterGestureCallbackResult); + return ClientReturnCode::ClientReturnCode_FailedToInitialize; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief Simple example of the SDK looking for manus core hosts on the network +/// and display them on screen. +ClientReturnCode SDKClient::LookingForHosts() +{ + ClientLog::print("Looking for hosts..."); + + // Underlying function will sleep for m_SecondsToFindHosts to allow servers to reply. + bool t_ConnectLocally = m_ConnectionType == ConnectionType::ConnectionType_Local; + const SDKReturnCode t_StartResult = CoreSdk_LookForHosts(m_SecondsToFindHosts, t_ConnectLocally); + if (t_StartResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to look for hosts. The error given was {}.", (int32_t)t_StartResult); + + return ClientReturnCode::ClientReturnCode_FailedToFindHosts; + } + + m_NumberOfHostsFound = 0; + const SDKReturnCode t_NumberResult = CoreSdk_GetNumberOfAvailableHostsFound(&m_NumberOfHostsFound); + if (t_NumberResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get the number of available hosts. The error given was {}.", (int32_t)t_NumberResult); + + return ClientReturnCode::ClientReturnCode_FailedToFindHosts; + } + + if (m_NumberOfHostsFound == 0) + { + ClientLog::warn("No hosts found."); + m_State = ClientState::ClientState_NoHostsFound; + + return ClientReturnCode::ClientReturnCode_FailedToFindHosts; + } + + m_AvailableHosts.reset(new ManusHost[m_NumberOfHostsFound]); + const SDKReturnCode t_HostsResult = CoreSdk_GetAvailableHostsFound(m_AvailableHosts.get(), m_NumberOfHostsFound); + if (t_HostsResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get the available hosts. The error given was {}.", (int32_t)t_HostsResult); + + return ClientReturnCode::ClientReturnCode_FailedToFindHosts; + } + + if (t_ConnectLocally) + { + m_State = ClientState::ClientState_ConnectingToCore; + return ClientReturnCode::ClientReturnCode_Success; + } + + m_State = ClientState::ClientState_PickingHost; + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief When no available hosts are found the user can either retry or exit. +ClientReturnCode SDKClient::NoHostsFound() +{ + if (m_ConsoleClearTickCount == 0) + { + AdvanceConsolePosition(-1); + ClientLog::print("No hosts were found. Retry?"); + ClientLog::print("[R] retry"); + ClientLog::print("[ESC] exit"); + } + + if (GetKeyDown('R')) + { + ClientLog::print("Retrying."); + + m_State = ClientState::ClientState_LookingForHosts; + } + + // Note: escape is handled by default below. + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief Print the found hosts and give the user the option to select one. +ClientReturnCode SDKClient::PickingHost() +{ + if (m_ConsoleClearTickCount == 0) + { + AdvanceConsolePosition(-1); + + ClientLog::print("[R] retry [ESC] exit"); + ClientLog::print("Pick a host to connect to."); + ClientLog::print("Found the following hosts:"); + + // Note: only 10 hosts are shown, to match the number of number keys, for easy selection. + for (unsigned int t_HostNumber = 0; t_HostNumber < 10 && t_HostNumber < m_NumberOfHostsFound; t_HostNumber++) + { + ClientLog::print( + "[{}] hostname \"{}\", IP address \"{}\" Version {}.{}.{}", + t_HostNumber, + m_AvailableHosts[t_HostNumber].hostName, + m_AvailableHosts[t_HostNumber].ipAddress, + m_AvailableHosts[t_HostNumber].manusCoreVersion.major, + m_AvailableHosts[t_HostNumber].manusCoreVersion.minor, + m_AvailableHosts[t_HostNumber].manusCoreVersion.patch); + } + } + + for (unsigned int t_HostNumber = 0; t_HostNumber < 10 && t_HostNumber < m_NumberOfHostsFound; t_HostNumber++) + { + if (GetKeyDown('0' + t_HostNumber)) + { + ClientLog::print("Selected host {}.", t_HostNumber); + + m_HostToConnectTo = t_HostNumber; + m_State = ClientState::ClientState_ConnectingToCore; + + break; + } + } + + if (GetKeyDown('R')) + { + ClientLog::print("Retrying."); + m_State = ClientState::ClientState_LookingForHosts; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief After a connection option was selected, the client will now try to connect to manus core via the SDK. +ClientReturnCode SDKClient::ConnectingToCore() +{ + SDKReturnCode t_ConnectResult = SDKReturnCode::SDKReturnCode_Error; + switch (m_ConnectionType) + { + case ConnectionType::ConnectionType_Integrated: + { + ManusHost t_Empty; + ManusHost_Init(&t_Empty); + t_ConnectResult = CoreSdk_ConnectToHost(t_Empty); + break; + } + case ConnectionType::ConnectionType_Local: + { + t_ConnectResult = CoreSdk_ConnectToHost(m_AvailableHosts[0]); + break; + } + case ConnectionType::ConnectionType_Remote: + { + t_ConnectResult = CoreSdk_ConnectToHost(m_AvailableHosts[m_HostToConnectTo]); + break; + } + } + + if (t_ConnectResult == SDKReturnCode::SDKReturnCode_NotConnected) + { + m_State = ClientState::ClientState_NoHostsFound; + + return ClientReturnCode::ClientReturnCode_Success; // Differentiating between error and no connect + } + if (t_ConnectResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to connect to Core. The error given was {}.", (int32_t)t_ConnectResult); + + return ClientReturnCode::ClientReturnCode_FailedToConnect; + } + + m_State = ClientState::ClientState_DisplayingData; + + // Note: a log message from somewhere in the SDK during the connection process can cause text + // to permanently turn green after this step. Adding a sleep here of 2+ seconds "fixes" the + // issue. It seems to be caused by a threading issue somewhere, resulting in a log call being + // interrupted while it is printing the green [info] text. The log output then gets stuck in + // green mode. + + return ClientReturnCode::ClientReturnCode_Success; +} + + +/// @brief Some things happen before every display update, no matter what state. +/// They happen here, such as the updating of the landscape and the generated tracker +/// @return +ClientReturnCode SDKClient::UpdateBeforeDisplayingData() +{ + AdvanceConsolePosition(-1); + + m_SkeletonMutex.lock(); + if (m_NextSkeleton != nullptr) + { + if (m_Skeleton != nullptr)delete m_Skeleton; + m_Skeleton = m_NextSkeleton; + m_NextSkeleton = nullptr; + } + m_SkeletonMutex.unlock(); + + m_RawSkeletonMutex.lock(); + if (m_NextRawSkeleton != nullptr) + { + if (m_RawSkeleton != nullptr)delete m_RawSkeleton; + m_RawSkeleton = m_NextRawSkeleton; + m_NextRawSkeleton = nullptr; + } + m_RawSkeletonMutex.unlock(); + + m_TrackerMutex.lock(); + if (m_NextTrackerData != nullptr) + { + if (m_TrackerData != nullptr)delete m_TrackerData; + m_TrackerData = m_NextTrackerData; + m_NextTrackerData = nullptr; + } + m_TrackerMutex.unlock(); + + m_GestureMutex.lock(); + if (m_NewFirstLeftGloveGestures != nullptr) + { + if (m_FirstLeftGloveGestures != nullptr) delete m_FirstLeftGloveGestures; + m_FirstLeftGloveGestures = m_NewFirstLeftGloveGestures; + m_NewFirstLeftGloveGestures = nullptr; + } + if (m_NewFirstRightGloveGestures != nullptr) + { + if (m_FirstRightGloveGestures != nullptr) delete m_FirstRightGloveGestures; + m_FirstRightGloveGestures = m_NewFirstRightGloveGestures; + m_NewFirstRightGloveGestures = nullptr; + } + m_GestureMutex.unlock(); + + m_LandscapeMutex.lock(); + if (m_NewLandscape != nullptr) + { + if (m_Landscape != nullptr) + { + delete m_Landscape; + } + m_Landscape = m_NewLandscape; + m_NewLandscape = nullptr; + m_GestureLandscapeData.swap(m_NewGestureLandscapeData); + } + m_LandscapeMutex.unlock(); + + m_FirstLeftGloveID = 0; + m_FirstRightGloveID = 0; + if (m_Landscape == nullptr)return ClientReturnCode::ClientReturnCode_Success; + for (size_t i = 0; i < m_Landscape->gloveDevices.gloveCount; i++) + { + if (m_FirstLeftGloveID == 0 && m_Landscape->gloveDevices.gloves[i].side == Side::Side_Left) + { + m_FirstLeftGloveID = m_Landscape->gloveDevices.gloves[i].id; + continue; + } + if (m_FirstRightGloveID == 0 && m_Landscape->gloveDevices.gloves[i].side == Side::Side_Right) + { + m_FirstRightGloveID = m_Landscape->gloveDevices.gloves[i].id; + continue; + } + } + + return ClientReturnCode::ClientReturnCode_Success; +} + + +/// @brief Once the connections are made we loop this function +/// it calls all the input handlers for different aspects of the SDK +/// and then prints any relevant data of it. +ClientReturnCode SDKClient::DisplayingData() +{ + ClientLog::print("<
> [ESC] quit"); + ClientLog::print("[G] Go To Gloves & Dongle Menu"); + ClientLog::print("[S] Go To Skeleton Menu"); + ClientLog::print("[X] Go To Temporary Skeleton Menu"); + ClientLog::print("[T] Go To Tracker Menu"); + ClientLog::print("[D] Go To Landscape Time Info"); + ClientLog::print("[J] Go To Gestures Menu"); + ClientLog::print("[C] Go To Glove Calibration Menu"); + ClientLog::print("[P] Go To Pairing Menu"); + + AdvanceConsolePosition(10); + + GO_TO_DISPLAY('G', DisplayingDataGlove) + GO_TO_DISPLAY('S', DisplayingDataSkeleton) + GO_TO_DISPLAY('X', DisplayingDataTemporarySkeleton) + GO_TO_DISPLAY('T', DisplayingDataTracker) + GO_TO_DISPLAY('D', DisplayingLandscapeTimeData) + GO_TO_DISPLAY('J', DisplayingDataGestures) + GO_TO_DISPLAY('C', DisplayingGloveCalibration) + GO_TO_DISPLAY('P', DisplayingPairing) + + PrintSystemMessage(); + + // AdvanceConsolePosition(10); + // GO_TO_DISPLAY('G', DisplayingDataGlove) + // PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief display the ergonomics data of the gloves, and handles haptic commands. +/// @return +ClientReturnCode SDKClient::DisplayingDataGlove() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("Haptic keys: left:([1]-[5] = pinky-thumb.) right:([6]-[0] = thumb-pinky.)"); + + AdvanceConsolePosition(3); + + GO_TO_MENU_IF_REQUESTED() + + HandleHapticCommands(); + + PrintErgonomicsData(); + PrintDongleData(); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingDataSkeleton() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print(" [B] Load Left Hand Skeleton [N] Load Right Hand Skeleton [M] Unload Skeleton"); + ClientLog::print(" [D] Toggle Send to DevTools ({})", m_SendToDevTools); + ClientLog::print(" left:([1]-[5] = pinky-thumb) right:([6]-[0] = thumb-pinky)"); + + AdvanceConsolePosition(4); + + GO_TO_MENU_IF_REQUESTED() + + HandleSkeletonCommands(); + HandleSkeletonHapticCommands(); + + PrintSkeletonData(); + + AdvanceConsolePosition(1); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingDataTracker() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("[O] Toggle Test Tracker [G] Toggle per user tracker display [T] Set Tracker Offset to Left Wrist"); + + AdvanceConsolePosition(3); + + GO_TO_MENU_IF_REQUESTED() + HandleTrackerCommands(); + + PrintRawSkeletonData(); + + PrintTrackerData(); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingDataTemporarySkeleton() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("[A] Auto allocate chains and load skeleton"); + ClientLog::print("[B] Build Temporary Skeleton [C] Clear Temporary Skeleton [D] Clear All Temporary Skeletons For The Current Session"); + ClientLog::print("[E] Save Temporary Skeleton To File, [F] Get Temporary Skeleton From File"); + + AdvanceConsolePosition(5); + + GO_TO_MENU_IF_REQUESTED() + + HandleTemporarySkeletonCommands(); + + PrintTemporarySkeletonInfo(); + GetTemporarySkeletonIfModified(); + + AdvanceConsolePosition(3); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingLandscapeTimeData() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + + AdvanceConsolePosition(2); + + GO_TO_MENU_IF_REQUESTED() + + PrintLandscapeTimeData(); + + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingDataGestures() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("[H] Show other Hand"); + + AdvanceConsolePosition(3); + + GO_TO_MENU_IF_REQUESTED() + + HandleGesturesCommands(); + + PrintGestureData(); + + AdvanceConsolePosition(1); + + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingGloveCalibration() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("[H] Toggle other Hand [P] Add 1 to the calibration step [M] Remove 1 from the calibration step"); + ClientLog::print("[S] Start glove calibration [C] Cancel glove calibration [F] Finish glove calibration"); + ClientLog::print("[E] Execute calibration step"); + + AdvanceConsolePosition(5); + + GO_TO_MENU_IF_REQUESTED() + + HandleGloveCalibrationCommands(); + + PrintGloveCalibrationData(); + + AdvanceConsolePosition(3); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +ClientReturnCode SDKClient::DisplayingPairing() +{ + ClientLog::print("[Q] Back <> [ESC] quit"); + ClientLog::print("<> [P] Pair first available Glove"); + ClientLog::print("<> [U] Unpair first available Glove"); + + + GO_TO_MENU_IF_REQUESTED() + + HandlePairingCommands(); + + AdvanceConsolePosition(4); + PrintSystemMessage(); + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief When the SDK loses the connection with Core the user can either +/// close the sdk or try to reconnect to a local or to a remote host. +ClientReturnCode SDKClient::DisconnectedFromCore() +{ + if (m_Host == nullptr) { return ClientReturnCode::ClientReturnCode_FailedToConnect; } + + AdvanceConsolePosition(-1); + + auto t_Duration = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - m_TimeSinceLastDisconnect).count(); + ClientLog::print("The SDK lost connection with Manus Core {} seconds ago.", t_Duration); + ClientLog::print("[P] Pick a new host. [ESC] exit"); + + AdvanceConsolePosition(3); + + bool t_ConnectLocally = m_ConnectionType == ConnectionType::ConnectionType_Local; + if (t_ConnectLocally) + { + ClientLog::print("Automatically trying to reconnect to local host."); + + ClientReturnCode t_ReconnectResult = ReconnectingToCore(); + if (t_ReconnectResult != ClientReturnCode::ClientReturnCode_FailedToConnect) + { + return t_ReconnectResult; + } + } + else + { + ClientLog::print("[R] Try to reconnect to the last host {} at {}.", m_Host->hostName, m_Host->ipAddress); + if (GetKeyDown('R')) + { + ClientLog::print("Reconnecting"); + + ClientReturnCode t_ReconnectResult = ReconnectingToCore(m_SecondsToAttemptReconnecting, m_MaxReconnectionAttempts); + if (t_ReconnectResult != ClientReturnCode::ClientReturnCode_FailedToConnect) + { + return t_ReconnectResult; + } + } + } + + AdvanceConsolePosition(10); + + + if (GetKeyDown('P')) + { + ClientLog::print("Picking new host."); + + // Restarting and initializing CoreConnection to make sure a new connection can be set up + const ClientReturnCode t_RestartResult = RestartSDK(); + if (t_RestartResult != ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::error("Failed to Restart CoreConnection."); + return ClientReturnCode::ClientReturnCode_FailedToRestart; + } + + m_State = ClientState::ClientState_LookingForHosts; + } + + return ClientReturnCode::ClientReturnCode_Success; +} + +/// @brief It is called when the sdk is disconnected from Core and the user select one of the options to reconnect. +ClientReturnCode SDKClient::ReconnectingToCore(int32_t p_ReconnectionTime, int32_t p_ReconnectionAttempts) +{ + if (p_ReconnectionTime <= 0) { p_ReconnectionTime = std::numeric_limits::max(); } + if (p_ReconnectionAttempts <= 0) { p_ReconnectionAttempts = std::numeric_limits::max(); } + + // Restarting and initializing CoreConnection to make sure a new connection can be set up + const ClientReturnCode t_RestartResult = RestartSDK(); + if (t_RestartResult != ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::error("Failed to Restart CoreConnection."); + return ClientReturnCode::ClientReturnCode_FailedToRestart; + } + + std::chrono::high_resolution_clock::time_point t_Start = std::chrono::high_resolution_clock::now(); + int t_Attempt = 0; + while ((p_ReconnectionAttempts > 0) && (p_ReconnectionTime > 0)) + { + ClientLog::print("Trying to reconnect to {} at {}. Attempt {}.", m_Host->hostName, m_Host->ipAddress, t_Attempt); + ClientLog::print("Attempts remaining: {}. Seconds before time out: {}.", p_ReconnectionAttempts, p_ReconnectionTime); + + bool t_ConnectLocally = m_ConnectionType == ConnectionType::ConnectionType_Local; + if (t_ConnectLocally) + { + ClientReturnCode t_ConnectionResult = LookingForHosts(); + if (t_ConnectionResult == ClientReturnCode::ClientReturnCode_Success) + { + ClientLog::print("Reconnected to ManusCore."); + return ClientReturnCode::ClientReturnCode_Success; + } + } + else + { + SDKReturnCode t_ConnectionResult = CoreSdk_ConnectToHost(*m_Host.get()); + if (t_ConnectionResult == SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::print("Reconnected to ManusCore."); + return ClientReturnCode::ClientReturnCode_Success; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(m_SleepBetweenReconnectingAttemptsInMs)); + p_ReconnectionTime -= static_cast(std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - t_Start).count()); + --p_ReconnectionAttempts; + ++t_Attempt; + } + + ClientLog::print("Failed to reconnect to ManusCore."); + m_State = ClientState::ClientState_Disconnected; + return ClientReturnCode::ClientReturnCode_FailedToConnect; +} + + +/// @brief Prints the ergonomics data of a hand. +/// @param p_ErgoData +void SDKClient::PrintHandErgoData(ErgonomicsData& p_ErgoData, bool p_Left) +{ + const std::string t_FingerNames[NUM_FINGERS_ON_HAND] = { "[thumb] ", "[index] ", "[middle]", "[ring] ", "[pinky] " }; + const std::string t_FingerJointNames[NUM_FINGERS_ON_HAND] = { "mcp", "pip", "dip" }; + const std::string t_ThumbJointNames[NUM_FINGERS_ON_HAND] = { "cmc", "mcp", "ip " }; + + int t_DataOffset = 0; + if (!p_Left)t_DataOffset = 20; + + const std::string* t_JointNames = t_ThumbJointNames; + for (unsigned int t_FingerNumber = 0; t_FingerNumber < NUM_FINGERS_ON_HAND; t_FingerNumber++) + { + ClientLog::printWithPadding("{} {} spread: {}, {} stretch: {}, {} stretch: {}, {} stretch: {} ", 6, + t_FingerNames[t_FingerNumber], // Name of the finger. + t_JointNames[0], + RoundFloatValue(p_ErgoData.data[t_DataOffset], 2), + t_JointNames[0], + RoundFloatValue(p_ErgoData.data[t_DataOffset + 1], 2), + t_JointNames[1], + RoundFloatValue(p_ErgoData.data[t_DataOffset + 2], 2), + t_JointNames[2], + RoundFloatValue(p_ErgoData.data[t_DataOffset + 3], 2)); + t_JointNames = t_FingerJointNames; + t_DataOffset += 4; + } +} + +/// @brief Print the ergonomics data received from Core. +void SDKClient::PrintErgonomicsData() +{ + // for testing purposes we only look at the first 2 gloves available + ClientLog::print(" -- Ergo Timestamp {}:{}:{}.{} ~ {}/{}/{}(D/M/Y)", + m_ErgoTimestampInfo.hour, m_ErgoTimestampInfo.minute, m_ErgoTimestampInfo.second, m_ErgoTimestampInfo.fraction, + m_ErgoTimestampInfo.day, m_ErgoTimestampInfo.month, std::to_string(m_ErgoTimestampInfo.year)); + ClientLog::print(" -- Left Glove -- 0x{} - Angles in degrees", m_FirstLeftGloveID); + if (m_LeftGloveErgoData.id == m_FirstLeftGloveID) + { + PrintHandErgoData(m_LeftGloveErgoData, true); + } + else + { + ClientLog::print(" ...No Data..."); + } + ClientLog::print(" -- Right Glove -- 0x{} - Angles in degrees", m_FirstRightGloveID); + if (m_RightGloveErgoData.id == m_FirstRightGloveID) + { + PrintHandErgoData(m_RightGloveErgoData, false); + } + else + { + ClientLog::print(" ...No Data..."); + } + + AdvanceConsolePosition(14); +} + +std::string ConvertDeviceClassTypeToString(DeviceClassType p_Type) +{ + switch (p_Type) + { + case DeviceClassType_Dongle: + return "Dongle"; + case DeviceClassType_Glove: + return "Glove"; + case DeviceClassType_Glongle: + return "Glongle (Glove Dongle)"; + default: + return "Unknown"; + } +} + +std::string ConvertDeviceFamilyTypeToString(DeviceFamilyType p_Type) +{ + switch (p_Type) + { + case DeviceFamilyType_Prime1: + return "Prime 1"; + case DeviceFamilyType_Prime2: + return "Prime 2"; + case DeviceFamilyType_PrimeX: + return "Prime X"; + case DeviceFamilyType_Metaglove: + return "Metaglove"; + case DeviceFamilyType_Prime3: + return "Prime 3"; + case DeviceFamilyType_Virtual: + return "Virtual"; + case DeviceFamilyType_MetaglovePro: + return "Metaglove Pro"; + default: + return "Unknown"; + } +} + +/// @brief Print the ergonomics data received from Core. +void SDKClient::PrintDongleData() +{ + // get a dongle id + uint32_t t_DongleCount = m_Landscape->gloveDevices.dongleCount; + if (t_DongleCount == 0) return; // we got no gloves to work on anyway! + + DongleLandscapeData t_DongleData; + + for (uint32_t i = 0; i < t_DongleCount; i++) + { + t_DongleData = m_Landscape->gloveDevices.dongles[i]; + ClientLog::print(" -- Dongle -- 0x{}", t_DongleData.id); + + ClientLog::print(" Type: {} - {}", + ConvertDeviceClassTypeToString(t_DongleData.classType), + ConvertDeviceFamilyTypeToString(t_DongleData.familyType)); + ClientLog::print(" License: {}", t_DongleData.licenseType); + + AdvanceConsolePosition(4); + } +} + +/// @brief Prints the last received system messages received from Core. +void SDKClient::PrintSystemMessage() +{ + m_SystemMessageMutex.lock(); + ClientLog::print(""); //blank line + ClientLog::print("Received System data:{} / code:{}", m_SystemMessage, (int32_t)m_SystemMessageCode); + m_SystemMessageMutex.unlock(); +} + +/// @brief Prints the finalized skeleton data received from Core. +/// Since our console cannot render this data visually in 3d (its not in the scope of this client) +/// we only display a little bit of data of the skeletons we receive. +void SDKClient::PrintSkeletonData() +{ + if (m_Skeleton == nullptr || m_Skeleton->skeletons.size() == 0) + { + return; + } + + ClientLog::print("Received Skeleton data. skeletons:{} first skeleton id:{}", m_Skeleton->skeletons.size(), (int32_t)m_Skeleton->skeletons[0].info.id); + + AdvanceConsolePosition(2); +} + +void SDKClient::PrintRawSkeletonData() +{ + if (m_RawSkeleton == nullptr || m_RawSkeleton->skeletons.size() == 0) + { + return; + } + ClientLog::print("Received Skeleton glove data from Core. skeletons:{} first skeleton glove id:{}", m_RawSkeleton->skeletons.size(), m_RawSkeleton->skeletons[0].info.gloveId); + + if (m_FirstLeftGloveID == 0 && m_FirstRightGloveID == 0) return; // no gloves connected to core + + uint32_t t_NodeCount = 0; + uint32_t t_GloveId; + if (m_FirstLeftGloveID != 0) + { + t_GloveId = m_FirstLeftGloveID; + } + else + { + t_GloveId = m_FirstRightGloveID; + } + // we need to know the number of nodes in the hierarchy, so we can initialize an array with the correct size. + SDKReturnCode t_Result = CoreSdk_GetRawSkeletonNodeCount(t_GloveId, t_NodeCount); + if (t_Result != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get Raw Skeleton Node Count. The error given was {}.", (int32_t)t_Result); + return; + } + + // now get the hierarchy data, this can be used to reconstruct the positions of each node in case the user set up the system with a local coordinate system, and to know what each node is exactly. + // having a node position defined as local means that this will be related to its parent + NodeInfo* t_NodeInfo = new NodeInfo[t_NodeCount]; + t_Result = CoreSdk_GetRawSkeletonNodeInfoArray(t_GloveId, t_NodeInfo, t_NodeCount); + if (t_Result != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get Raw Skeleton Hierarchy. The error given was {}.", (int32_t)t_Result); + return; + } + + ClientLog::print("Fetched array with Raw Skeleton Hierarchy with {} nodes.", (int32_t)t_NodeCount); + AdvanceConsolePosition(2); +} + +/// @brief Prints the tracker data +/// Since our console cannot render this data visually in 3d (its not in the scope of this client) +/// we only display a little bit of data of the trackers we receive. +void SDKClient::PrintTrackerData() +{ + ClientLog::print("Tracker test active: {}.", m_TrackerTest); //To show that test tracker is being sent to core + ClientLog::print("Per user tracker display: {}.", m_TrackerDataDisplayPerUser); + + AdvanceConsolePosition(2); + + if (m_TrackerDataDisplayPerUser) + { + PrintTrackerDataPerUser(); + AdvanceConsolePosition(10); + } + else + { + PrintTrackerDataGlobal(); + AdvanceConsolePosition(3); + } + + // now, as a test, print the tracker data received from the stream + if (m_TrackerData == nullptr || m_TrackerData->trackerData.size() == 0) + { + return; + } + + ClientLog::print("Received Tracker data. number of received trackers:{} first tracker type:{}", m_TrackerData->trackerData.size(), (int32_t)m_TrackerData->trackerData[0].trackerType); +} + +/// @brief Prints the tracker data without taking users into account. +/// This shows how one can get the tracker data that is being streamed without caring about users. +void SDKClient::PrintTrackerDataGlobal() +{ + uint32_t t_NumberOfAvailabletrackers = 0; + SDKReturnCode t_TrackerResult = CoreSdk_GetNumberOfAvailableTrackers(&t_NumberOfAvailabletrackers); + if (t_TrackerResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get tracker data. The error given was {}.", (int32_t)t_TrackerResult); + return; + } + + ClientLog::print("received available trackers :{} ", t_NumberOfAvailabletrackers); + + if (t_NumberOfAvailabletrackers == 0) return; // nothing else to do. + TrackerId* t_TrackerId = new TrackerId[t_NumberOfAvailabletrackers]; + t_TrackerResult = CoreSdk_GetIdsOfAvailableTrackers(t_TrackerId, t_NumberOfAvailabletrackers); + if (t_TrackerResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get tracker data. The error given was {}.", (int32_t)t_TrackerResult); + return; + } +} + +/// @brief Prints the tracker data per user, this shows how to access this data for each user. +void SDKClient::PrintTrackerDataPerUser() +{ + uint32_t t_NumberOfAvailableUsers = 0; + SDKReturnCode t_UserResult = CoreSdk_GetNumberOfAvailableUsers(&t_NumberOfAvailableUsers); + if (t_UserResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get user count. The error given was {}.", (int32_t)t_UserResult); + return; + } + if (t_NumberOfAvailableUsers == 0) return; // nothing to get yet + + + for (uint32_t i = 0; i < t_NumberOfAvailableUsers; i++) + { + uint32_t t_NumberOfAvailabletrackers = 0; + SDKReturnCode t_TrackerResult = CoreSdk_GetNumberOfAvailableTrackersForUserIndex(&t_NumberOfAvailabletrackers, i); + if (t_TrackerResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get tracker data. The error given was {}.", (int32_t)t_TrackerResult); + return; + } + + if (t_NumberOfAvailabletrackers == 0) continue; + + ClientLog::print("received available trackers for user index[{}] :{} ", i, t_NumberOfAvailabletrackers); + + if (t_NumberOfAvailabletrackers == 0) return; // nothing else to do. + TrackerId* t_TrackerId = new TrackerId[t_NumberOfAvailabletrackers]; + t_TrackerResult = CoreSdk_GetIdsOfAvailableTrackersForUserIndex(t_TrackerId, i, t_NumberOfAvailabletrackers); + if (t_TrackerResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get tracker data. The error given was {}.", (int32_t)t_TrackerResult); + return; + } + } +} + +std::string GetFPSEnumName(TimecodeFPS p_FPS) +{ + switch (p_FPS) + { + case TimecodeFPS::TimecodeFPS_23_976: + return "23.976 FPS (24 dropframe)"; + case TimecodeFPS::TimecodeFPS_24: + return "24 FPS"; + case TimecodeFPS::TimecodeFPS_25: + return "25 FPS"; + case TimecodeFPS::TimecodeFPS_29_97: + return "29.97 FPS (30 dropframe)"; + case TimecodeFPS::TimecodeFPS_30: + return "30 FPS"; + case TimecodeFPS::TimecodeFPS_50: + return "50 FPS"; + case TimecodeFPS::TimecodeFPS_59_94: + return "59.94 FPS (60 dropframe)"; + case TimecodeFPS::TimecodeFPS_60: + return "60 FPS"; + default: + return "Undefined FPS"; + } +} + +void SDKClient::PrintLandscapeTimeData() +{ + ClientLog::print("Total count of Interfaces: {}", m_Landscape->time.interfaceCount); + ClientLog::print("Current Interface: {} {} at index {}", m_Landscape->time.currentInterface.name, m_Landscape->time.currentInterface.api, m_Landscape->time.currentInterface.index); + + ClientLog::print("FPS: {}", GetFPSEnumName(m_Landscape->time.fps)); + ClientLog::print("Fake signal: {} | Sync Pulse: {} | Sync Status: {}", m_Landscape->time.fakeTimecode, m_Landscape->time.useSyncPulse, m_Landscape->time.syncStatus); + ClientLog::print("Device keep alive: {} | Timecode Status: {}", m_Landscape->time.deviceKeepAlive, m_Landscape->time.timecodeStatus); + + AdvanceConsolePosition(6); +} + +void SDKClient::PrintGestureData() +{ + ClientGestures* t_Gest = m_FirstLeftGloveGestures; + std::string t_Side = "Left"; + if (!m_ShowLeftGestures) + { + t_Side = "Right"; + t_Gest = m_FirstRightGloveGestures; + } + + if (t_Gest == nullptr) + { + ClientLog::print("No Gesture information for first {} glove.", t_Side); + AdvanceConsolePosition(1); + return; + } + ClientLog::print("Total count of gestures for the {} glove: {}", t_Side, t_Gest->info.totalGestureCount); + uint32_t t_Max = t_Gest->info.totalGestureCount; + if (t_Max > 20) t_Max = 20; + ClientLog::print("Showing result of first {} gestures.", t_Max); + for (uint32_t i = 0; i < t_Max; i++) + { + char* t_Name = ""; + for (uint32_t g = 0; g < m_GestureLandscapeData.size(); g++) + { + if (m_GestureLandscapeData[g].id == t_Gest->probabilities[i].id) + { + t_Name = m_GestureLandscapeData[g].name; + } + } + ClientLog::print("Gesture {} ({}) has a probability of {}%.", t_Name, t_Gest->probabilities[i].id, t_Gest->probabilities[i].percent * 100.0f); + } + + AdvanceConsolePosition(t_Max + 2); +} + +void SDKClient::PrintGloveCalibrationData() +{ + std::string t_Side = "Left"; + if (!m_CalibrateLeftHand) + { + t_Side = "Right"; + } + ClientLog::print("Calibrating: {} - Step {} ", t_Side, m_CalibrationStep); + ClientLog::print("Glove: {}", m_CalibrationGloveId); + ClientLog::print("Number of steps: {}", m_NumberOfCalibrationSteps); + ClientLog::print("Title: {}", m_StepData.title); + ClientLog::print("Description: {}", m_StepData.description); + ClientLog::print("Time: {}", m_StepData.time); + ClientLog::print("In progress: {}", m_IsCalibrationInProgress); + ClientLog::print("Message: {}", m_CalibrationMessage); + + AdvanceConsolePosition(6); +} + +/// @brief Prints the type of the first chain generated by the AllocateChain function, this is used for testing. +void SDKClient::PrintSkeletonInfo() +{ + switch (m_ChainType) + { + case ChainType::ChainType_FingerIndex: + { + ClientLog::print("received Skeleton chain type: ChainType_FingerIndex"); + break; + } + case ChainType::ChainType_FingerMiddle: + { + ClientLog::print("received Skeleton chain type: ChainType_FingerMiddle"); + break; + } + case ChainType::ChainType_FingerPinky: + { + ClientLog::print("received Skeleton chain type: ChainType_FingerPinky"); + break; + } + case ChainType::ChainType_FingerRing: + { + ClientLog::print("received Skeleton chain type: ChainType_FingerRing"); + break; + } + case ChainType::ChainType_FingerThumb: + { + ClientLog::print("received Skeleton chain type: ChainType_FingerThumb"); + break; + } + case ChainType::ChainType_Hand: + { + ClientLog::print("received Skeleton chain type: ChainType_Hand"); + break; + } + case ChainType::ChainType_Head: + { + ClientLog::print("received Skeleton chain type: ChainType_Head"); + break; + } + case ChainType::ChainType_Leg: + { + ClientLog::print("received Skeleton chain type: ChainType_Leg"); + break; + } + case ChainType::ChainType_Neck: + { + ClientLog::print("received Skeleton chain type: ChainType_Neck"); + break; + } + case ChainType::ChainType_Pelvis: + { + ClientLog::print("received Skeleton chain type: ChainType_Pelvis"); + break; + } + case ChainType::ChainType_Shoulder: + { + ClientLog::print("received Skeleton chain type: ChainType_Shoulder"); + break; + } + case ChainType::ChainType_Spine: + { + ClientLog::print("received Skeleton chain type: ChainType_Spine"); + break; + } + case ChainType::ChainType_Arm: + { + ClientLog::print("received Skeleton chain type: ChainType_Arm"); + break; + } + case ChainType_Foot: + { + ClientLog::print("received Skeleton chain type: ChainType_Foot"); + break; + } + case ChainType_Toe: + { + ClientLog::print("received Skeleton chain type: ChainType_Toe"); + break; + } + case ChainType::ChainType_Invalid: + default: + { + ClientLog::print("received Skeleton chain type: ChainType_Invalid"); + break; + } + } + AdvanceConsolePosition(2); +} + +/// @brief This support function checks if a temporary skeleton related to the current session has been modified and gets it. +/// Whenever the Dev Tools saves the changes to the skeleton, it sets the boolean t_IsSkeletonModified to true for that skeleton. +/// With callback OnSystemCallback this application can be notified when one of its temporary skeletons has been modified, so it can get it and, potentially, load it. +void SDKClient::GetTemporarySkeletonIfModified() +{ + // if a temporary skeleton associated to the current session has been modified we can get it and, potentially, load it + if (m_ModifiedSkeletonIndex != UINT_MAX) + { + // get the temporary skeleton + uint32_t t_SessionId = m_SessionId; + SDKReturnCode t_Res = CoreSdk_GetTemporarySkeleton(m_ModifiedSkeletonIndex, t_SessionId); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // At this point if we are satisfied with the modifications to the skeleton we can load it into Core. + // Remember to always call function CoreSdk_ClearTemporarySkeleton after loading a temporary skeleton, + // this will keep the temporary skeleton list in sync between Core and the SDK. + + //uint32_t t_ID = 0; + //SDKReturnCode t_Res = CoreSdk_LoadSkeleton(m_ModifiedSkeletonIndex, &t_ID); + //if (t_Res != SDKReturnCode::SDKReturnCode_Success) + //{ + // ClientLog::error("Failed to load skeleton. The error given was {}.", t_Res); + // return; + //} + //if (t_ID == 0) + //{ + // ClientLog::error("Failed to give skeleton an ID."); + //} + //m_LoadedSkeletons.push_back(t_ID); + //t_Res = CoreSdk_ClearTemporarySkeleton(m_ModifiedSkeletonIndex, m_SessionId); + //if (t_Res != SDKReturnCode::SDKReturnCode_Success) + //{ + // ClientLog::error("Failed to clear temporary skeleton. The error given was {}.", t_Res); + // return; + //} + m_ModifiedSkeletonIndex = UINT_MAX; + } +} +/// @brief This support function gets the temporary skeletons for all sessions connected to Core and it prints the total number of temporary skeletons associated to the current session. +/// Temporary skeletons are used for auto allocation and in the Dev Tools. +void SDKClient::PrintTemporarySkeletonInfo() +{ + ClientLog::print("Number of temporary skeletons in the SDK: {} ", m_TemporarySkeletons.size()); + + static uint32_t t_TotalNumberOfTemporarySkeletonsInCore = 0; + auto t_TimeSinceLastTemporarySkeletonUpdate = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - m_LastTemporarySkeletonUpdate).count(); + if (t_TimeSinceLastTemporarySkeletonUpdate < static_cast(MILLISECONDS_BETWEEN_TEMPORARY_SKELETONS_UPDATE)) + { + ClientLog::print("Total number of temporary skeletons in core: {} ", t_TotalNumberOfTemporarySkeletonsInCore); + return; + } + TemporarySkeletonCountForAllSessions t_TemporarySkeletonCountForAllSessions; + SDKReturnCode t_Res = CoreSdk_GetTemporarySkeletonCountForAllSessions(&t_TemporarySkeletonCountForAllSessions); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get all temporary skeletons. The error given was {}.", (int32_t)t_Res); + return; + } + + t_TotalNumberOfTemporarySkeletonsInCore = 0; + for (uint32_t i = 0; i < t_TemporarySkeletonCountForAllSessions.sessionsCount; i++) + { + t_TotalNumberOfTemporarySkeletonsInCore += t_TemporarySkeletonCountForAllSessions.temporarySkeletonCountForSessions[i].skeletonCount; + } + + // print total number of temporary skeletons: + ClientLog::print("Total number of temporary skeletons in core: {} ", t_TotalNumberOfTemporarySkeletonsInCore); + m_LastTemporarySkeletonUpdate = std::chrono::high_resolution_clock::now(); +} + +void SDKClient::HandlePairingCommands() +{ + if (GetKeyDown('P')) // press P to pair + { + PairGlove(); + } + + if (GetKeyDown('U')) // press U to Unpair + { + UnpairGlove(); + } +} + +/// @brief This showcases haptics support on gloves. +/// Send haptics commands to connected gloves if specific keys were pressed. +/// We have two ways of sending the vibration commands, based on the dongle id or based on the skeleton id. +void SDKClient::HandleHapticCommands() +{ + const int t_LeftHand = 0; + const int t_RightHand = 1; + + uint32_t t_GloveIDs[2] = { 0, 0 }; + + for (size_t i = 0; i < m_Landscape->gloveDevices.gloveCount; i++) + { + GloveLandscapeData t_GloveLandscapeData = m_Landscape->gloveDevices.gloves[i]; + if (t_GloveLandscapeData.side == Side_Left && t_GloveLandscapeData.isHaptics) + { + t_GloveIDs[t_LeftHand] = t_GloveLandscapeData.id; + break; + } + } + + for (size_t i = 0; i < m_Landscape->gloveDevices.gloveCount; i++) + { + GloveLandscapeData t_GloveLandscapeData = m_Landscape->gloveDevices.gloves[i]; + if (t_GloveLandscapeData.side == Side_Right && t_GloveLandscapeData.isHaptics) + { + t_GloveIDs[t_RightHand] = t_GloveLandscapeData.id; + break; + } + } + + ClientHapticSettings t_HapticState[NUMBER_OF_HANDS_SUPPORTED]{}; + + // The strange key number sequence here results from having gloves lie in front of you, and have the keys and haptics in the same order. + t_HapticState[t_LeftHand].shouldHapticFinger[0] = GetKey('5'); // left thumb + t_HapticState[t_LeftHand].shouldHapticFinger[1] = GetKey('4'); // left index + t_HapticState[t_LeftHand].shouldHapticFinger[2] = GetKey('3'); // left middle + t_HapticState[t_LeftHand].shouldHapticFinger[3] = GetKey('2'); // left ring + t_HapticState[t_LeftHand].shouldHapticFinger[4] = GetKey('1'); // left pinky + t_HapticState[t_RightHand].shouldHapticFinger[0] = GetKey('6'); // right thumb + t_HapticState[t_RightHand].shouldHapticFinger[1] = GetKey('7'); // right index + t_HapticState[t_RightHand].shouldHapticFinger[2] = GetKey('8'); // right middle + t_HapticState[t_RightHand].shouldHapticFinger[3] = GetKey('9'); // right ring + t_HapticState[t_RightHand].shouldHapticFinger[4] = GetKey('0'); // right pinky + + const float s_FullPower = 1.0f; + + for (unsigned int t_HandNumber = 0; t_HandNumber < NUMBER_OF_HANDS_SUPPORTED; t_HandNumber++) + { + //If the Glove ID is 0, no glove was found for that side + if (t_GloveIDs[t_HandNumber] == 0) + { + continue; + } + + float t_HapticsPowers[NUM_FINGERS_ON_HAND]{}; + for (unsigned int t_FingerNumber = 0; t_FingerNumber < NUM_FINGERS_ON_HAND; t_FingerNumber++) + { + t_HapticsPowers[t_FingerNumber] = t_HapticState[t_HandNumber].shouldHapticFinger[t_FingerNumber] ? s_FullPower : 0.0f; + } + + CoreSdk_VibrateFingersForGlove(t_GloveIDs[t_HandNumber], t_HapticsPowers); + } +} + +/// @brief Handles the console commands for the skeletons. +void SDKClient::HandleSkeletonCommands() +{ + if (GetKeyDown('B')) + { + LoadTestSkeleton(Side_Left); + } + if (GetKeyDown('N')) + { + LoadTestSkeleton(Side_Right); + } + if (GetKeyDown('M')) + { + UnloadTestSkeleton(); + } + if (GetKeyDown('D')) + { + //Toggle send to DevTools + m_SendToDevTools = !m_SendToDevTools; + } +} + +/// @brief This showcases haptics support on the skeletons. +/// Send haptics commands to the gloves connected to a skeleton if specific keys were pressed. +/// We have two ways of sending the vibration commands, based on the dongle id or based on the skeleton id. +void SDKClient::HandleSkeletonHapticCommands() +{ + if (m_Skeleton == nullptr || m_Skeleton->skeletons.size() == 0) + { + return; + } + + ClientHapticSettings t_HapticState[NUMBER_OF_HANDS_SUPPORTED]{}; + const int t_LeftHand = 0; + const int t_RightHand = 1; + + // The strange key number sequence here results from having gloves lie in front of you, and have the keys and haptics in the same order. + t_HapticState[t_LeftHand].shouldHapticFinger[0] = GetKey('5'); // left thumb + t_HapticState[t_LeftHand].shouldHapticFinger[1] = GetKey('4'); // left index + t_HapticState[t_LeftHand].shouldHapticFinger[2] = GetKey('3'); // left middle + t_HapticState[t_LeftHand].shouldHapticFinger[3] = GetKey('2'); // left ring + t_HapticState[t_LeftHand].shouldHapticFinger[4] = GetKey('1'); // left pinky + t_HapticState[t_RightHand].shouldHapticFinger[0] = GetKey('6'); // right thumb + t_HapticState[t_RightHand].shouldHapticFinger[1] = GetKey('7'); // right index + t_HapticState[t_RightHand].shouldHapticFinger[2] = GetKey('8'); // right middle + t_HapticState[t_RightHand].shouldHapticFinger[3] = GetKey('9'); // right ring + t_HapticState[t_RightHand].shouldHapticFinger[4] = GetKey('0'); // right pinky + + // Note: this timer is apparently not very accurate. + // It is good enough for this test client, but should probably be replaced for other uses. + static std::chrono::high_resolution_clock::time_point s_TimeOfLastHapticsCommandSent; + const std::chrono::high_resolution_clock::time_point s_Now = std::chrono::high_resolution_clock::now(); + const long long s_MillisecondsSinceLastHapticCommand = std::chrono::duration_cast(s_Now - s_TimeOfLastHapticsCommandSent).count(); + + if (s_MillisecondsSinceLastHapticCommand < static_cast(MINIMUM_MILLISECONDS_BETWEEN_HAPTICS_COMMANDS)) + { + return; + } + + const Side s_Hands[NUMBER_OF_HANDS_SUPPORTED] = { Side::Side_Left, Side::Side_Right }; + const float s_FullPower = 1.0f; + + // The preferred way of sending the haptics commands is based on skeleton id + + for (unsigned int t_HandNumber = 0; t_HandNumber < NUMBER_OF_HANDS_SUPPORTED; t_HandNumber++) + { + float t_HapticsPowers[NUM_FINGERS_ON_HAND]{}; + for (unsigned int t_FingerNumber = 0; t_FingerNumber < NUM_FINGERS_ON_HAND; t_FingerNumber++) + { + t_HapticsPowers[t_FingerNumber] = t_HapticState[t_HandNumber].shouldHapticFinger[t_FingerNumber] ? s_FullPower : 0.0f; + } + bool t_IsHaptics = false; + if (CoreSdk_DoesSkeletonGloveSupportHaptics(m_Skeleton->skeletons[0].info.id, s_Hands[t_HandNumber], &t_IsHaptics) != SDKReturnCode::SDKReturnCode_Success) + { + continue; + } + if (!t_IsHaptics) + { + continue; + } + CoreSdk_VibrateFingersForSkeleton(m_Skeleton->skeletons[0].info.id, s_Hands[t_HandNumber], t_HapticsPowers); + } +} + +/// @brief Handles the console commands for the temporary skeletons. +void SDKClient::HandleTemporarySkeletonCommands() +{ + if (GetKeyDown('A')) + { + AllocateChains(); + } + if (GetKeyDown('B')) + { + BuildTemporarySkeleton(); + } + if (GetKeyDown('C')) + { + ClearTemporarySkeleton(); + } + if (GetKeyDown('D')) + { + ClearAllTemporarySkeletons(); + } + if (GetKeyDown('E')) + { + SaveTemporarySkeletonToFile(); + } + if (GetKeyDown('F')) + { + GetTemporarySkeletonFromFile(); + } +} + +/// @brief This support function is used to set a test tracker and add it to the landscape. +void SDKClient::HandleTrackerCommands() +{ + if (GetKeyDown('O')) + { + m_TrackerTest = !m_TrackerTest; + } + + if (GetKeyDown('G')) + { + m_TrackerDataDisplayPerUser = !m_TrackerDataDisplayPerUser; + } + + if (GetKeyDown('T')) + { + SetTrackerOffset(); + } + + if (m_TrackerTest) + { + m_TrackerOffset += 0.0005f; + if (m_TrackerOffset >= 10.0f) + { + m_TrackerOffset = 0.0f; + } + + TrackerId t_TrackerId; + CopyString(t_TrackerId.id, sizeof(t_TrackerId.id), std::string("Test Tracker")); + TrackerData t_TrackerData = {}; + t_TrackerData.isHmd = false; + t_TrackerData.trackerId = t_TrackerId; + t_TrackerData.trackerType = TrackerType::TrackerType_Unknown; + t_TrackerData.position = { 0.0f, m_TrackerOffset, 0.0f }; + t_TrackerData.rotation = { 1.0f, 0.0f, 0.0f, 0.0f }; + t_TrackerData.quality = TrackerQuality::TrackingQuality_Trackable; + TrackerData t_TrackerDatas[MAX_NUMBER_OF_TRACKERS]; + t_TrackerDatas[0] = t_TrackerData; + + const SDKReturnCode t_TrackerSend = CoreSdk_SendDataForTrackers(t_TrackerDatas, 1); + if (t_TrackerSend != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to send tracker data. The error given was {}.", (int32_t)t_TrackerSend); + return; + } + } +} + +/// @brief This support function is used to set the tracker offset. It sets the tracker offset for the left hand tracker to the wrist. +void SDKClient::SetTrackerOffset() { + + TrackerOffset t_TrackerOffset; + + //Positions the tracker 3mm right, 58mm up and 43mm forward relative to the wrist. This value is illustrative and should be adjusted to the actual tracker position. + t_TrackerOffset.translation = { 0.003f, -0.058f, -0.043f }; + t_TrackerOffset.rotation = { 1.0f, 0.0f, 0.0f, 0.0f }; + t_TrackerOffset.entryType = TrackerOffsetType::TrackerOffsetType_LeftHandTrackerToWrist; + + auto t_UserId = m_Landscape->users.users[0].id; + + SDKReturnCode t_SetTrackerReturn = CoreSdk_SetTrackerOffset(t_UserId, &t_TrackerOffset); + if (t_SetTrackerReturn != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to set tracker offset. The error given was {}.", (int32_t)t_SetTrackerReturn); + return; + } +} + +/// @brief Handles the console commands for the gestures. +void SDKClient::HandleGesturesCommands() +{ + if (GetKeyDown('H')) + { + m_ShowLeftGestures = !m_ShowLeftGestures; + } +} + +void SDKClient::ExecuteGloveCalibrationStep(GloveCalibrationStepArgs p_Args) +{ + bool t_Result; + auto t_Res = CoreSdk_GloveCalibrationStartStep(p_Args, &t_Result); + + if (t_Res == SDKReturnCode_Success && t_Result) + m_CalibrationMessage = "Step finished"; + else + m_CalibrationMessage = "Step failed"; + + m_IsCalibrationInProgress = false; +} + + +/// @brief Handles the console commands for the glove calibration. +void SDKClient::HandleGloveCalibrationCommands() +{ + // Reset data + m_NumberOfCalibrationSteps = 0; + m_CalibrationGloveId = 0; + m_ValidStepData = false; + GloveCalibrationStepData_Init(&m_StepData); + + if (GetKeyDown('H')) + { + m_CalibrateLeftHand = !m_CalibrateLeftHand; + } + + // Get glove id + if (m_Landscape == nullptr || m_Landscape->users.userCount == 0) + return; + + uint32_t t_GloveID = m_Landscape->users.users[0].leftGloveID; + if (!m_CalibrateLeftHand) + t_GloveID = m_Landscape->users.users[0].rightGloveID; + + m_CalibrationGloveId = t_GloveID; + + // Check if glove ID is valid, if not return + if (m_CalibrationGloveId == 0) + return; + + GloveCalibrationArgs t_Args; + t_Args.gloveId = m_CalibrationGloveId; + + // Get the number of calibration steps + auto t_Res = CoreSdk_GloveCalibrationGetNumberOfSteps(t_Args, &m_NumberOfCalibrationSteps); + if (t_Res != SDKReturnCode_Success || m_NumberOfCalibrationSteps == 0) + return; + + if (GetKeyDown('P') && m_CalibrationStep < 9) + { + m_CalibrationStep++; + } + + if (GetKeyDown('M') && m_CalibrationStep != 0) + { + m_CalibrationStep--; + } + + // Clamp current step to max number of steps + if (m_CalibrationStep >= m_NumberOfCalibrationSteps) + m_CalibrationStep = m_NumberOfCalibrationSteps - 1; + + // Get step data + GloveCalibrationStepArgs t_StepArgs; + t_StepArgs.gloveId = t_Args.gloveId; + t_StepArgs.stepIndex = m_CalibrationStep; + t_Res = CoreSdk_GloveCalibrationGetStepData(t_StepArgs, &m_StepData); + if (t_Res != SDKReturnCode_Success) + return; + + // Dont allow starting/stopping/etc while in progress + if (m_IsCalibrationInProgress) + return; + + if (GetKeyDown('S')) // Start + { + bool t_Result = false; + t_Res = CoreSdk_GloveCalibrationStart(t_Args, &t_Result); + + if (t_Res == SDKReturnCode::SDKReturnCode_Success && t_Result) + m_CalibrationMessage = "Glove calibration started"; + else + m_CalibrationMessage = "Failed to start glove calibration"; + } + + if (GetKeyDown('C')) // Cancel + { + bool t_Result = false; + t_Res = CoreSdk_GloveCalibrationStop(t_Args, &t_Result); + + if (t_Res == SDKReturnCode::SDKReturnCode_Success && t_Result) + m_CalibrationMessage = "Glove calibration stopped"; + else + m_CalibrationMessage = "Failed to stop glove calibration"; + } + + if (GetKeyDown('F')) // Finish + { + bool t_Result = false; + t_Res = CoreSdk_GloveCalibrationFinish(t_Args, &t_Result); + + if (t_Res == SDKReturnCode::SDKReturnCode_Success && t_Result) + m_CalibrationMessage = "Glove calibration finished"; + else + m_CalibrationMessage = "Failed to finish glove calibration"; + } + + if (GetKeyDown('E')) // Execute step + { + m_CalibrationMessage = "Step " + std::to_string(m_CalibrationStep) + " in progress"; + m_IsCalibrationInProgress = true; + std::thread t_Thread(&SDKClient::ExecuteGloveCalibrationStep, this, t_StepArgs); + t_Thread.detach(); + } +} + +/// @brief Skeletons are pretty extensive in their data setup +/// so we have several support functions so we can correctly receive and parse the data, +/// this function helps setup the data. +/// @param p_Id the id of the created node setup +/// @param p_ParentId the id of the node parent +/// @param p_PosX X position of the node, this is defined with respect to the global coordinate system or the local one depending on +/// the parameter p_UseWorldCoordinates set when initializing the sdk, +/// @param p_PosY Y position of the node this is defined with respect to the global coordinate system or the local one depending on +/// the parameter p_UseWorldCoordinates set when initializing the sdk, +/// @param p_PosZ Z position of the node this is defined with respect to the global coordinate system or the local one depending on +/// the parameter p_UseWorldCoordinates set when initializing the sdk, +/// @param p_Name the name of the node setup +/// @return the generated node setup +NodeSetup SDKClient::CreateNodeSetup(uint32_t p_Id, uint32_t p_ParentId, float p_PosX, float p_PosY, float p_PosZ, std::string p_Name) +{ + NodeSetup t_Node; + NodeSetup_Init(&t_Node); + t_Node.id = p_Id; //Every ID needs to be unique per node in a skeleton. + CopyString(t_Node.name, sizeof(t_Node.name), p_Name); + t_Node.type = NodeType::NodeType_Joint; + //Every node should have a parent unless it is the Root node. + t_Node.parentID = p_ParentId; //Setting the node ID to its own ID ensures it has no parent. + t_Node.settings.usedSettings = NodeSettingsFlag::NodeSettingsFlag_None; + + t_Node.transform.position.x = p_PosX; + t_Node.transform.position.y = p_PosY; + t_Node.transform.position.z = p_PosZ; + return t_Node; +} + +ManusVec3 SDKClient::CreateManusVec3(float p_X, float p_Y, float p_Z) +{ + ManusVec3 t_Vec; + t_Vec.x = p_X; + t_Vec.y = p_Y; + t_Vec.z = p_Z; + return t_Vec; +} + +/// @brief This support function sets up the nodes for the skeleton hand +/// In order to have any 3d positional/rotational information from the gloves or body, +/// one needs to setup the skeleton on which this data should be applied. +/// In the case of this sample we create a Hand skeleton for which we want to get the calculated result. +/// The ID's for the nodes set here are the same IDs which are used in the OnSkeletonStreamCallback, +/// this allows us to create the link between Manus Core's data and the data we enter here. +bool SDKClient::SetupHandNodes(uint32_t p_SklIndex, Side p_Side) +{ + // Define number of fingers per hand and number of joints per finger + const uint32_t t_NumFingers = 5; + const uint32_t t_NumJoints = 4; + + // Because the skeleton is build up in world space coordinates all joints have to be defined in coordate space as well + // If it is required for the skeleton to work in local space, substract all previous positions from the vectors + static ManusVec3 s_LeftHandPositions[t_NumFingers * t_NumJoints] + { + CreateManusVec3(0.025320f, -0.024950f, 0.000000f), //Thumb CMC joint + CreateManusVec3(0.025320f + 0.032742f, -0.024950f, 0.000000f), //Thumb MCP joint + CreateManusVec3(0.025320f + 0.032742f + 0.028739f, -0.024950f, 0.000000f), //Thumb IP joint + CreateManusVec3(0.025320f + 0.032742f + 0.028739f + 0.028739f, -0.024950f, 0.000000f), //Thumb Tip joint + + CreateManusVec3(0.052904f, -0.011181f, 0.000000f), //Index MCP joint + CreateManusVec3(0.052904f + 0.038257f, -0.011181f, 0.000000f), //Index PIP joint + CreateManusVec3(0.052904f + 0.038257f + 0.020884f, -0.011181f, 0.000000f), //Index DIP joint + CreateManusVec3(0.052904f + 0.038257f + 0.020884f + 0.018759f, -0.011181f, 0.000000f), //Index Tip joint + + CreateManusVec3(0.051287f, 0.000000f, 0.000000f), //Middle MCP joint + CreateManusVec3(0.051287f + 0.041861f, 0.000000f, 0.000000f), //Middle PIP joint + CreateManusVec3(0.051287f + 0.041861f + 0.024766f, 0.000000f, 0.000000f), //Middle DIP joint + CreateManusVec3(0.051287f + 0.041861f + 0.024766f + 0.019683f, 0.000000f, 0.000000f), //Middle Tip joint + + CreateManusVec3(0.049802f, 0.011274f, 0.000000f ), //Ring MCP joint + CreateManusVec3(0.049802f + 0.039736f, 0.011274f, 0.000000f), //Ring PIP joint + CreateManusVec3(0.049802f + 0.039736f + 0.023564f, 0.011274f, 0.000000f), //Ring DIP joint + CreateManusVec3(0.049802f + 0.039736f + 0.023564f + 0.019868f, 0.011274f, 0.000000f), //Ring Tip joint + + CreateManusVec3(0.047309f, 0.020145f,0.000000f), //Pinky MCP joint + CreateManusVec3(0.047309f + 0.033175f, 0.020145f, 0.000000f), //Pinky PIP joint + CreateManusVec3(0.047309f + 0.033175f + 0.018020f, 0.020145f, 0.000000f), //Pinky DIP joint + CreateManusVec3(0.047309f + 0.033175f + 0.018020f + 0.019129f, 0.020145f, 0.000000f), //Pinky Tip joint + }; + + static ManusVec3 s_RightHandPositions[t_NumFingers * t_NumJoints] + { + CreateManusVec3(0.025320f, 0.024950f, 0.000000f), //Thumb CMC joint + CreateManusVec3(0.025320f + 0.032742f, 0.024950f, 0.000000f), //Thumb MCP joint + CreateManusVec3(0.025320f + 0.032742f + 0.028739f, 0.024950f, 0.000000f), //Thumb IP joint + CreateManusVec3(0.025320f + 0.032742f + 0.028739f + 0.028739f, 0.024950f, 0.000000f), //Thumb Tip joint + + CreateManusVec3(0.052904f, 0.011181f, 0.000000f), //Index MCP joint + CreateManusVec3(0.052904f + 0.038257f, 0.011181f, 0.000000f), //Index PIP joint + CreateManusVec3(0.052904f + 0.038257f + 0.020884f, 0.011181f, 0.000000f), //Index DIP joint + CreateManusVec3(0.052904f + 0.038257f + 0.020884f + 0.018759f, 0.011181f, 0.000000f), //Index Tip joint + + CreateManusVec3(0.051287f, 0.000000f, 0.000000f), //Middle MCP joint + CreateManusVec3(0.051287f + 0.041861f, 0.000000f, 0.000000f), //Middle PIP joint + CreateManusVec3(0.051287f + 0.041861f + 0.024766f, 0.000000f, 0.000000f), //Middle DIP joint + CreateManusVec3(0.051287f + 0.041861f + 0.024766f + 0.019683f, 0.000000f, 0.000000f), //Middle Tip joint + + CreateManusVec3(0.049802f, -0.011274f, 0.000000f),//Ring MCP joint + CreateManusVec3(0.049802f + 0.039736f, -0.011274f, 0.000000f), //Ring PIP joint + CreateManusVec3(0.049802f + 0.039736f + 0.023564f, -0.011274f, 0.000000f), //Ring DIP joint + CreateManusVec3(0.049802f + 0.039736f + 0.023564f + 0.019868f, -0.011274f, 0.000000f), //Ring Tip joint + + CreateManusVec3(0.047309f, -0.020145f, 0.000000f),//Pinky MCP joint + CreateManusVec3(0.047309f + 0.033175f, -0.020145f, 0.000000f), //Pinky PIP joint + CreateManusVec3(0.047309f + 0.033175f + 0.018020f, -0.020145f, 0.000000f), //Pinky DIP joint + CreateManusVec3(0.047309f + 0.033175f + 0.018020f + 0.019129f, -0.020145f, 0.000000f), //Pinky Tip joint + }; + + // Create an array with the initial position of each hand node. + // Note, these values are just an example of node positions and refer to the hand laying on a flat surface. + ManusVec3* t_Fingers; + if (p_Side == Side::Side_Left) + { + t_Fingers = s_LeftHandPositions; + } + else + { + t_Fingers = s_RightHandPositions; + } + + // skeleton entry is already done. just the nodes now. + // setup a very simple node hierarchy for fingers + // first setup the root node + // + // root, This node has ID 0 and parent ID 0, to indicate it has no parent. + SDKReturnCode t_Res = CoreSdk_AddNodeToSkeletonSetup(p_SklIndex, CreateNodeSetup(0, 0, 0, 0, 0, "Hand")); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Node To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return false; + } + + // then loop for 5 fingers + int t_FingerId = 0; + for (uint32_t i = 0; i < t_NumFingers; i++) + { + uint32_t t_ParentID = 0; + // then the digits of the finger that are linked to the root of the finger. + for (uint32_t j = 0; j < t_NumJoints; j++) + { + t_Res = CoreSdk_AddNodeToSkeletonSetup(p_SklIndex, CreateNodeSetup(1 + t_FingerId + j, t_ParentID, t_Fingers[i * 4 + j].x, t_Fingers[i * 4 + j].y, t_Fingers[i * 4 + j].z, "fingerdigit")); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + printf("Failed to Add Node To Skeleton Setup. The error given %d.", t_Res); + return false; + } + t_ParentID = 1 + t_FingerId + j; + } + t_FingerId += t_NumJoints; + } + return true; +} + +/// @brief This function sets up some basic hand chains. +/// Chains are required for a Skeleton to be able to be animated, it basically tells Manus Core +/// which nodes belong to which body part and what data needs to be applied to which node. +/// @param p_SklIndex The index of the temporary skeleton on which the chains will be added. +/// @return Returns true if everything went fine, otherwise returns false. +bool SDKClient::SetupHandChains(uint32_t p_SklIndex, Side p_Side) +{ + // Add the Hand chain, this identifies the wrist of the hand + { + ChainSettings t_ChainSettings; + ChainSettings_Init(&t_ChainSettings); + t_ChainSettings.usedSettings = ChainType::ChainType_Hand; + t_ChainSettings.hand.handMotion = HandMotion::HandMotion_IMU; + t_ChainSettings.hand.fingerChainIdsUsed = 5; //we will have 5 fingers + t_ChainSettings.hand.fingerChainIds[0] = 1; //links to the other chains we will define further down + t_ChainSettings.hand.fingerChainIds[1] = 2; + t_ChainSettings.hand.fingerChainIds[2] = 3; + t_ChainSettings.hand.fingerChainIds[3] = 4; + t_ChainSettings.hand.fingerChainIds[4] = 5; + + ChainSetup t_Chain; + ChainSetup_Init(&t_Chain); + t_Chain.id = 0; //Every ID needs to be unique per chain in a skeleton. + t_Chain.type = ChainType::ChainType_Hand; + t_Chain.dataType = ChainType::ChainType_Hand; + t_Chain.side = p_Side; + t_Chain.dataIndex = 0; + t_Chain.nodeIdCount = 1; + t_Chain.nodeIds[0] = 0; //this links to the hand node created in the SetupHandNodes + t_Chain.settings = t_ChainSettings; + + SDKReturnCode t_Res = CoreSdk_AddChainToSkeletonSetup(p_SklIndex, t_Chain); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Chain To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return false; + } + } + + // Add the 5 finger chains + const ChainType t_FingerTypes[5] = { ChainType::ChainType_FingerThumb, + ChainType::ChainType_FingerIndex, + ChainType::ChainType_FingerMiddle, + ChainType::ChainType_FingerRing, + ChainType::ChainType_FingerPinky }; + for (int i = 0; i < 5; i++) + { + ChainSettings t_ChainSettings; + ChainSettings_Init(&t_ChainSettings); + t_ChainSettings.usedSettings = t_FingerTypes[i]; + t_ChainSettings.finger.handChainId = 0; //This links to the wrist chain above. + //This identifies the metacarpal bone, if none exists, or the chain is a thumb it should be set to -1. + //The metacarpal bone should not be part of the finger chain, unless you are defining a thumb which does need it. + t_ChainSettings.finger.metacarpalBoneId = -1; + t_ChainSettings.finger.useLeafAtEnd = false; //this is set to true if there is a leaf bone to the tip of the finger. + ChainSetup t_Chain; + ChainSetup_Init(&t_Chain); + t_Chain.id = i + 1; //Every ID needs to be unique per chain in a skeleton. + t_Chain.type = t_FingerTypes[i]; + t_Chain.dataType = t_FingerTypes[i]; + t_Chain.side = p_Side; + t_Chain.dataIndex = 0; + if (i == 0) // Thumb + { + t_Chain.nodeIdCount = 4; //The amount of node id's used in the array + t_Chain.nodeIds[0] = 1; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[1] = 2; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[2] = 3; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[3] = 4; //this links to the hand node created in the SetupHandNodes + } + else // All other fingers + { + t_Chain.nodeIdCount = 4; //The amount of node id's used in the array + t_Chain.nodeIds[0] = (i * 4) + 1; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[1] = (i * 4) + 2; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[2] = (i * 4) + 3; //this links to the hand node created in the SetupHandNodes + t_Chain.nodeIds[3] = (i * 4) + 4; //this links to the hand node created in the SetupHandNodes + } + t_Chain.settings = t_ChainSettings; + + SDKReturnCode t_Res = CoreSdk_AddChainToSkeletonSetup(p_SklIndex, t_Chain); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + return false; + } + } + return true; +} + +/// @brief This function sets up a very minimalistic hand skeleton. +/// In order to have any 3d positional/rotational information from the gloves or body, +/// one needs to setup a skeleton on which this data can be applied. +/// In the case of this sample we create a Hand skeleton in order to get skeleton information +/// in the OnSkeletonStreamCallback function. This sample does not contain any 3D rendering, so +/// we will not be applying the returned data on anything. +void SDKClient::LoadTestSkeleton(Side p_Side) +{ + uint32_t t_SklIndex = 0; + + SkeletonSetupInfo t_SKL; + SkeletonSetupInfo_Init(&t_SKL); + t_SKL.type = SkeletonType::SkeletonType_Hand; + t_SKL.settings.scaleToTarget = true; + t_SKL.settings.useEndPointApproximations = true; + t_SKL.settings.targetType = SkeletonTargetType::SkeletonTargetType_UserIndexData; + //If the user does not exist then the added skeleton will not be animated. + //Same goes for any other skeleton made for invalid users/gloves. + t_SKL.settings.skeletonTargetUserIndexData.userIndex = 0; + + CopyString(t_SKL.name, sizeof(t_SKL.name), p_Side == Side::Side_Left ? std::string("LeftHand") : std::string("RightHand")); + + SDKReturnCode t_Res = CoreSdk_CreateSkeletonSetup(t_SKL, &t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Create Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.push_back(t_SklIndex); + + // setup nodes and chains for the skeleton hand + if (!SetupHandNodes(t_SklIndex, p_Side)) return; + if (!SetupHandChains(t_SklIndex, p_Side)) return; + + if (m_SendToDevTools) + { + SendLoadedSkeleton(t_SklIndex); + } + + // load skeleton + uint32_t t_ID = 0; + t_Res = CoreSdk_LoadSkeleton(t_SklIndex, &t_ID); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to load skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + RemoveIndexFromTemporarySkeletonList(t_SklIndex); + + if (t_ID == 0) + { + ClientLog::error("Failed to give skeleton an ID."); + } + m_LoadedSkeletons.push_back(t_ID); +} + +/// @brief This support function is used to unload a skeleton from Core. +void SDKClient::UnloadTestSkeleton() +{ + if (m_LoadedSkeletons.size() == 0) + { + ClientLog::error("There was no skeleton for us to unload."); + return; + } + SDKReturnCode t_Res = CoreSdk_UnloadSkeleton(m_LoadedSkeletons[0]); + m_LoadedSkeletons.erase(m_LoadedSkeletons.begin()); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to unload skeleton. The error given was {}.", (int32_t)t_Res); + + return; + } +} + +/// @brief +/// Sends the loaded skeleton to the Core SDK. +/// This function saves the temporary skeleton with the given session ID and returns an error if the operation fails. +void SDKClient::SendLoadedSkeleton(uint32_t p_SklIndex) +{ + // save the temporary skeleton + SDKReturnCode t_Res = CoreSdk_SaveTemporarySkeleton(p_SklIndex, m_SessionId, true); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to save temporary skeleton. The error given was {}.", (int32_t)t_Res); + } +} + + +/// @brief This support function sets up an incomplete hand skeleton and then uses manus core to allocate chains for it. +void SDKClient::AllocateChains() +{ + m_ChainType = ChainType::ChainType_Invalid; + + uint32_t t_SklIndex = 0; + + SkeletonSettings t_Settings; + SkeletonSettings_Init(&t_Settings); + t_Settings.scaleToTarget = true; + t_Settings.targetType = SkeletonTargetType::SkeletonTargetType_UserData; + t_Settings.skeletonTargetUserData.userID = 0; + + SkeletonSetupInfo t_SKL; + SkeletonSetupInfo_Init(&t_SKL); + t_SKL.id = 0; + t_SKL.type = SkeletonType::SkeletonType_Hand; + t_SKL.settings = t_Settings; + CopyString(t_SKL.name, sizeof(t_SKL.name), std::string("hand")); + + SDKReturnCode t_Res = CoreSdk_CreateSkeletonSetup(t_SKL, &t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Create Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.push_back(t_SklIndex); + + // setup nodes for the skeleton hand + SetupHandNodes(t_SklIndex, Side::Side_Left); + + // allocate chains for skeleton + t_Res = CoreSdk_AllocateChainsForSkeletonSetup(t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to allocate chains for skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // get the skeleton info + SkeletonSetupArraySizes t_SkeletonInfo; + t_Res = CoreSdk_GetSkeletonSetupArraySizes(t_SklIndex, &t_SkeletonInfo); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get info about skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + ChainSetup* t_Chains = new ChainSetup[t_SkeletonInfo.chainsCount]; + // now get the chain data + t_Res = CoreSdk_GetSkeletonSetupChainsArray(t_SklIndex, t_Chains, t_SkeletonInfo.chainsCount); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get skeleton setup chains. The error given was {}.", (int32_t)t_Res); + delete[] t_Chains; + return; + } + // as proof store the first chain type + m_ChainType = t_Chains[0].dataType; + + // but since we want to cleanly load the skeleton without holding everything up + // we need to set its side first + for (size_t i = 0; i < t_SkeletonInfo.chainsCount; i++) + { + if (t_Chains[i].dataType == ChainType::ChainType_Hand) + { + t_Chains[i].side = Side::Side_Left; // we're just picking a side here. + + t_Res = CoreSdk_OverwriteChainToSkeletonSetup(t_SklIndex, t_Chains[i]); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to overwrite Chain To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + delete[] t_Chains; + return; + } + break; // no need to continue checking the others. + } + } + // cleanup + delete[] t_Chains; + + // load skeleton so it is done. + uint32_t t_ID = 0; + t_Res = CoreSdk_LoadSkeleton(t_SklIndex, &t_ID); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to load skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + RemoveIndexFromTemporarySkeletonList(t_SklIndex); + + if (t_ID == 0) + { + ClientLog::error("Failed to give skeleton an ID."); + } + m_LoadedSkeletons.push_back(t_ID); +} + +/// @brief This support function is used for the manual allocation of the skeleton chains with means of a temporary skeleton. +/// Temporary Skeletons can be helpful when the user wants to modify the skeletons chains or nodes more than once before retargeting, +/// as they can be saved in core and retrieved later by the user to apply further modifications. This can be done easily with means of the Dev Tools. +/// The current function contains an example that shows how to create a temporary skeleton, modify its chains, nodes and +/// skeleton info, and save it into Core. This process should be used during development. +/// After that, the final skeleton should be loaded into core for retargeting and for getting the actual data,as displayed in the LoadTestSkeleton +/// function. +void SDKClient::BuildTemporarySkeleton() +{ + // define the session Id for which we want to save + // in this example we want to save a skeleton for the current session so we use our own Session Id + uint32_t t_SessionId = m_SessionId; + + bool t_IsSkeletonModified = false; // this bool is set to true by the Dev Tools after saving any modification to the skeleton, + // this triggers the OnSyStemCallback which is used in the SDK to be notified about a change to its temporary skeletons. + // for the purpose of this example setting this bool to true is not really necessary. + + // first create a skeleton setup of type Body + uint32_t t_SklIndex = 0; + + SkeletonSettings t_Settings; + SkeletonSettings_Init(&t_Settings); + t_Settings.scaleToTarget = true; + t_Settings.targetType = SkeletonTargetType::SkeletonTargetType_UserData; + t_Settings.skeletonTargetUserData.userID = 0; // this needs to be a real user Id when retargeting, when editing the temporary skeleton this may (hopefully) not cause issues + + SkeletonSetupInfo t_SKL; + SkeletonSetupInfo_Init(&t_SKL); + t_SKL.id = 0; + t_SKL.type = SkeletonType::SkeletonType_Body; + t_SKL.settings = t_Settings; + CopyString(t_SKL.name, sizeof(t_SKL.name), std::string("body")); + + SDKReturnCode t_Res = CoreSdk_CreateSkeletonSetup(t_SKL, &t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Create Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.push_back(t_SklIndex); + //Add 3 nodes to the skeleton setup + t_Res = CoreSdk_AddNodeToSkeletonSetup(t_SklIndex, CreateNodeSetup(0, 0, 0, 0, 0, "root")); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Node To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + t_Res = CoreSdk_AddNodeToSkeletonSetup(t_SklIndex, CreateNodeSetup(1, 0, 0, 1, 0, "branch")); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Node To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + t_Res = CoreSdk_AddNodeToSkeletonSetup(t_SklIndex, CreateNodeSetup(2, 1, 0, 2, 0, "leaf")); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Node To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + //Add one chain of type Leg to the skeleton setup + ChainSettings t_ChainSettings; + ChainSettings_Init(&t_ChainSettings); + t_ChainSettings.usedSettings = ChainType::ChainType_Leg; + t_ChainSettings.leg.footForwardOffset = 0; + t_ChainSettings.leg.footSideOffset = 0; + t_ChainSettings.leg.reverseKneeDirection = false; + t_ChainSettings.leg.kneeRotationOffset = 0; + + ChainSetup t_Chain; + ChainSetup_Init(&t_Chain); + t_Chain.id = 0; + t_Chain.type = ChainType::ChainType_Leg; + t_Chain.dataType = ChainType::ChainType_Leg; + t_Chain.dataIndex = 0; + t_Chain.nodeIdCount = 3; + t_Chain.nodeIds[0] = 0; + t_Chain.nodeIds[1] = 1; + t_Chain.nodeIds[2] = 2; + t_Chain.settings = t_ChainSettings; + t_Chain.side = Side::Side_Left; + + t_Res = CoreSdk_AddChainToSkeletonSetup(t_SklIndex, t_Chain); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Chain To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + // save the temporary skeleton + t_Res = CoreSdk_SaveTemporarySkeleton(t_SklIndex, t_SessionId, t_IsSkeletonModified); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to save temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // if we want to go on with the modifications to the same temporary skeleton + // get the skeleton + t_Res = CoreSdk_GetTemporarySkeleton(t_SklIndex, t_SessionId); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // now add second chain to the same temporary skeleton + t_ChainSettings.usedSettings = ChainType::ChainType_Head; + + t_Chain.id = 1; + t_Chain.type = ChainType::ChainType_Head; + t_Chain.dataType = ChainType::ChainType_Head; + t_Chain.dataIndex = 0; + t_Chain.nodeIdCount = 1; + t_Chain.nodeIds[0] = 0; + t_Chain.settings = t_ChainSettings; + t_Chain.side = Side::Side_Center; + + t_Res = CoreSdk_AddChainToSkeletonSetup(t_SklIndex, t_Chain); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Chain To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + // save the temporary skeleton + t_Res = CoreSdk_SaveTemporarySkeleton(t_SklIndex, t_SessionId, t_IsSkeletonModified); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to save temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // get the skeleton info (number of nodes and chains for that skeleton) + SkeletonSetupArraySizes t_SkeletonInfo; + t_Res = CoreSdk_GetSkeletonSetupArraySizes(t_SklIndex, &t_SkeletonInfo); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get info about skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // now get the chain data + ChainSetup* t_Chains = new ChainSetup[t_SkeletonInfo.chainsCount]; + t_Res = CoreSdk_GetSkeletonSetupChainsArray(t_SklIndex, t_Chains, t_SkeletonInfo.chainsCount); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get skeleton setup chains. The error given was {}.", (int32_t)t_Res); + return; + } + + // get the node data + NodeSetup* t_Nodes = new NodeSetup[t_SkeletonInfo.nodesCount]; + t_Res = CoreSdk_GetSkeletonSetupNodesArray(t_SklIndex, t_Nodes, t_SkeletonInfo.nodesCount); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get skeleton setup nodes. The error given was {}.", (int32_t)t_Res); + return; + } + + // just as an example try to get the skeleton setup info + SkeletonSetupInfo t_SKeletonSetupInfo; + SkeletonSetupInfo_Init(&t_SKeletonSetupInfo); + t_Res = CoreSdk_GetSkeletonSetupInfo(t_SklIndex, &t_SKeletonSetupInfo); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to overwrite Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + // if we want to modify the skeleton setup or if we want to apply some changes to the chains or nodes: + // first overwrite the existing skeleton setup and then re-add all the chains and nodes to it + SkeletonSettings_Init(&t_Settings); + t_Settings.targetType = SkeletonTargetType::SkeletonTargetType_GloveData; + + t_SKL.settings = t_Settings; + CopyString(t_SKL.name, sizeof(t_SKL.name), std::string("body2")); + + // this way we overwrite the temporary skeleton with index t_SklIndex with the modified skeleton setup + t_Res = CoreSdk_OverwriteSkeletonSetup(t_SklIndex, t_SKL); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to overwrite Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + + // modify chains and nodes + t_Chains[0].side = Side::Side_Right; + t_Nodes[0].type = NodeType::NodeType_Mesh; + + // add all the existing nodes to the new skeleton setup + for (size_t i = 0; i < t_SkeletonInfo.nodesCount; i++) + { + t_Res = CoreSdk_AddNodeToSkeletonSetup(t_SklIndex, t_Nodes[i]); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Node To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + } + + // then add all the existing chains to the new skeleton setup + for (size_t i = 0; i < t_SkeletonInfo.chainsCount; i++) + { + t_Res = CoreSdk_AddChainToSkeletonSetup(t_SklIndex, t_Chains[i]); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Add Chains To Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + } + + // cleanup + delete[] t_Chains; + delete[] t_Nodes; + + // save temporary skeleton + // in the Dev Tools this bool is set to true when saving the temporary skeleton, this triggers OnSystemCallback which + // notifies the SDK sessions about a modifications to one of their temporary skeletons. + // setting the bool to true in this example is not really necessary, it's just for testing purposes. + t_IsSkeletonModified = true; + t_Res = CoreSdk_SaveTemporarySkeleton(t_SklIndex, t_SessionId, t_IsSkeletonModified); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to save temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } +} + +/// @brief This support function is used to clear a temporary skeleton from the temporary skeleton list, for example it can be used when the +/// max number of temporary skeletons has been reached for a specific session. +void SDKClient::ClearTemporarySkeleton() +{ + // clear the first element of the temporary skeleton list + if (m_TemporarySkeletons.size() == 0) + { + ClientLog::error("There are no Temporary Skeletons to clear!"); + return; + } + uint32_t t_SklIndex = m_TemporarySkeletons[0]; + SDKReturnCode t_Res = CoreSdk_ClearTemporarySkeleton(t_SklIndex, m_SessionId); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Clear Temporary Skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.erase(m_TemporarySkeletons.begin()); +} + +/// @brief This support function is used to clear all temporary skeletons associated to the current SDK session, it can be used when the +/// max number of temporary skeletons has been reached for the current session and we want to make room for more. +void SDKClient::ClearAllTemporarySkeletons() +{ + if (m_TemporarySkeletons.size() == 0) + { + ClientLog::error("There are no Temporary Skeletons to clear!"); + return; + } + SDKReturnCode t_Res = CoreSdk_ClearAllTemporarySkeletons(); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Clear All Temporary Skeletons. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.clear(); +} + +void SDKClient::SaveTemporarySkeletonToFile() +{ + // this example shows how to save a temporary skeleton to a file + // first create a temporary skeleton: + + // define the session Id for which we want to save + uint32_t t_SessionId = m_SessionId; + + bool t_IsSkeletonModified = false; // setting this bool to true is not necessary here, it is mostly used by the Dev Tools + // to notify the SDK sessions about their skeleton being modified. + + // first create a skeleton setup + uint32_t t_SklIndex = 0; + + SkeletonSetupInfo t_SKL; + SkeletonSetupInfo_Init(&t_SKL); + t_SKL.type = SkeletonType::SkeletonType_Hand; + t_SKL.settings.scaleToTarget = true; + t_SKL.settings.targetType = SkeletonTargetType::SkeletonTargetType_GloveData; + t_SKL.settings.skeletonTargetUserIndexData.userIndex = 0; + + CopyString(t_SKL.name, sizeof(t_SKL.name), std::string("LeftHand")); + + SDKReturnCode t_Res = CoreSdk_CreateSkeletonSetup(t_SKL, &t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Create Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.push_back(t_SklIndex); + + // setup nodes and chains for the skeleton hand + if (!SetupHandNodes(t_SklIndex, Side_Left)) return; + if (!SetupHandChains(t_SklIndex, Side_Left)) return; + + // save the temporary skeleton + t_Res = CoreSdk_SaveTemporarySkeleton(t_SklIndex, t_SessionId, t_IsSkeletonModified); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to save temporary skeleton. The error given was {}.", (int32_t)t_Res); + return; + } + + // now compress the temporary skeleton data and get the size of the compressed data: + uint32_t t_TemporarySkeletonLengthInBytes; + + t_Res = CoreSdk_CompressTemporarySkeletonAndGetSize(t_SklIndex, t_SessionId, &t_TemporarySkeletonLengthInBytes); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to compress temporary skeleton and get size. The error given was {}.", (int32_t)t_Res); + return; + } + unsigned char* t_TemporarySkeletonData = new unsigned char[t_TemporarySkeletonLengthInBytes]; + + // get the array of bytes with the compressed temporary skeleton data, remember to always call function CoreSdk_CompressTemporarySkeletonAndGetSize + // before trying to get the compressed temporary skeleton data + t_Res = CoreSdk_GetCompressedTemporarySkeletonData(t_TemporarySkeletonData, t_TemporarySkeletonLengthInBytes); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to get compressed temporary skeleton data. The error given was {}.", (int32_t)t_Res); + return; + } + + // now save the data into a .mskl file + // as an example we save the temporary skeleton in a folder called ManusTemporarySkeleton inside the documents directory + // get the path for the documents directory + std::string t_DirectoryPathString = GetDocumentsDirectoryPath_UTF8(); + + // create directory name and file name for storing the temporary skeleton + std::string t_DirectoryPath = + t_DirectoryPathString + + s_SlashForFilesystemPath + + "ManusTemporarySkeleton"; + + CreateFolderIfItDoesNotExist(t_DirectoryPath); + + std::string t_DirectoryPathAndFileName = + t_DirectoryPath + + s_SlashForFilesystemPath + + "TemporarySkeleton.mskl"; + + // write the temporary skeleton data to .mskl file + std::ofstream t_File = GetOutputFileStream(t_DirectoryPathAndFileName); + t_File.write((char*)t_TemporarySkeletonData, t_TemporarySkeletonLengthInBytes); + t_File.close(); + + t_Res = CoreSdk_ClearTemporarySkeleton(t_SklIndex, t_SessionId); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Clear Temporary Skeleton after saving. The error given was {}.", (int32_t)t_Res); + return; + } + RemoveIndexFromTemporarySkeletonList(t_SklIndex); +} + + +void SDKClient::GetTemporarySkeletonFromFile() +{ + // this example shows how to load a temporary skeleton data from a file + + // as an example we try to get the temporary skeleton data previously saved as .mskl file in directory Documents/ManusTemporarySkeleton + // get the path for the documents directory + std::string t_DirectoryPathString = GetDocumentsDirectoryPath_UTF8(); + + // check if directory exists + std::string t_DirectoryPath = + t_DirectoryPathString + + s_SlashForFilesystemPath + + "ManusTemporarySkeleton"; + + if (!DoesFolderOrFileExist(t_DirectoryPath)) + { + ClientLog::warn("Failed to read from client file, the mentioned directory does not exist"); + return; + } + + // create string with file name + std::string t_DirectoryPathAndFileName = + t_DirectoryPath + + s_SlashForFilesystemPath + + "TemporarySkeleton.mskl"; + + // read from file + std::ifstream t_File = GetInputFileStream(t_DirectoryPathAndFileName); + + if (!t_File) + { + ClientLog::warn("Failed to read from client file, the file does not exist in the mentioned directory"); + return; + } + + // get file dimension + t_File.seekg(0, t_File.end); + int t_FileLength = (int)t_File.tellg(); + t_File.seekg(0, t_File.beg); + + // get temporary skeleton data from file + unsigned char* t_TemporarySkeletonData = new unsigned char[t_FileLength]; + t_File.read((char*)t_TemporarySkeletonData, t_FileLength); + t_File.close(); + + + // save the zipped temporary skeleton information, they will be used internally for sending the data to Core + uint32_t t_TemporarySkeletonLengthInBytes = t_FileLength; + + if (t_TemporarySkeletonData == nullptr) + { + ClientLog::warn("Failed to read the compressed temporary skeleton data from file"); + delete[] t_TemporarySkeletonData; + return; + } + + // create a skeleton setup where we will store the temporary skeleton retrieved from file + SkeletonSetupInfo t_SKL; + SkeletonSetupInfo_Init(&t_SKL); + uint32_t t_SklIndex = 0; + SDKReturnCode t_Res = CoreSdk_CreateSkeletonSetup(t_SKL, &t_SklIndex); + if (t_Res != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to Create Skeleton Setup. The error given was {}.", (int32_t)t_Res); + return; + } + m_TemporarySkeletons.push_back(t_SklIndex); + + // associate the retrieved temporary skeleton to the current session id + uint32_t t_SessionId = m_SessionId; + + // load the temporary skeleton data retrieved from the zipped file and save it with index t_SklIndex and session id of the current session + SDKReturnCode t_Result = CoreSdk_GetTemporarySkeletonFromCompressedData(t_SklIndex, t_SessionId, t_TemporarySkeletonData, t_TemporarySkeletonLengthInBytes); + if (t_Result != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::warn("Failed to load temporary skeleton data from client file in Core, the error code was: {}.", (int32_t)t_Result); + return; + } + + delete[] t_TemporarySkeletonData; +} + +void SDKClient::TestTimestamp() +{ + ManusTimestamp t_TS; + ManusTimestamp_Init(&t_TS); + ManusTimestampInfo t_TSInfo; + ManusTimestampInfo_Init(&t_TSInfo); + t_TSInfo.fraction = 30; + t_TSInfo.second = 15; + t_TSInfo.minute = 30; + t_TSInfo.hour = 12; + t_TSInfo.day = 1; + t_TSInfo.month = 8; + t_TSInfo.year = 2012; + t_TSInfo.timecode = true; + + CoreSdk_SetTimestampInfo(&t_TS, t_TSInfo); + + ManusTimestampInfo t_TSInfo2; + CoreSdk_GetTimestampInfo(t_TS, &t_TSInfo2); +} + +void SDKClient::RemoveIndexFromTemporarySkeletonList(uint32_t p_Idx) +{ + for (int i = 0; i < m_TemporarySkeletons.size(); i++) + { + if (m_TemporarySkeletons[i] == p_Idx) + { + m_TemporarySkeletons.erase(m_TemporarySkeletons.begin() + i); + } + } +} + +void SDKClient::PairGlove() +{ + bool t_GloveFound = false; + uint32_t t_GloveToPairId = -1; + + for (size_t i = 0; i < m_Landscape->gloveDevices.gloveCount; i++) + { + if (m_Landscape->gloveDevices.gloves[i].pairedState == DevicePairedState_Unpaired) + { + t_GloveToPairId = m_Landscape->gloveDevices.gloves[i].id; + t_GloveFound = true; + break; + } + } + + if (!t_GloveFound) + { + ClientLog::warn("No glove available for pairing found."); + return; + } + + bool t_Paired; + SDKReturnCode t_PairGloveResult = CoreSdk_PairGlove(t_GloveToPairId, &t_Paired); + if (t_PairGloveResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to pair glove with ID: {}. The error given was {}", t_GloveToPairId, (int32_t)t_PairGloveResult); + return; + } + + if (!t_Paired) + { + ClientLog::error("Failed to pair glove with ID: {}", t_GloveToPairId); + return; + } + + ClientLog::print("Succesfully paired glove with ID: {}", t_GloveToPairId); + + return; +} + +void SDKClient::UnpairGlove() +{ + bool t_GloveFound = false; + uint32_t t_GloveToUnPairId = -1; + + for (size_t i = 0; i < m_Landscape->gloveDevices.gloveCount; i++) + { + if (m_Landscape->gloveDevices.gloves[i].id == m_Landscape->gloveDevices.gloves[i].dongleID) + continue; + + if (m_Landscape->gloveDevices.gloves[i].pairedState == DevicePairedState_Paired) + { + t_GloveToUnPairId = m_Landscape->gloveDevices.gloves[i].id; + t_GloveFound = true; + break; + } + } + + if (!t_GloveFound) + { + ClientLog::error("No glove to unpair found"); + return; + } + + bool t_Unpaired; + SDKReturnCode t_UnpairGloveResult = CoreSdk_UnpairGlove(t_GloveToUnPairId, &t_Unpaired); + if (t_UnpairGloveResult != SDKReturnCode::SDKReturnCode_Success) + { + ClientLog::error("Failed to unpair glove with ID: {}. The error given was ", t_GloveToUnPairId, (int32_t)t_UnpairGloveResult); + return; + } + + ClientLog::print("Succesfully unpaired glove with ID: {}", t_GloveToUnPairId); + return; +} +/// @brief Prints the type of the first chain generated by the AllocateChain function, this is used for testing. +void SDKClient::PrintLogs() +{ + std::vector t_SDKLogs; + s_Instance->m_LogMutex.lock(); + t_SDKLogs.swap(s_Instance->m_Logs); + s_Instance->m_LogMutex.unlock(); + + for (size_t i = 0; i < t_SDKLogs.size(); i++) + { + ClientLog::print(t_SDKLogs[i]->string.c_str()); + delete t_SDKLogs[i]; + } +} diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.hpp b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.hpp new file mode 100644 index 0000000..edbdc1b --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient.hpp @@ -0,0 +1,349 @@ +#ifndef _SDK_CLIENT_HPP_ +#define _SDK_CLIENT_HPP_ + +// Set up a Doxygen group. +/** @addtogroup SDKClient + * @{ + */ + +#include // Used for haptic commands. +#include // Used for rounding float values. +#include // Used for printing glove data, and converting glove IDs to strings. +#include // Used for smart pointers. +#include +#include +#include +#include +#include + +#include "ClientPlatformSpecific.hpp" +#include "ManusSDK.h" + +extern std::map> output_map; + + +/// @brief Constant expression: number of hands supported by demo. +constexpr unsigned int NUMBER_OF_HANDS_SUPPORTED = 2; + +/// @brief Constant expression used to define the time between two possible haptics commands sent +constexpr unsigned long long int MINIMUM_MILLISECONDS_BETWEEN_HAPTICS_COMMANDS = 20; + +/// @brief Constant expression used to define the time between two updates of the temporary skeleton count printing +constexpr unsigned long long int MILLISECONDS_BETWEEN_TEMPORARY_SKELETONS_UPDATE = 1000; + +/// @brief The type of connection to core. +enum class ConnectionType : int +{ + ConnectionType_Invalid = 0, + ConnectionType_Integrated, + ConnectionType_Local, + ConnectionType_Remote, + ClientState_MAX_CLIENT_STATE_SIZE +}; + +/// @brief The current state of the client. +enum class ClientState : int +{ + ClientState_Starting = 0, + ClientState_LookingForHosts, + ClientState_NoHostsFound, + ClientState_PickingHost, + ClientState_ConnectingToCore, + ClientState_DisplayingData, + ClientState_Disconnected, + + ClientState_MAX_CLIENT_STATE_SIZE +}; + +/// @brief Values that can be returned by this application. +enum class ClientReturnCode : int +{ + ClientReturnCode_Success = 0, + ClientReturnCode_FailedPlatformSpecificInitialization, + ClientReturnCode_FailedToResizeWindow, + ClientReturnCode_FailedToInitialize, + ClientReturnCode_FailedToFindHosts, + ClientReturnCode_FailedToConnect, + ClientReturnCode_UnrecognizedStateEncountered, + ClientReturnCode_FailedToShutDownSDK, + ClientReturnCode_FailedPlatformSpecificShutdown, + ClientReturnCode_FailedToRestart, + ClientReturnCode_FailedWrongTimeToGetData, + + ClientReturnCode_MAX_CLIENT_RETURN_CODE_SIZE +}; + +struct SDKLog +{ + LogSeverity severity; + std::string string; +}; + +/// @brief Haptic settings for a single glove. +/// +/// This is used to store if a specific motor on the glove should be enabled or not. +struct ClientHapticSettings +{ + bool shouldHapticFinger[NUM_FINGERS_ON_HAND]; +}; + +/// @brief Used to store the information about the final animated skeletons. +class ClientSkeleton +{ +public: + SkeletonInfo info; + std::vector nodes; +}; + +/// @brief Used to store all the final animated skeletons received from Core. +class ClientSkeletonCollection +{ +public: + std::vector skeletons; +}; + +/// @brief Used to store the information about the skeleton data coming from the estimation system in Core. +class ClientRawSkeleton +{ +public: + RawSkeletonInfo info; + std::vector nodes; +}; + +/// @brief Used to store all the skeleton data coming from the estimation system in Core. +class ClientRawSkeletonCollection +{ +public: + std::vector skeletons; +}; + +/// @brief Used to store all the tracker data coming from Core. +class TrackerDataCollection +{ +public: + std::vector trackerData; +}; + +/// @brief Used to store the information about the final animated skeletons. +class ClientGestures +{ +public: + GestureProbabilities info; + std::vector probabilities; +}; + +class SDKClient : public SDKClientPlatformSpecific +{ +public: + SDKClient(); + ~SDKClient(); + + ClientReturnCode Initialize(); + ClientReturnCode Run(); + ClientReturnCode ShutDown(); + + //Callbacks + static void OnConnectedCallback(const ManusHost* const p_Host); + static void OnDisconnectedCallback(const ManusHost* const p_Host); + static void OnLogCallback(LogSeverity p_Severity, const char* const p_Log, uint32_t p_Length); + static void OnSkeletonStreamCallback(const SkeletonStreamInfo* const p_Skeleton); + static void OnLandscapeCallback(const Landscape* const p_Landscape); + static void OnSystemCallback(const SystemMessage* const p_SystemMessage); + static void OnErgonomicsCallback(const ErgonomicsStream* const p_Ergo); + static void OnRawSkeletonStreamCallback(const SkeletonStreamInfo* const p_RawSkeletonStreamInfo); + static void OnTrackerStreamCallback(const TrackerStreamInfo* const p_TrackerStreamInfo); + static void OnGestureStreamCallback(const GestureStreamInfo* const p_GestureStream); + + float RoundFloatValue(float p_Value, int p_NumDecimalsToKeep); + void AdvanceConsolePosition(short int p_Y); + +protected: + virtual ClientReturnCode InitializeSDK(); + virtual ClientReturnCode RestartSDK(); + virtual ClientReturnCode RegisterAllCallbacks(); + + virtual ClientReturnCode LookingForHosts(); + virtual ClientReturnCode NoHostsFound(); + virtual ClientReturnCode PickingHost(); + virtual ClientReturnCode ConnectingToCore(); + + virtual ClientReturnCode UpdateBeforeDisplayingData(); + + virtual ClientReturnCode DisplayingData(); + virtual ClientReturnCode DisplayingDataGlove(); + virtual ClientReturnCode DisplayingDataSkeleton(); + virtual ClientReturnCode DisplayingDataTracker(); + virtual ClientReturnCode DisplayingDataTemporarySkeleton(); + virtual ClientReturnCode DisplayingLandscapeTimeData(); + virtual ClientReturnCode DisplayingDataGestures(); + virtual ClientReturnCode DisplayingGloveCalibration(); + virtual ClientReturnCode DisplayingPairing(); + + virtual ClientReturnCode DisconnectedFromCore(); + virtual ClientReturnCode ReconnectingToCore(int32_t p_ReconnectionTime = 0, int32_t p_ReconnectionAttempts = 0); + + void PrintHandErgoData(ErgonomicsData& p_ErgoData, bool p_Left); + void PrintErgonomicsData(); + void PrintDongleData(); + void PrintSystemMessage(); + void PrintSkeletonData(); + void PrintRawSkeletonData(); + + + void PrintTrackerData(); + void PrintTrackerDataGlobal(); + void PrintTrackerDataPerUser(); + + void PrintLandscapeTimeData(); + + void PrintGestureData(); + void PrintGloveCalibrationData(); + + void PrintSkeletonInfo(); + void PrintTemporarySkeletonInfo(); + void HandlePairingCommands(); + void GetTemporarySkeletonIfModified(); + + void HandleHapticCommands(); + + void HandleSkeletonCommands(); + void HandleSkeletonHapticCommands(); + void HandleTemporarySkeletonCommands(); + void HandleTrackerCommands(); + void SetTrackerOffset(); + void HandleGesturesCommands(); + void HandleGloveCalibrationCommands(); + + static NodeSetup CreateNodeSetup(uint32_t p_Id, uint32_t p_ParentId, float p_PosX, float p_PosY, float p_PosZ, std::string p_Name); + bool SetupHandNodes(uint32_t p_SklIndex, Side p_Side); + bool SetupHandChains(uint32_t p_SklIndex, Side p_Side); + + void LoadTestSkeleton(Side p_Side); + void UnloadTestSkeleton(); + void SendLoadedSkeleton(uint32_t p_SklIndex); + + void AllocateChains(); + + void BuildTemporarySkeleton(); + void ClearTemporarySkeleton(); + void ClearAllTemporarySkeletons(); + void SaveTemporarySkeletonToFile(); + void GetTemporarySkeletonFromFile(); + + void TestTimestamp(); + + void RemoveIndexFromTemporarySkeletonList(uint32_t p_Idx); + void PairGlove(); + void UnpairGlove(); + static ManusVec3 CreateManusVec3(float p_X, float p_Y, float p_Z); + + void ExecuteGloveCalibrationStep(GloveCalibrationStepArgs p_Args); + + void PrintLogs(); + +protected: + static SDKClient* s_Instance; + + bool m_RequestedExit = false; + ConnectionType m_ConnectionType = ConnectionType::ConnectionType_Invalid; + + //Console + uint32_t m_ConsoleClearTickCount = 0; + const short int m_ConsoleWidth = 220; + const short int m_ConsoleHeight = 55; + const short int m_ConsoleScrollback = 500; + int m_ConsoleCurrentOffset = 0; + + SessionType m_ClientType = SessionType::SessionType_CoreSDK; + + ClientState m_State = ClientState::ClientState_Starting; + ClientState m_PreviousState = m_State; + + std::function m_CurrentInteraction = nullptr; + + uint32_t m_HostToConnectTo = 0; + uint32_t m_NumberOfHostsFound = 0; + uint32_t m_SecondsToFindHosts = 2; + + int32_t m_SecondsToAttemptReconnecting = 60; + int32_t m_MaxReconnectionAttempts = 10; + uint32_t m_SleepBetweenReconnectingAttemptsInMs = 100; + + std::unique_ptr m_AvailableHosts = nullptr; + std::unique_ptr m_Host; + + //Data + uint32_t m_SessionId = 0; + + bool m_RumblingWrist[NUMBER_OF_HANDS_SUPPORTED] = { false, false }; + + std::vector m_LoadedSkeletons; + std::vector m_TemporarySkeletons; + + std::mutex m_SkeletonMutex; + std::mutex m_RawSkeletonMutex; + + ClientSkeletonCollection* m_NextSkeleton = nullptr; + ClientSkeletonCollection* m_Skeleton = nullptr; + + ClientRawSkeletonCollection* m_NextRawSkeleton = nullptr; + ClientRawSkeletonCollection* m_RawSkeleton = nullptr; + + std::mutex m_TrackerMutex; + + TrackerDataCollection* m_NextTrackerData = nullptr; + TrackerDataCollection* m_TrackerData = nullptr; + + std::chrono::time_point m_TimeSinceLastDisconnect; + std::chrono::time_point m_LastTemporarySkeletonUpdate = std::chrono::high_resolution_clock::now(); + + std::mutex m_SystemMessageMutex; + std::string m_SystemMessage = ""; + SystemMessageType m_SystemMessageCode = SystemMessageType::SystemMessageType_Unknown; + uint32_t m_ModifiedSkeletonIndex = UINT_MAX; + + ManusTimestampInfo m_ErgoTimestampInfo; + ErgonomicsData m_LeftGloveErgoData; + ErgonomicsData m_RightGloveErgoData; + + ChainType m_ChainType = ChainType::ChainType_Invalid; + + bool m_TrackerTest = false; + bool m_TrackerDataDisplayPerUser = false; + float m_TrackerOffset = 0.0f; + + std::mutex m_LandscapeMutex; + Landscape* m_NewLandscape = nullptr; + Landscape* m_Landscape = nullptr; + std::vector m_NewGestureLandscapeData; + std::vector m_GestureLandscapeData; + + uint32_t m_FirstLeftGloveID = 0; + uint32_t m_FirstRightGloveID = 0; + + std::mutex m_GestureMutex; + ClientGestures* m_NewFirstLeftGloveGestures = nullptr; + ClientGestures* m_FirstLeftGloveGestures = nullptr; + + ClientGestures* m_NewFirstRightGloveGestures = nullptr; + ClientGestures* m_FirstRightGloveGestures = nullptr; + + bool m_ShowLeftGestures = true; + + bool m_CalibrateLeftHand = true; + uint32_t m_CalibrationStep = 0; + uint32_t m_CalibrationGloveId = 0; + uint32_t m_NumberOfCalibrationSteps = 0; + bool m_ValidStepData = false; + GloveCalibrationStepData m_StepData; + bool m_IsCalibrationInProgress = false; + std::string m_CalibrationMessage = ""; + bool m_SendToDevTools = false; + + std::mutex m_LogMutex; + std::vector m_Logs; +}; + +// Close the Doxygen group. +/** @} */ +#endif diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.code-workspace b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.code-workspace new file mode 100644 index 0000000..ef9f5d2 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.code-workspace @@ -0,0 +1,7 @@ +{ + "folders": [ + { + "path": "." + } + ] +} \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj new file mode 100644 index 0000000..205c4d6 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj @@ -0,0 +1,249 @@ + + + + + Debug + ARM + + + Release + ARM + + + Debug + ARM64 + + + Release + ARM64 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + + + + + {DADD0545-4F93-41DE-B0B7-C28B8497D16B} + Linux + SDKClient_Linux + 15.0 + Linux + 1.0 + Generic + {D51BCBC9-82E9-4017-911E-C93873C4EA2B} + + + + true + Remote_Clang_1_0 + + + false + Remote_Clang_1_0 + + + true + Remote_GCC_1_0 + + + false + Remote_GCC_1_0 + + + false + Remote_Clang_1_0 + + + true + Remote_Clang_1_0 + + + + + + + + $(RemoteRootDir)/$(RemoteOutRelDir)/$(TargetName)$(TargetExt) + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + $(SolutionDir)Output\$(Platform)\$(Configuration)\ + $(SolutionDir)Temp\$(ProjectGuid)\$(Platform)\$(Configuration)\ + 8 + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + FMT_HEADER_ONLY;DEBUG;_DEBUG;%(PreprocessorDefinitions) + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + ~/ManusSDK/include;%(AdditionalIncludeDirectories) + c++17 + FMT_HEADER_ONLY;NDEBUG;%(PreprocessorDefinitions) + + + ~/ManusSDK/lib/libManusSDK.so;%(AdditionalDependencies) + ncurses + -pthread + + + rem Copy SDK client base files + copy /Y "$(SolutionDir)SDKClient_Windows\Main.cpp" "$(ProjectDir)\Main.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.hpp" "$(ProjectDir)\SDKClient.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\SDKClient.cpp" "$(ProjectDir)\SDKClient.cpp" + + rem Copy platform specific files + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\ClientPlatformSpecific.hpp" "$(ProjectDir)\ClientPlatformSpecific.hpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecific.cpp" "$(ProjectDir)\ClientPlatformSpecific.cpp" + copy /Y "$(SolutionDir)SDKClient_Windows\PlatformSpecific\Linux\ClientPlatformSpecificTypes.hpp" "$(ProjectDir)\ClientPlatformSpecificTypes.hpp" + + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj.filters b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj.filters new file mode 100644 index 0000000..85be791 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/SDKClient_Linux.vcxproj.filters @@ -0,0 +1,25 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + + + + + + Platform Specific + + + + + + + Platform Specific + + + Platform Specific + + + + \ No newline at end of file diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.d b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.d new file mode 100644 index 0000000..95cf4b3 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.d @@ -0,0 +1,4 @@ +objects/ClientPlatformSpecific.o: ClientPlatformSpecific.cpp \ + ClientPlatformSpecific.hpp ClientPlatformSpecificTypes.hpp \ + ManusSDK/include/ManusSDK.h ManusSDK/include/ManusSDKTypes.h \ + ManusSDK/include/ManusSDKTypeInitializers.h ClientLogging.hpp diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.o b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.o new file mode 100644 index 0000000..6f7208e Binary files /dev/null and b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/ClientPlatformSpecific.o differ diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.d b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.d new file mode 100644 index 0000000..0c16f29 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.d @@ -0,0 +1,124 @@ +objects/Main.o: Main.cpp ClientLogging.hpp SDKClient.hpp \ + ClientPlatformSpecific.hpp ClientPlatformSpecificTypes.hpp \ + ManusSDK/include/ManusSDK.h ManusSDK/include/ManusSDKTypes.h \ + ManusSDK/include/ManusSDKTypeInitializers.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/pybind11.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/class.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/attr.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/common.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/Python.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/patchlevel.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pyconfig.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pymacconfig.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pyport.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/exports.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pymacro.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pymath.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pymem.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pymem.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/object.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/object.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/objimpl.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/objimpl.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/typeslots.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pyhash.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pydebug.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/bytearrayobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/bytearrayobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/bytesobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/bytesobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/unicodeobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/unicodeobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/longobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/longintrepr.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/boolobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/floatobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/complexobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/rangeobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/memoryobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/tupleobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/tupleobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/listobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/listobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/dictobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/dictobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/odictobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/enumobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/setobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/methodobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/methodobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/moduleobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/funcobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/classobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/fileobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/fileobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pycapsule.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/code.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/code.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pyframe.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/traceback.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/traceback.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/sliceobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cellobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/iterobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/initconfig.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/genobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pystate.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pystate.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/abstract.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/abstract.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/descrobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/genericaliasobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/warnings.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/weakrefobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/structseq.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/namespaceobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/picklebufobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pytime.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/codecs.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pyerrors.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pyerrors.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pythread.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/context.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/modsupport.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/compile.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/compile.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pythonrun.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pythonrun.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pylifecycle.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pylifecycle.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/ceval.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/ceval.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/sysmodule.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/sysmodule.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/osmodule.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/intrcheck.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/import.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/import.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/bltinmodule.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/eval.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pyctype.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pystrtod.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pystrcmp.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/fileutils.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/fileutils.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/pyfpe.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/tracemalloc.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/frameobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/cpython/frameobject.h \ + /home/gear/miniconda3/envs/gr00t_wbc/include/python3.10/pythread.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/cast.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/descr.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/type_caster_base.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/pytypes.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/buffer_info.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/cpp_conduit.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/internals.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/typeid.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/value_and_holder.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/options.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/exception_translation.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/detail/init.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/gil.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/gil_safe_call_once.h \ + /home/gear/miniconda3/envs/gr00t_wbc/lib/python3.10/site-packages/pybind11/include/pybind11/typing.h diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.o b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.o new file mode 100644 index 0000000..fbf06ed Binary files /dev/null and b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/Main.o differ diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.d b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.d new file mode 100644 index 0000000..281f130 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.d @@ -0,0 +1,5 @@ +objects/SDKClient.o: SDKClient.cpp SDKClient.hpp \ + ClientPlatformSpecific.hpp ClientPlatformSpecificTypes.hpp \ + ManusSDK/include/ManusSDK.h ManusSDK/include/ManusSDKTypes.h \ + ManusSDK/include/ManusSDKTypeInitializers.h \ + ManusSDK/include/ManusSDKTypes.h ClientLogging.hpp diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.o b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.o new file mode 100644 index 0000000..3a05d8e Binary files /dev/null and b/gr00t_wbc/control/teleop/device/SDKClient_Linux/objects/SDKClient.o differ diff --git a/gr00t_wbc/control/teleop/device/SDKClient_Linux/test.py b/gr00t_wbc/control/teleop/device/SDKClient_Linux/test.py new file mode 100644 index 0000000..6483c1d --- /dev/null +++ b/gr00t_wbc/control/teleop/device/SDKClient_Linux/test.py @@ -0,0 +1,14 @@ +import time + +import ManusServer + +ManusServer.init() + +for i in range(10): + time.sleep(1) + output = ManusServer.get_latest_state() + # print(output.keys()) + # if(output['3762867141_angle']!=[] and output['3822396207_angle']!=[]): + # print(output['3762867141_angle']) + +ManusServer.shutdown() diff --git a/gr00t_wbc/control/teleop/device/iphone/iphone.py b/gr00t_wbc/control/teleop/device/iphone/iphone.py new file mode 100644 index 0000000..1606e2d --- /dev/null +++ b/gr00t_wbc/control/teleop/device/iphone/iphone.py @@ -0,0 +1,102 @@ +import json +import threading + +from flask import Flask +import socketio + + +class IPhoneDevice: + """ + Returns the absolute pose of the iPhone device relative to the world frame. + The coordinate system is defined when ARKit is initialized. + + Absolute pose coordinate system (at ARKit start): + + y (out of screen) + ⊙---> x + | + | + ↓ z + + + If you compute the relative pose (T_0_inv @ T_now), the coordinate system becomes: + + z (out of screen) + ⊙---> y + | + | + ↓ x + + """ + + def __init__(self, port: int = 5557, silent: bool = True): + self._silent = silent + self._port = port + self._latest_transform: dict = {} + self._latest_speed: dict = {} + self._commands: list[str] = [] + + # Use threading mode for socketio + self._sio = socketio.Server(async_mode="threading", cors_allowed_origins="*") + self._app = Flask(__name__) + self._app.wsgi_app = socketio.WSGIApp(self._sio, self._app.wsgi_app) + + # Set up the event handler for updates + @self._sio.event + def connect(sid, environ): + if not self._silent: + print(f"===============>Client connected: {sid}") + + @self._sio.event + def disconnect(sid): + if not self._silent: + print(f"===============>Client disconnected: {sid}") + + @self._sio.event + def update(sid, data): + try: + data = json.loads(data) + self._latest_transform = data + self._sio.emit("commands", json.dumps(self._commands), to=sid) + except Exception as e: + if not self._silent: + print(f"Update failed: {e}") + + def _run_server(self): + """Run the Flask server with threading.""" + self._app.run(host="0.0.0.0", port=self._port, threaded=True) + + def start(self): + """Start the server in a background thread.""" + server_thread = threading.Thread(target=self._run_server, daemon=True) + server_thread.start() + if not self._silent: + print(f"IPhone Device running at http://0.0.0.0:{self._port}") + + def stop(self): + if not self._silent: + print("IPhone Device stopped.") + + def get_cmd(self) -> dict: + return self._latest_transform + + def send_cmd(self, enable: bool) -> None: + self._commands = ["start_haptics" if enable else "stop_haptics"] + + +if __name__ == "__main__": + import time + + device = IPhoneDevice() + device.start() + + # or _ in range(100): + try: + while True: + data = device.get_cmd() + print("Latest data:", data) + time.sleep(1.0) + except KeyboardInterrupt: + print("Stopping device...") + finally: + device.stop() diff --git a/gr00t_wbc/control/teleop/device/manus.py b/gr00t_wbc/control/teleop/device/manus.py new file mode 100644 index 0000000..afd7cc1 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/manus.py @@ -0,0 +1,138 @@ +from contextlib import contextmanager +import pickle +import time + +import click +import numpy as np +from scipy.spatial.transform import Rotation as R +import zmq + +from gr00t_wbc.control.teleop.device.SDKClient_Linux import ManusServer + +manus_idx = { + "left": ["3822396207", "3998055887", "432908014"], + "right": ["3762867141", "831307785", "3585023564"], +} + + +class Manus: + def __init__(self, port=5556): + # Storage for the latest finger data + self.latest_finger_data = { + "left_fingers": {"angle": np.zeros([40]), "position": np.zeros([25, 4, 4])}, + "right_fingers": {"angle": np.zeros([40]), "position": np.zeros([25, 4, 4])}, + } + self.manus_server = None + self.port = port + + def process_finger_pose(self, raw_position, raw_orientation, dir): + raw_position = np.asarray(raw_position).reshape([-1, 3]) + raw_orientation = np.asarray(raw_orientation).reshape([-1, 4]) + + def reorder(data): + return np.concatenate( + [ + data[0:1], # root + data[21:25], # thumb + data[1:6], # index + data[6:11], # middle + data[16:21], # ring + data[11:16], # pinky + ] + ) + + raw_position = reorder(raw_position) + raw_orientation = reorder(raw_orientation) + + transformation_matrix = np.zeros([25, 4, 4]) + rot_matrix = R.from_quat(raw_orientation[0][[1, 2, 3, 0]]).as_matrix() + transformation_matrix[:, :3, :3] = rot_matrix + transformation_matrix[:, :3, 3] = raw_position + transformation_matrix[:, 3, 3] = 1.0 + + T_root = transformation_matrix[0] + T_root_inv = np.linalg.inv(T_root) + transformation_matrix = np.matmul(T_root_inv[None], transformation_matrix) + + T_manus2avp = np.identity(4) + if dir == "right": + T_manus2avp[:3, :3] = R.from_euler("zx", [180, -90], degrees=True).as_matrix() + else: + T_manus2avp[:3, :3] = R.from_euler("x", [90], degrees=True).as_matrix() + transformation_matrix = np.matmul(T_manus2avp[None], transformation_matrix) + + return transformation_matrix + + def process_finger_angle(self, raw_finger_data, dir): + if dir == "right": + non_zero_data = [value for value in raw_finger_data if value != 0.0] + trailing_zeros_count = len(raw_finger_data) - len(non_zero_data) + raw_finger_data = non_zero_data + [0.0] * trailing_zeros_count + + return raw_finger_data + + def request_finger_data(self): + output = ManusServer.get_latest_state() + for dir, val in manus_idx.items(): + for id in val: + angle_name = f"{id}_angle" + if angle_name in output and len(output[angle_name]) > 0: + self.latest_finger_data[f"{dir}_fingers"]["angle"] = self.process_finger_angle( + np.asarray(output[angle_name]), dir + ) + + position_name = f"{id}_position" + orientation_name = f"{id}_orientation" + if ( + position_name in output + and orientation_name in output + and len(output[position_name]) > 0 + and len(output[orientation_name]) > 0 + ): + self.latest_finger_data[f"{dir}_fingers"]["position"] = ( + self.process_finger_pose( + output[position_name], output[orientation_name], dir + ) + ) + + return self.latest_finger_data + + @contextmanager + def activate(self): + try: + ManusServer.init() + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REP) + self.socket.bind(f"tcp://*:{self.port}") + yield self + finally: + ManusServer.shutdown() + + def run(self): + while True: + # Wait for a request from the client + _ = self.socket.recv() + # Process finger data + data = self.request_finger_data() + data["timestamp"] = time.time() + + # Serialize the data to send back + serialized_data = pickle.dumps(data) + + # Send the serialized data back to the client + self.socket.send(serialized_data) + + +@click.command() +@click.option("--port", type=int, default=5556) +def main(port): + print("... starting manus server ... at port", port, flush=True) + manus = Manus(port=port) + print("... manus server activating ...") + with manus.activate(): + print("==> Manus server is running at port", port, flush=True) + manus.run() + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/teleop/device/pico/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb b/gr00t_wbc/control/teleop/device/pico/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb new file mode 100644 index 0000000..3d1239d --- /dev/null +++ b/gr00t_wbc/control/teleop/device/pico/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:61961067eb4b41f81ed7cae35f4690dbb0ddfefb329a12b24e0b90ebc46ada91 +size 100046068 diff --git a/gr00t_wbc/control/teleop/device/pico/roboticsservice_1.0.0.0_arm64.deb b/gr00t_wbc/control/teleop/device/pico/roboticsservice_1.0.0.0_arm64.deb new file mode 100644 index 0000000..b003c80 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/pico/roboticsservice_1.0.0.0_arm64.deb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:532c605dfa1a02b05b7c285b856c91771c78623cded30ef5b16ea371de49ed5f +size 38477318 diff --git a/gr00t_wbc/control/teleop/device/pico/xr_client.py b/gr00t_wbc/control/teleop/device/pico/xr_client.py new file mode 100644 index 0000000..378ffc6 --- /dev/null +++ b/gr00t_wbc/control/teleop/device/pico/xr_client.py @@ -0,0 +1,155 @@ +import numpy as np +import xrobotoolkit_sdk as xrt + +# xrt interface is defined in +# https://github.com/XR-Robotics/XRoboToolkit-PC-Service-Pybind/blob/main/bindings/py_bindings.cpp + + +class XrClient: + """Client for the XrClient SDK to interact with XR devices.""" + + def __init__(self): + """Initializes the XrClient and the SDK.""" + xrt.init() + print("XRoboToolkit SDK initialized.") + + def get_pose_by_name(self, name: str) -> np.ndarray: + """Returns the pose of the specified device by name. + Valid names: "left_controller", "right_controller", "headset". + Pose is [x, y, z, qx, qy, qz, qw].""" + if name == "left_controller": + return xrt.get_left_controller_pose() + elif name == "right_controller": + return xrt.get_right_controller_pose() + elif name == "headset": + return xrt.get_headset_pose() + else: + raise ValueError( + f"Invalid name: {name}. Valid names are: 'left_controller', 'right_controller', 'headset'." + ) + + def get_key_value_by_name(self, name: str) -> float: + """Returns the trigger/grip value by name (float). + Valid names: "left_trigger", "right_trigger", "left_grip", "right_grip". + """ + if name == "left_trigger": + return xrt.get_left_trigger() + elif name == "right_trigger": + return xrt.get_right_trigger() + elif name == "left_grip": + return xrt.get_left_grip() + elif name == "right_grip": + return xrt.get_right_grip() + else: + raise ValueError( + f"Invalid name: {name}. Valid names are: \ + 'left_trigger', 'right_trigger', 'left_grip', 'right_grip'." + ) + + def get_button_state_by_name(self, name: str) -> bool: + """Returns the button state by name (bool). + Valid names: "A", "B", "X", "Y", + "left_menu_button", "right_menu_button", + "left_axis_click", "right_axis_click" + """ + if name == "A": + return xrt.get_A_button() + elif name == "B": + return xrt.get_B_button() + elif name == "X": + return xrt.get_X_button() + elif name == "Y": + return xrt.get_Y_button() + elif name == "left_menu_button": + return xrt.get_left_menu_button() + elif name == "right_menu_button": + return xrt.get_right_menu_button() + elif name == "left_axis_click": + return xrt.get_left_axis_click() + elif name == "right_axis_click": + return xrt.get_right_axis_click() + else: + raise ValueError( + f"Invalid name: {name}. Valid names are: 'A', 'B', 'X', 'Y', " + "'left_menu_button', 'right_menu_button', 'left_axis_click', 'right_axis_click'." + ) + + def get_timestamp_ns(self) -> int: + """Returns the current timestamp in nanoseconds (int).""" + return xrt.get_time_stamp_ns() + + def get_hand_tracking_state(self, hand: str) -> np.ndarray | None: + """Returns the hand tracking state for the specified hand. + Valid hands: "left", "right". + State is a 27 x 7 numpy array, where each row is [x, y, z, qx, qy, qz, qw] for each joint. + Returns None if hand tracking is inactive (low quality). + """ + if hand.lower() == "left": + if not xrt.get_left_hand_is_active(): + return None + return xrt.get_left_hand_tracking_state() + elif hand.lower() == "right": + if not xrt.get_right_hand_is_active(): + return None + return xrt.get_right_hand_tracking_state() + else: + raise ValueError(f"Invalid hand: {hand}. Valid hands are: 'left', 'right'.") + + def get_joystick_state(self, controller: str) -> list[float]: + """Returns the joystick state for the specified controller. + Valid controllers: "left", "right". + State is a list with shape (2) representing [x, y] for each joystick. + """ + if controller.lower() == "left": + return xrt.get_left_axis() + elif controller.lower() == "right": + return xrt.get_right_axis() + else: + raise ValueError( + f"Invalid controller: {controller}. Valid controllers are: 'left', 'right'." + ) + + def get_motion_tracker_data(self) -> dict: + """Returns a dictionary of motion tracker data, where the keys are the tracker serial numbers. + Each value is a dictionary containing the pose, velocity, and acceleration of the tracker. + """ + num_motion_data = xrt.num_motion_data_available() + if num_motion_data == 0: + return {} + + poses = xrt.get_motion_tracker_pose() + velocities = xrt.get_motion_tracker_velocity() + accelerations = xrt.get_motion_tracker_acceleration() + serial_numbers = xrt.get_motion_tracker_serial_numbers() + + tracker_data = {} + for i in range(num_motion_data): + serial = serial_numbers[i] + tracker_data[serial] = { + "pose": poses[i], + "velocity": velocities[i], + "acceleration": accelerations[i], + } + + return tracker_data + + def get_body_tracking_data(self) -> dict | None: + """Returns complete body tracking data or None if unavailable. + + Returns: + Dict with keys: 'poses', 'velocities', 'accelerations', 'imu_timestamps', 'body_timestamp' + - poses: (24, 7) array [x,y,z,qx,qy,qz,qw] for each joint + - velocities: (24, 6) array [vx,vy,vz,wx,wy,wz] for each joint + - accelerations: (24, 6) array [ax,ay,az,wax,way,waz] for each joint + """ + if not xrt.is_body_data_available(): + return None + + return { + "poses": xrt.get_body_joints_pose(), + "velocities": xrt.get_body_joints_velocity(), + "accelerations": xrt.get_body_joints_acceleration(), + } + + def close(self): + xrt.close() diff --git a/gr00t_wbc/control/teleop/gui/cli.py b/gr00t_wbc/control/teleop/gui/cli.py new file mode 100644 index 0000000..4126953 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/cli.py @@ -0,0 +1,20 @@ +import subprocess + +import click + + +@click.command() +def teleop(): + """CLI for interacting with the osmo.""" + subprocess.run(["python", "gr00t_wbc/control/teleop/gui/main.py"]) + + +@click.group() +def cli(): + """CLI for interacting with the osmo.""" + + +cli.add_command(teleop) + +if __name__ == "__main__": + cli() diff --git a/gr00t_wbc/control/teleop/gui/core/events3d.py b/gr00t_wbc/control/teleop/gui/core/events3d.py new file mode 100644 index 0000000..13f9696 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/core/events3d.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Marc Flerackers + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +This module contains classes to allow an object to handle events. +""" +import library.log as log +import library.profiler as profiler +from library.universal import G + + +class Event: + """ + Base class for all events, does not contain information. + """ + + def __init__(self): + pass + + def __repr__(self) -> str: + return "event:" + + +class MouseEvent(Event): + """ + Contains information about a mouse event. + + :param button: the button that is pressed in case of a mousedown or mouseup event, + or button flags in case of a mousemove event. + :type button: int + :param x: the x position of the mouse in window coordinates. + :type x: int + :param y: the y position of the mouse in window coordinates. + :type y: int + :param dx: the difference in x position in case of a mousemove event. + :type dx: int + :param dy: the difference in y position in case of a mousemove event. + :type dy: int + """ + + def __init__(self, button, x, y, dx=0, dy=0): + self.button = button + self.x = x + self.y = y + self.dx = dx + self.dy = dy + + def __repr__(self): + return "MouseEvent(%d, %d, %d, %d, %d)" % (self.button, self.x, self.y, self.dx, self.dy) + + +class MouseWheelEvent(Event): + """ + Contains information about a mouse wheel event. + + :param wheelDelta: the amount and direction that the wheel was scrolled. + :type wheelDelta: int + """ + + def __init__(self, wheelDelta, x, y): + self.wheelDelta = wheelDelta + self.x = x + self.y = y + + def __repr__(self): + return "MouseWheelEvent(%d)" % self.wheelDelta + + +class KeyEvent(Event): + """ + Contains information about a keyboard event. + + :param key: the key code of the key that was pressed or released. + :type key: int + :param character: the unicode character if the key represents a character. + :type character: unicode + :param modifiers: the modifier keys that were down at the time of pressing the key. + :type modifiers: int + """ + + def __init__(self, key, character, modifiers): + self.key = key + self.character = character + self.modifiers = modifiers + + def __repr__(self): + return "KeyEvent(%d, %04x %s, %d)" % ( + self.key, + ord(self.character), + self.character, + self.modifiers, + ) + + +class FocusEvent(Event): + """ + Contains information about a view focus/blur event + + :param blurred: the view that lost the focus. + :type blurred: guid3d.View + :param focused: the view that gained the focus. + :type focused: guid3d.View + """ + + def __init__(self, blurred, focused): + self.blurred = blurred + self.focused = focused + + def __repr__(self): + return "FocusEvent(%s, %s)" % (self.blurred, self.focused) + + +class ResizeEvent(Event): + """ + Contains information about a resize event + + :param width: the new width of the window in pixels. + :type width: int + :param height: the new height of the window in pixels. + :type height: int + :param fullscreen: the new fullscreen state of the window. + :type fullscreen: Boolean + :param dx: the change in width of the window in pixels. + :type dx: int + :param dy: the change in height of the window in pixels. + :type dy: int + """ + + def __init__(self, width, height, fullscreen): + self.width = width + self.height = height + self.fullscreen = fullscreen + + def __repr__(self): + return "ResizeEvent(%d, %d, %s)" % (self.width, self.height, self.fullscreen) + + +class HumanEvent(Event): + def __init__(self, human, change): + self.human = human + self.change = change + + def __repr__(self): + return "event: %s, %s" % (self.human, self.change) + + +class EventHandler(object): + """ + Base event handler class. Derive from this class if an object needs to be able to have events attached to it. + Currently only one event per event name can be attached. This is because we either allow a class method or + a custom method to be attached as event handling method. Since the custom method replaces the class method, + it is needed in some case to call the base class's method from the event handling method. + + There are 2 ways to attach handlers: + + 1. Override the method. This is the most appropriate way when you want to add distinctive behaviour to many + EventHandlers. + + :: + + class Widget(View): + + def onMouseDown(self, event): + #Handle event + + 2. Use the event decorator. This is the most appropriate way when you want + to attach distinctive behaviour to one EventHandler. + + :: + + widget = Widget() + + @widget.mhEvent: + def onMouseDown(event): + #Handle event + + Note that self is not passed to the handler in this case, which should not be a problem + as you can just use the variable since you are creating a closure. + """ + + def __init__(self): + self.sortOrder = None + + _logger = log.getLogger("mh.callEvent") + _depth = 0 + + def callEvent(self, eventType, event): + if not hasattr(self, eventType): + return False + topLevel = EventHandler._depth == 0 + EventHandler._depth += 1 + try: + self._logger.debug("callEvent[%d]: %s.%s(%s)", self._depth, self, eventType, event) + method = getattr(self, eventType) + if topLevel and profiler.active(): + profiler.accum("method(event)", globals(), locals()) + else: + method(event) + except Exception as _: + log.warning("Exception during event %s", eventType, exc_info=True) + self.eventFailed(EventHandler._depth) + EventHandler._depth -= 1 + if topLevel: + self._logger.debug("callEvent: done") + if G.app: + G.app.redraw() + return True + return False + + def eventFailed(self, level): + # Reset progress + if G.app: + G.app.progress(1) + + def attachEvent(self, eventName, eventMethod): + setattr(self, eventName, eventMethod) + + def detachEvent(self, eventName): + delattr(self, eventName) + + def mhEvent(self, eventMethod): + self.attachEvent(eventMethod.__name__, eventMethod) diff --git a/gr00t_wbc/control/teleop/gui/core/gui3d.py b/gr00t_wbc/control/teleop/gui/core/gui3d.py new file mode 100644 index 0000000..f9c83c4 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/core/gui3d.py @@ -0,0 +1,670 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Joel Palmius, Marc Flerackers + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +This module contains classes defined to implement widgets that provide utility functions +to the graphical user interface. + +Importing this module loads OpenGL dependencies. +""" + +import weakref + +import events3d +import library.log as log +import mh +import module3d + + +class View(events3d.EventHandler): + """ + The base view from which all widgets are derived. + """ + + def __init__(self): + + self.children = [] + self.objects = [] + self._visible = False + self._totalVisibility = False + self._parent = None + self._attached = False + self.widgets = [] + + @property + def parent(self): + if self._parent: + return self._parent() + else: + return None + + def _attach(self): + + self._attached = True + + for object in self.objects: + object._attach() + + for child in self.children: + child._attach() + + def _detach(self): + self._attached = False + + for object in self.objects: + object._detach() + + for child in self.children: + child._detach() + + def addView(self, view): + """ + Adds the view to this view. If this view is attached to the app, the view will also be attached. + + :param view: The view to be added. + :type view: gui3d.View + :return: The view, for convenience. + :rvalue: gui3d.View + """ + if view.parent: + raise RuntimeError("The view is already added to a view") + + view._parent = weakref.ref(self) + view._updateVisibility() + if self._attached: + view._attach() + + self.children.append(view) + + return view + + def removeView(self, view): + """ + Removes the view from this view. If this view is attached to the app, the view will be detached. + + :param view: The view to be removed. + :type view: gui3d.View + """ + if view not in self.children: + raise RuntimeError("The view is not a child of this view") + + view._parent = None + if self._attached: + view._detach() + + self.children.remove(view) + + def addObject(self, object): + """ + Adds the object to the view. If the view is attached to the app, + the object will also be attached and will get an OpenGL counterpart. + + :param object: The object to be added. + :type object: gui3d.Object + :return: The object, for convenience. + :rvalue: gui3d.Object + """ + if object._view: + raise RuntimeError("The object is already added to a view") + + object._view = weakref.ref(self) + if self._attached: + object._attach() + + self.objects.append(object) + + return object + + def removeObject(self, object): + """ + Removes the object from the view. If the object was attached to the app, + its OpenGL counterpart will be removed as well. + + :param object: The object to be removed. + :type object: gui3d.Object + """ + if object not in self.objects: + raise RuntimeError("The object is not a child of this view") + + object._view = None + if self._attached: + object._detach() + + self.objects.remove(object) + + def show(self): + self._visible = True + self._updateVisibility() + + def hide(self): + self._visible = False + self._updateVisibility() + + def isShown(self): + return self._visible + + def isVisible(self): + return self._totalVisibility + + def _updateVisibility(self): + previousVisibility = self._totalVisibility + + self._totalVisibility = self._visible and (not self.parent or self.parent.isVisible()) + + for o in self.objects: + o.setVisibility(self._totalVisibility) + + for v in self.children: + v._updateVisibility() + + if self._totalVisibility != previousVisibility: + if self._totalVisibility: + self.callEvent("onShow", None) + else: + self.callEvent("onHide", None) + + def onShow(self, event): + self.show() + + def onHide(self, event): + self.hide() + + def onMouseDown(self, event): + self.parent.callEvent("onMouseDown", event) + + def onMouseMoved(self, event): + self.parent.callEvent("onMouseMoved", event) + + def onMouseDragged(self, event): + self.parent.callEvent("onMouseDragged", event) + + def onMouseUp(self, event): + self.parent.callEvent("onMouseUp", event) + + def onMouseEntered(self, event): + self.parent.callEvent("onMouseEntered", event) + + def onMouseExited(self, event): + self.parent.callEvent("onMouseExited", event) + + def onClicked(self, event): + self.parent.callEvent("onClicked", event) + + def onMouseWheel(self, event): + self.parent.callEvent("onMouseWheel", event) + + def addTopWidget(self, widget): + mh.addTopWidget(widget) + self.widgets.append(widget) + widget._parent = self + if self.isVisible(): + widget.show() + else: + widget.hide() + return widget + + def removeTopWidget(self, widget): + self.widgets.remove(widget) + mh.removeTopWidget(widget) + + def showWidgets(self): + for w in self.widgets: + w.show() + + def hideWidgets(self): + for w in self.widgets: + w.hide() + + +class TaskView(View): + + def __init__(self, category, name, label=None): + super(TaskView, self).__init__() + self.name = name + self.category = category + self.label = label + self.focusWidget = None + self.tab = None + self.left, self.right = mh.addPanels() + self.sortOrder = None + + def getModifiers(self): + return {} + + def showWidgets(self): + super(TaskView, self).showWidgets() + mh.showPanels(self.left, self.right) + + def addLeftWidget(self, widget): + return self.left.addWidget(widget) + + def addRightWidget(self, widget): + return self.right.addWidget(widget) + + def removeLeftWidget(self, widget): + self.left.removeWidget(widget) + + def removeRightWidget(self, widget): + self.right.removeWidget(widget) + + +class Category(View): + + def __init__(self, name, label=None): + super(Category, self).__init__() + self.name = name + self.label = label + self.tasks = [] + self.tasksByName = {} + self.tab = None + self.tabs = None + self.panel = None + self.task = None + self.sortOrder = None + + def _taskTab(self, task): + if task.tab is None: + task.tab = self.tabs.addTab(task.name, task.label or task.name, self.tasks.index(task)) + + def realize(self, app): + self.tasks.sort(key=lambda t: t.sortOrder) + for task in self.tasks: + self._taskTab(task) + + @self.tabs.mhEvent + def onTabSelected(tab): + self.task = tab.name + app.switchTask(tab.name) + + def addTask(self, task): + if task.name in self.tasksByName: + raise KeyError("A task with this name already exists", task.name) + if task.sortOrder is None: + orders = [t.sortOrder for t in self.tasks] + o = 0 + while o in orders: + o = o + 1 + task.sortOrder = o + + self.tasks.append(task) + self.tasks.sort(key=lambda t: t.sortOrder) + + self.tasksByName[task.name] = task + self.addView(task) + if self.tabs is not None: + self._taskTab(task) + self.task = self.tasks[0].name + + categories = sorted(list(self.parent.categories.values()), key=lambda c: c.sortOrder) + categoryOrder = categories.index(self) + # Ensure that event order is per category, per task + eventOrder = 1000 * categoryOrder + task.sortOrder + self.parent.addEventHandler(task, eventOrder) + + return task + + def getTaskByName(self, name): + return self.tasksByName.get(name) + + +# The application +app = None + + +class Application(events3d.EventHandler): + """ + The Application. + """ + + singleton = None + + def __init__(self): + global app + app = self + self.parent = self + self.children = [] + self.objects = [] + self.categories = {} + self.currentCategory = None + self.currentTask = None + self.mouseDownObject = None + self.enteredObject = None + self.fullscreen = False + + self.tabs = None # Assigned in mhmain.py + + def addObject(self, object): + """ + Adds the object to the application. The object will also be attached and will get an OpenGL counterpart. + + :param object: The object to be added. + :type object: gui3d.Object + :return: The object, for convenience. + :rvalue: gui3d.Object + """ + if object._view: + raise RuntimeError("The object is already attached to a view") + + object._view = weakref.ref(self) + object._attach() + + self.objects.append(object) + + return object + + def removeObject(self, object): + """ + Removes the object from the application. Its OpenGL counterpart will be removed as well. + + :param object: The object to be removed. + :type object: gui3d.Object + """ + if object not in self.objects: + raise RuntimeError("The object is not a child of this view") + + object._view = None + object._detach() + + self.objects.remove(object) + + def addView(self, view): + """ + Adds the view to the application.The view will also be attached. + + :param view: The view to be added. + :type view: gui3d.View + :return: The view, for convenience. + :rvalue: gui3d.View + """ + if view.parent: + raise RuntimeError("The view is already attached") + + view._parent = weakref.ref(self) + view._updateVisibility() + view._attach() + + self.children.append(view) + + return view + + def removeView(self, view): + """ + Removes the view from the application. The view will be detached. + + :param view: The view to be removed. + :type view: gui3d.View + """ + if view not in self.children: + raise RuntimeError("The view is not a child of this view") + + view._parent = None + view._detach() + + self.children.remove(view) + + def isVisible(self): + return True + + def addCategory(self, category, sortOrder=None): + if category.name in self.categories: + raise KeyError("A category with this name already exists", category.name) + + if category.parent: + raise RuntimeError("The category is already attached") + + if sortOrder is None: + orders = [c.sortOrder for c in list(self.categories.values())] + o = 0 + while o in orders: + o = o + 1 + sortOrder = o + + category.sortOrder = sortOrder + self.categories[category.name] = category + + categories = list(self.categories.values()) + categories.sort(key=lambda c: c.sortOrder) + + category.tab = self.tabs.addTab( + category.name, category.label or category.name, categories.index(category) + ) + category.tabs = category.tab.child + self.addView(category) + category.realize(self) + + return category + + def switchTask(self, name): + if not self.currentCategory: + return + newTask = self.currentCategory.tasksByName[name] + + if self.currentTask and self.currentTask is newTask: + return + + if self.currentTask: + log.debug("hiding task %s", self.currentTask.name) + self.currentTask.hide() + self.currentTask.hideWidgets() + + self.currentTask = self.currentCategory.tasksByName[name] + + if self.currentTask: + log.debug("showing task %s", self.currentTask.name) + self.currentTask.show() + self.currentTask.showWidgets() + + def switchCategory(self, name): + + # Do we need to switch at all + + if self.currentCategory and self.currentCategory.name == name: + return + + # Does the category exist + + if name not in self.categories: + return + + category = self.categories[name] + + # Does the category have at least one view + + if len(category.tasks) == 0: + return + + if self.currentCategory: + log.debug("hiding category %s", self.currentCategory.name) + self.currentCategory.hide() + self.currentCategory.hideWidgets() + + self.currentCategory = category + + log.debug("showing category %s", self.currentCategory.name) + self.currentCategory.show() + self.currentCategory.showWidgets() + + self.switchTask(category.task) + + def getCategory(self, name, sortOrder=None): + category = self.categories.get(name) + if category: + return category + return self.addCategory(Category(name), sortOrder=sortOrder) + + def getTask(self, category, task): + """ + Retrieve a task by category and name. + Will not create a task or category if it does not exist. + + Set category to None or False to search for a task by name. Will raise + an exception when the result is ambiguous (there are multiple tasks with + the same name in different categories). + This quickhand is mostly useful for shell usage, but dangerous to use in + a plugin. + """ + if category: + if category not in list(self.categories.keys()): + raise RuntimeWarning('Category with name "%s" does not exist.' % category) + c = self.getCategory(category) + if task not in list(c.tasksByName.keys()): + raise RuntimeWarning( + 'Category "%s" does not contain a task with name "%s".' % (category, task) + ) + return c.getTaskByName(task) + else: + tasks = [] + for c in list(self.categories.keys()): + if task in list(self.getCategory(c).tasksByName.keys()): + tasks.append(self.getCategory(c).tasksByName[task]) + if len(tasks) == 0: + raise RuntimeWarning('No task with name "%s" found.' % task) + if len(tasks) > 1: + raise RuntimeWarning( + 'Ambiguous result for task "%s", there are multiple tasks with that name.' + % task + ) + return tasks[0] + + # called from native + + def onMouseDownCallback(self, event): + # Get picked object + pickedObject = self.getSelectedFaceGroupAndObject() + + # Do not allow picking detached objects (in case of stale picking buffer) + if pickedObject and hasattr(pickedObject, "view") and not pickedObject.view: + pickedObject = None + + if pickedObject: + object = pickedObject[1].object + else: + object = self + + # It is the object which will receive the following mouse messages + self.mouseDownObject = object + + # Send event to the object + if object: + object.callEvent("onMouseDown", event) + + def onMouseUpCallback(self, event): + if event.button == 4 or event.button == 5: + return + + # Get picked object + pickedObject = self.getSelectedFaceGroupAndObject() + + # Do not allow picking detached objects (in case of stale picking buffer) + if pickedObject and hasattr(pickedObject, "view") and not pickedObject.view: + pickedObject = None + + if pickedObject: + object = pickedObject[1].object + else: + object = self + + # Clean up handles to detached (guicommon.Object) objects + if ( + self.mouseDownObject + and hasattr(self.mouseDownObject, "view") + and not self.mouseDownObject.view + ): + self.mouseDownObject = None + + if self.mouseDownObject: + self.mouseDownObject.callEvent("onMouseUp", event) + if self.mouseDownObject is object: + self.mouseDownObject.callEvent("onClicked", event) + + def onMouseMovedCallback(self, event): + # Get picked object + picked = self.getSelectedFaceGroupAndObject() + + # Do not allow picking detached objects (in case of stale picking buffer) + if picked and hasattr(picked, "view") and not picked.view: + picked = None + + if picked and picked[1]: + group = picked[0] + object = picked[1].object or self + else: + group = None + object = self + + event.object = object + event.group = group + + # Clean up handles to detached (guicommon.Object) objects + if ( + self.mouseDownObject + and hasattr(self.mouseDownObject, "view") + and not self.mouseDownObject.view + ): + self.mouseDownObject = None + if ( + self.enteredObject + and hasattr(self.enteredObject, "view") + and not self.enteredObject.view + ): + self.enteredObject = None + + if event.button: + if self.mouseDownObject: + self.mouseDownObject.callEvent("onMouseDragged", event) + else: + if self.enteredObject != object: + if self.enteredObject: + self.enteredObject.callEvent("onMouseExited", event) + self.enteredObject = object + self.enteredObject.callEvent("onMouseEntered", event) + if object != self: + object.callEvent("onMouseMoved", event) + elif self.currentTask: + self.currentTask.callEvent("onMouseMoved", event) + + def onMouseWheelCallback(self, event): + if self.currentTask: + self.currentTask.callEvent("onMouseWheel", event) + + def onResizedCallback(self, event): + if self.fullscreen != event.fullscreen: + module3d.reloadTextures() + + self.fullscreen = event.fullscreen + + for category in list(self.categories.values()): + category.callEvent("onResized", event) + for task in category.tasks: + task.callEvent("onResized", event) diff --git a/gr00t_wbc/control/teleop/gui/core/guicommon.py b/gr00t_wbc/control/teleop/gui/core/guicommon.py new file mode 100644 index 0000000..e8f69d1 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/core/guicommon.py @@ -0,0 +1,860 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Joel Palmius, Marc Flerackers + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Common GUI elements extracted from gui3d to minimize coupling with gui backend. +""" + +import collections + +import events3d +import library.log as log +import library.matrix as matrix +import numpy as np +import shared.material as material + + +class Action(object): + def __init__(self, name): + self.name = name + + def do(self): + return True + + def undo(self): + return True + + +# Wrapper around Object3D +class Object(events3d.EventHandler): + """ + An object on the screen. + + :param position: The position in 3d space. + :type position: list or tuple + :param mesh: The mesh object. + :param visible: Wether the object should be initially visible. + :type visible: Boolean + """ + + def __init__(self, mesh, position=[0.0, 0.0, 0.0], visible=True): + + if mesh.object: + raise RuntimeError("This mesh is already attached to an object") + + self.mesh = mesh + self.mesh.object = self + self.mesh.setVisibility(visible) + + self._material = material.Material(self.name + "_Material") # Render material + + self._loc = np.zeros(3, dtype=np.float32) + self._rot = np.zeros(3, dtype=np.float32) + self._scale = np.ones(3, dtype=np.float32) + + self.setLoc(*position) + # Set to true to make the rotation of this object independent of the camera rotation + self.lockRotation = False + # Set to true to automatically set Y loc (position) to height of ground helper joint of the human + self.placeAtFeet = False + + self._view = None + + self.visible = visible + self.excludeFromProduction = False # Set to true to exclude from production renders + + self.proxy = None + + self.__seedMesh = self.mesh + self.__proxyMesh = None + self.__subdivisionMesh = None + self.__proxySubdivisionMesh = None + + self.setUVMap(mesh.material.uvMap) + + def __str__(self): + return "" % self.name + + # TODO + def clone(self): + raise NotImplementedError("You probably want to do object.mesh.clone()") + + def _attach(self): + if self.view.isVisible() and self.visible: + self.mesh.setVisibility(1) + else: + self.mesh.setVisibility(0) + + for mesh in self._meshes(): + self.attachMesh(mesh) + + def _detach(self): + for mesh in self._meshes(): + self.detachMesh(mesh) + + @staticmethod + def attachMesh(mesh): + import object3d + import selection + + selection.selectionColorMap.assignSelectionID(mesh) + object3d.Object3D.attach(mesh) + + @staticmethod + def detachMesh(mesh): + import object3d + + object3d.Object3D.detach(mesh) + + def _meshes(self): + for mesh in ( + self.__seedMesh, + self.__proxyMesh, + self.__subdivisionMesh, + self.__proxySubdivisionMesh, + ): + if mesh is not None: + yield mesh + + @property + def view(self): + if not self._view or not isinstance(self._view, collections.abc.Callable): + return None + return self._view() + + def show(self): + self.visible = True + self.setVisibility(True) + + def hide(self): + self.visible = False + self.setVisibility(False) + + def isVisible(self): + return self.visible + + @property + def name(self): + return self.mesh.name + + def setVisibility(self, visibility): + if not self.view: + self.mesh.setVisibility(0) + if self.view.isVisible() and self.visible and visibility: + self.mesh.setVisibility(1) + else: + self.mesh.setVisibility(0) + + def getPriority(self): + return self.mesh.priority + + def setPriority(self, priority): + self.mesh.priority = priority + + priority = property(getPriority, setPriority) + + ## + # Orientation properties + ## + + def getLoc(self): + result = np.zeros(3, dtype=np.float32) + result[:] = self._loc[:] + if self.placeAtFeet: + from core import G + + human = G.app.selectedHuman + result[1] = human.getJointPosition("ground")[1] + return result + + def setLoc(self, locx, locy, locz): + """ + This method is used to set the location of the object in the 3D coordinate space of the scene. + + :param locx: The x coordinate of the object. + :type locx: float + :param locy: The y coordinate of the object. + :type locy: float + :param locz: The z coordinate of the object. + :type locz: float + """ + self._loc[...] = (locx, locy, locz) + + loc = property(getLoc, setLoc) + + def get_x(self): + return self.loc[0] + + def set_x(self, x): + self._loc[0] = x + + x = property(get_x, set_x) + + def get_y(self): + return self.loc[1] + + def set_y(self, y): + self._loc[1] = y + + y = property(get_y, set_y) + + def get_z(self): + return self.loc[2] + + def set_z(self, z): + self._loc[2] = z + + z = property(get_z, set_z) + + def get_rx(self): + return self._rot[0] + + def set_rx(self, rx): + self._rot[0] = rx + + rx = property(get_rx, set_rx) + + def get_ry(self): + return self._rot[1] + + def set_ry(self, ry): + self._rot[1] = ry + + ry = property(get_ry, set_ry) + + def get_rz(self): + return self._rot[2] + + def set_rz(self, rz): + self._rot[2] = rz + + rz = property(get_rz, set_rz) + + def get_sx(self): + return self._scale[0] + + def set_sx(self, sx): + self._scale[0] = sx + + sx = property(get_sx, set_sx) + + def get_sy(self): + return self._scale[1] + + def set_sy(self, sy): + self._scale[1] = sy + + sy = property(get_sy, set_sy) + + def get_sz(self): + return self._scale[2] + + def set_sz(self, sz): + self._scale[2] = sz + + sz = property(get_sz, set_sz) + + def getPosition(self): + return [self.x, self.y, self.z] + + def setPosition(self, position): + self.setLoc(position[0], position[1], position[2]) + + def getRotation(self): + return [self.rx, self.ry, self.rz] + + def setRotation(self, rotation): + rotation[0] = rotation[0] % 360 + rotation[1] = rotation[1] % 360 + + if rotation[2] != 0.0: + log.warning("Setting a non-zero rotation around Z axis is not supported!") + rotation[2] = 0.0 + + self.setRot(rotation[0], rotation[1], rotation[2]) + + def setRot(self, rx, ry, rz): + """ + This method sets the orientation of the object in the 3D coordinate space of the scene. + + :param rx: Rotation around the x-axis. + :type rx: float + :param ry: Rotation around the y-axis. + :type ry: float + :param rz: Rotation around the z-axis. + :type rz: float + """ + self._rot[...] = (rx, ry, rz) + + rot = property(getRotation, setRotation) + + def setScale(self, scale, scaleY=None, scaleZ=1): + """ + This method sets the scale of the object in the 3D coordinate space of + the scene, relative to the initially defined size of the object. + + :param scale: Scale along the x-axis, uniform scale if other params not + specified. + :type scale: float + :param scaleY: Scale along the x-axis. + :type scaleY: float + :param scaleZ: Scale along the x-axis. + :type scaleZ: float + """ + if scaleY is None: + scaleY = scale + scaleZ = scale + self._scale[...] = (scale, scaleY, scaleZ) + + def getScale(self): + return list(self._scale) + + scale = property(getScale, setScale) + + @property + def transform(self): + m = matrix.translate(self.loc) + if any(x != 0 for x in self.rot): + m = m * matrix.rotx(self.rx) + m = m * matrix.roty(self.ry) + m = m * matrix.rotz(self.rz) + if any(x != 1 for x in self.scale): + m = m * matrix.scale(self.scale) + return m + + def getSeedMesh(self): + return self.__seedMesh + + def getProxyMesh(self): + return self.__proxyMesh + + def updateProxyMesh(self, fit_to_posed=False): + if self.proxy and self.__proxyMesh: + self.proxy.update(self.__proxyMesh, fit_to_posed) + self.__proxyMesh.update() + + def isProxied(self): + return self.mesh == self.__proxyMesh or self.mesh == self.__proxySubdivisionMesh + + def setProxy(self, proxy): + isSubdivided = self.isSubdivided() + + if self.proxy: + self.proxy = None + self.detachMesh(self.__proxyMesh) + self.__proxyMesh.clear() + self.__proxyMesh = None + if self.__proxySubdivisionMesh: + self.detachMesh(self.__proxySubdivisionMesh) + self.__proxySubdivisionMesh.clear() + self.__proxySubdivisionMesh = None + self.mesh = self.__seedMesh + self.mesh.setVisibility(1) + + if proxy: + + self.proxy = proxy + + self.__proxyMesh = proxy.object.mesh.clone() + self.__proxyMesh.object = self + + # Copy attributes from human mesh to proxy mesh + for attr in ("visibility", "pickable", "cameraMode"): + setattr(self.__proxyMesh, attr, getattr(self.mesh, attr)) + + self.updateProxyMesh() + + # Attach to GL object if this object is attached to viewport + if self.__seedMesh.object3d: + self.attachMesh(self.__proxyMesh) + + self.mesh.setVisibility(0) + self.mesh = self.__proxyMesh + self.mesh.setVisibility(1) + + self.setSubdivided(isSubdivided) + + def getProxy(self): + return self.proxy + + def getSubdivisionMesh(self, update=True): + """ + Create or update the Catmull-Clark subdivided (or smoothed) mesh for + this mesh. + This does not change the status of isSubdivided(), use setSubdivided() + for that. + + If this mesh is doubled by a proxy, when isProxied() is true, a + subdivision mesh for the proxy is used. + + Returns the subdivided mesh data. + + """ + import catmull_clark_subdivision as cks + + if self.isProxied(): + if not self.__proxySubdivisionMesh: + self.__proxySubdivisionMesh = cks.createSubdivisionObject(self.__proxyMesh, None) + if self.__seedMesh.object3d: + self.attachMesh(self.__proxySubdivisionMesh) + elif update: + cks.updateSubdivisionObject(self.__proxySubdivisionMesh) + + return self.__proxySubdivisionMesh + else: + if not self.__subdivisionMesh: + self.__subdivisionMesh = cks.createSubdivisionObject( + self.__seedMesh, self.staticFaceMask + ) + if self.__seedMesh.object3d: + self.attachMesh(self.__subdivisionMesh) + elif update: + cks.updateSubdivisionObject(self.__subdivisionMesh) + + return self.__subdivisionMesh + + def isSubdivided(self): + """ + Returns whether this mesh is currently set to be subdivided + (or smoothed). + + """ + return self.mesh == self.__subdivisionMesh or self.mesh == self.__proxySubdivisionMesh + + def setSubdivided(self, flag, update=True): + """ + Set whether this mesh is to be subdivided (or smoothed). + When set to true, the subdivision mesh is automatically created or + updated. + + """ + if flag == self.isSubdivided(): + return False + + if flag: + self.mesh.setVisibility(0) + originalMesh = self.mesh + self.mesh = self.getSubdivisionMesh(update) + self.mesh.setVisibility(1) + else: + originalMesh = ( + self.__seedMesh if self.mesh == self.__subdivisionMesh else self.__proxyMesh + ) + + self.mesh.setVisibility(0) + self.mesh = originalMesh + if update: + self.mesh.calcNormals() + self.mesh.update() + self.mesh.setVisibility(1) + + return True + + def updateSubdivisionMesh(self, rebuildIndexBuffer=False): + if rebuildIndexBuffer: + # Purge old subdivision mesh and recalculate entirely + self.setSubdivided(False) + self.__subdivisionMesh = self.__proxySubdivisionMesh = None + self.setSubdivided(True) + else: + self.getSubdivisionMesh(True) + + def _setMeshUVMap(self, filename, mesh): + if filename == self.material.uvMap: + # No change, do nothing + return + + if not hasattr(mesh, "_originalUVMap") or not mesh._originalUVMap: + # Backup original mesh UVs + mesh._originalUVMap = dict() + mesh._originalUVMap["texco"] = mesh.texco + mesh._originalUVMap["fuvs"] = mesh.fuvs + # self._originalUVMap['fvert'] = mesh.fvert + # self._originalUVMap['group'] = mesh.group + + faceMask = mesh.getFaceMask() + faceGroups = mesh.group + + self.material.uvMap = filename + + if not filename: + # Restore original UVs + mesh.setUVs(mesh._originalUVMap["texco"]) + mesh.setFaces(mesh.fvert, mesh._originalUVMap["fuvs"], faceGroups) + mesh._originalUVMap = None + else: + uvset = material.UVMap(filename) + uvset.read(mesh, filename) + + if len(uvset.fuvs) != len(mesh.fuvs): + raise NameError( + "The UV file %s is not valid for mesh %s. \ + Number of faces %d != %d" + % (filename, mesh.name, len(uvset.fuvs), len(mesh.fuvs)) + ) + + mesh.setUVs(uvset.uvs) + mesh.setFaces(mesh.fvert, uvset.fuvs, faceGroups) + + mesh.changeFaceMask(faceMask) + mesh.updateIndexBuffer() + + @property + def staticFaceMask(self): + if not hasattr(self, "_staticFaceMask") or self._staticFaceMask is None: + # If not already set, consider the current face mask state of the + # mesh to be the static face mask + self._staticFaceMask = self.__seedMesh.face_mask.copy() + return self._staticFaceMask + + def changeVertexMask(self, vertsMask): + """ + Apply a face mask to the meshes (original seed mesh, subdivided mesh + and proxied meshes) specified by a vertex mask. + The vertex mask is a list of booleans, one for each vertex, where True + means not masked (visible), and False means masked (hidden). + A face is masked if all of the vertices that define it are masked. + """ + if vertsMask is None: + # Undo face mask set by vertex mask + self.__seedMesh.changeFaceMask(self.staticFaceMask) + self.__seedMesh.updateIndexBufferFaces() + if self.__subdivisionMesh: + self.__subdivisionMesh.changeFaceMask(self.staticFaceMask) + self.__subdivisionMesh.updateIndexBufferFaces() + if self.__proxyMesh: + self.__proxyMesh.changeFaceMask( + np.ones(self.__proxyMesh.getFaceCount(), dtype=bool) + ) + self.__proxyMesh.updateIndexBufferFaces() + if self.__proxySubdivisionMesh: + self.__proxySubdivisionMesh.changeFaceMask( + np.ones(self.__proxySubdivisionMesh.getFaceCount(), dtype=bool) + ) + self.__proxySubdivisionMesh.updateIndexBufferFaces() + return + + # Mask seed mesh + faceMask = self.__seedMesh.getFaceMaskForVertices(np.argwhere(vertsMask)[..., 0]) + self.__seedMesh.changeFaceMask(np.logical_and(faceMask, self.staticFaceMask)) + self.__seedMesh.updateIndexBufferFaces() + + import log + + log.debug("%s faces masked for %s", np.count_nonzero(~faceMask), self.__seedMesh.name) + + # Mask smoothed seed mesh + if self.__subdivisionMesh: + # Remap faceMask to subdivision mesh base faces, accounting for the + # excluded faces of the static facemask (staticFaceMask). + + # Statically masked faces (staticFaceMask) are excluded from + # subdivision mesh geometry, for performance. + # Dynamically masked faces (eg. using this method) are not excluded + # from the subdivision mesh and simply masked, allowing faster + # changes to the face mask without requiring a rebuild of the + # subdivision mesh. + self.__subdivisionMesh.changeFaceMask(faceMask) + self.__subdivisionMesh.updateIndexBufferFaces() + + # Mask proxy and subdivided proxy mesh + if self.__proxyMesh: + import proxy + + # Transfer face mask to proxy + proxyVertMask = proxy.transferVertexMaskToProxy(vertsMask, self.proxy) + proxyFaceMask = self.__proxyMesh.getFaceMaskForVertices( + np.argwhere(proxyVertMask)[..., 0] + ) + + self.__proxyMesh.changeFaceMask(proxyFaceMask) + self.__proxyMesh.updateIndexBufferFaces() + + if self.__proxySubdivisionMesh: + self.__proxySubdivisionMesh.changeFaceMask(proxyFaceMask) + self.__proxySubdivisionMesh.updateIndexBufferFaces() + + ## + # Material properties + ## + + def setTexture(self, path): + """ + This method is used to specify the path of a file on disk containing the object texture. + + :param path: The path of a texture file. + :type path: str + :param cache: The texture cache to use. + :type cache: dict + """ + self.material.diffuseTexture = path + + def getTexture(self): + return self.material.diffuseTexture + + texture = property(getTexture, setTexture) + + def clearTexture(self): + """ + This method is used to clear an object's texture. + """ + self.material.diffuseTexture = None + + def hasTexture(self): + return self.texture is not None + + def setShader(self, shader): + """ + This method is used to specify the shader. + + :param shader: The path to a pair of shader files. + :type shader: string + """ + self.material.setShader(shader) + + def getShader(self): + return self.material.shader + + shader = property(getShader, setShader) + + @property + def shaderObj(self): + return self.material.shaderObj + + def configureShading( + self, diffuse=None, bump=None, normal=None, displacement=None, spec=None, vertexColors=None + ): + """ + Configure shader options and set the necessary properties based on + the material configuration of this object. + This can be done without an actual shader being set for this object. + Call this method when changes are made to the material property. + """ + self.material.configureShading(diffuse, bump, normal, displacement, spec, vertexColors) + + def getMaterial(self): + return self._material + + def setMaterial(self, material): + if self.material.uvMap != material.uvMap: + # UV map has changed + self.setUVMap(material.uvMap) + self._material.copyFrom(material) + + material = property(getMaterial, setMaterial) + + def setShaderParameter(self, name, value): + """ + Updates the shader parameters. + """ + self.material.setShaderParameter(name, value) + + @property + def shaderParameters(self): + return self.material.shaderParameters + + @property + def shaderConfig(self): + return self.material.shaderConfig + + @property + def shaderDefines(self): + return self.material.shaderDefines + + def addShaderDefine(self, defineStr): + self.material.addShaderDefine(defineStr) + + def removeShaderDefine(self, defineStr): + self.material.removeShaderDefine(defineStr) + + def clearShaderDefines(self): + self.material.clearShaderDefines() + + def setShadeless(self, shadeless): + """ + This method is used to specify whether or not the object is affected by lights. + This is used for certain GUI controls to give them a more 2D type + appearance (predominantly the top bar of GUI controls). + + NOTE enabling this option disables the use of the shader configured in the material. + + :param shadeless: Whether or not the object is unaffected by lights. + :type shadeless: Boolean + """ + self.material.shadeless = shadeless + + def getShadeless(self): + return self.material.shadeless + + shadeless = property(getShadeless, setShadeless) + + def setDepthless(self, depthless): + """ + This method is used to specify whether or not the object occludes or is occluded + by other objects + + :param depthless: Whether or not the object is occluded or occludes. + :type depthless: Boolean + """ + self.material.depthless = depthless + + def getDepthless(self): + return self.material.depthless + + depthless = property(getDepthless, setDepthless) + + def setSolid(self, solid): + """ + This method is used to specify whether or not the object is drawn solid or wireframe. + + :param solid: Whether or not the object is drawn solid or wireframe. + :type solid: Boolean + """ + self.material.wireframe = not solid + + def isSolid(self): + return self.solid + + def getSolid(self): + return not self.material.wireframe + + solid = property(getSolid, setSolid) + + def setCull(self, cull): + """ + This method is used to specify whether or not the object is back-face culled. + + :param cull: Whether and how to cull + :type cull: 0 => no culling, >0 => draw front faces, <0 => draw back faces + """ + + # Because we don't really need frontface culling, we simplify to only backface culling + if (isinstance(cull, bool) and cull) or cull > 0: + self.material.backfaceCull = True + else: + self.material.backfaceCull = False + + def getCull(self): + # Because we don't really need frontface culling, we simplify to only backface culling + if self.material.backfaceCull: + return 1 + else: + return 0 + + cull = property(getCull, setCull) + + def getAlphaToCoverage(self): + return self.material.alphaToCoverage + + def setAlphaToCoverage(self, a2cEnabled): + self.material.alphaToCoverage = a2cEnabled + + alphaToCoverage = property(getAlphaToCoverage, setAlphaToCoverage) + + def setUVMap(self, filename): + subdivided = self.isSubdivided() + + if subdivided: + # Re-generate the subdivided mesh + self.setSubdivided(False) + + # Remove stale subdivision cache if present + self.__subdivisionMesh = None + + # Set uv map on original, unsubdivided, unproxied mesh + self._setMeshUVMap(filename, self.__seedMesh) + + if self.isProxied(): + # TODO transfer UV coordinates to proxy mesh + pass + + if subdivided: + # Re-generate the subdivided mesh with new UV coordinates + self.setSubdivided(True) + + def onMouseDown(self, event): + if self.view: + self.view.callEvent("onMouseDown", event) + else: + import log + + log.debug("FAILED: mouseDown") + + def onMouseMoved(self, event): + if self.view: + self.view.callEvent("onMouseMoved", event) + else: + import log + + log.debug("FAILED: mouseMoved") + + def onMouseDragged(self, event): + if self.view: + self.view.callEvent("onMouseDragged", event) + else: + import log + + log.debug("FAILED: mouseDragged") + + def onMouseUp(self, event): + if self.view: + self.view.callEvent("onMouseUp", event) + + def onMouseEntered(self, event): + if self.view: + self.view.callEvent("onMouseEntered", event) + else: + import log + + log.debug("FAILED: mouseEntered") + + def onMouseExited(self, event): + if self.view: + self.view.callEvent("onMouseExited", event) + else: + import log + + log.debug("FAILED: mouseExited") + + def onClicked(self, event): + if self.view: + self.view.callEvent("onClicked", event) + else: + import log + + log.debug("FAILED: clicked") + + def onMouseWheel(self, event): + if self.view: + self.view.callEvent("onMouseWheel", event) diff --git a/gr00t_wbc/control/teleop/gui/core/module3d.py b/gr00t_wbc/control/teleop/gui/core/module3d.py new file mode 100644 index 0000000..e2dc87a --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/core/module3d.py @@ -0,0 +1,1307 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Marc Flerackers, Glynn Clements, Jonas Hauquier + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +TODO +""" + +import weakref + +# import material +import library.log as log +import numpy as np + +# import unique # Bugfix for numpy.unique on older numpy versions + + +class FaceGroup(object): + """ + A FaceGroup (a group of faces with a unique name). + + Each Face object can be part of one FaceGroup. Each face object has an + attribute, *group*, storing the FaceGroup it is a member of. + + The FaceGroup object contains a list of the faces in the group and must be + kept in sync with the FaceGroup references stored by the individual faces. + + .. py:attribute:: name + + The name. str + + .. py:attribute:: parent + + The parent. :py:class:`module3d.Object3D` + + :param name: The name of the group. + :type name: str + """ + + black = np.zeros(3, dtype=np.uint8) + + def __init__(self, object, name, idx): + self.object = object + self.name = name + self.idx = idx + self.color = None + self.colorID = self.black.copy() + + def __str__(self): + """ + This method returns a string containing the name of the FaceGroup. This + method is called when the object is passed to the 'print' function. + + **Parameters:** This method has no parameters. + + """ + return "facegroup %s" % self.name + + def setColor(self, rgba): + self.color = np.asarray(rgba, dtype=np.uint8) + + def getObject(self): + if self.__object: + return self.__object() + else: + return None + + def setObject(self, value): + if value is None: + self.__object = None + else: + # Ensure link to gui3d.object is weak to avoid circular references (which break garbage collection) + self.__object = weakref.ref(value) + + object = property(getObject, setObject) + + @property + def parent(self): + return self.object + + +class Object3D(object): + def __init__(self, objName, vertsPerPrimitive=4): + self.clear() + + self.name = objName + self.vertsPerPrimitive = vertsPerPrimitive + self._faceGroups = [] + self._groups_rev = {} + self.cameraMode = 0 + self._visibility = True + self.pickable = False + self.calculateTangents = True # TODO disable when not needed by shader + self.object3d = None + self._priority = 0 + self.MAX_FACES = 8 + + # Cache used for retrieving vertex colors multiplied with material diffuse color + self._old_diff = None + self._r_color_diff = None + + self.__object = None + + def clone( + self, scale=1.0, filterMaskedVerts=False + ): # TODO it's also possible to add offset to the parameters + """ + Create a clone of this mesh, with adapted scale. + If filterVerts is True, all vertices that are not required (do not + belong to any visible face) are removed and vertex mapping is added to + cloned object (see filterMaskedVerts()). For a face mapping, the + facemask of the original mesh can be used. + + By default cloning a mesh links it to the object of its parent, remove + the link by setting other.object to None before attaching it to a new + object. + """ + if self.getFaceCount(excludeMaskedFaces=filterMaskedVerts) == 0: + raise RuntimeError( + "Error cloning mesh %s. Cannot clone a mesh with 0 (unmasked) faces!", self.name + ) + + other = type(self)(self.name, self.vertsPerPrimitive) + + for prop in [ + "cameraMode", + "visibility", + "pickable", + "calculateTangents", + "priority", + "MAX_FACES", + "object", + ]: + setattr(other, prop, getattr(self, prop)) + + for fg in self.faceGroups: + ofg = other.createFaceGroup(fg.name) + if fg.color is not None: + ofg.color = fg.color.copy() + else: + ofg.color = fg.color + + if filterMaskedVerts: + self.filterMaskedVerts(other, update=False) + if scale != 1: + other.coord = scale * other.coord + else: + other.setCoords(scale * self.coord) + other.setColor(self.color.copy()) + other.setUVs(self.texco.copy()) + other.setFaces(self.fvert.copy(), self.fuvs.copy(), self.group.copy()) + other.changeFaceMask(self.face_mask.copy()) + + other.calcNormals() + other.updateIndexBuffer() + + return other + + def transformed(self, transform_mat, filterMaskedVerts=False): + """Create a clone of this mesh with its coordinates transformed + with the specified transformation matrix. filterMaskedVerts works the + same is for clone() + """ + if transform_mat.shape == (4, 4): + translation = transform_mat[:3, 3] + transform_mat = transform_mat[:3, :3] + else: + translation = np.zeros(3, dtype=np.float32) + + result = self.clone(filterMaskedVerts=filterMaskedVerts) + + coords = np.dot(transform_mat, result.getCoords().T).T + coords += translation + result.changeCoords(coords) + result.calcNormals() + result.update() + return result + + @property + def parent_map(self): + """ + Maps vertex indices from this mesh to its original parent mesh + (self.parent). This happens recursively to the topmost parent. + + Forward vertex mapping (self -> self.parent): + parent_map[idx] = mIdx: self.coord[idx] -> self.parent.coord[mIdx] + + Will return a (n, self.MAX_FACES) array if this chain of meshes + contains a subdivided mesh. + + Note that vertex maps only support one subdivision in a chain of mesh + to parent meshes. + """ + if not hasattr(self, "parent") or not self.parent: + return None + + if not hasattr(self, "_parent_map"): + mapping = np.arange(self.getVertexCount(), dtype=np.uint32) + else: + mapping = self._parent_map + + # Traverse to parent + pmap = self.parent.parent_map + if pmap is None: + return mapping + else: + # Combine mappings to topmost parent + return pmap[mapping] + + @property + def parent_map_weights(self): + return np.ones(self.getVertexCount(), dtype=np.float32) + + @property + def inverse_parent_map(self): + """ + Maps vertex indices from original parent mesh (self.parent) to + this mesh (-1 if vertex is removed in this mesh). + This happens recursively to the topmost parent. + + Reverse vertex mapping: + inverse_parent_map[idx] = mIdx: self.parent.coord[idx] -> self.coord[mIdx] + + Will return a (n, 1+self.MAX_FACES*2) array if this chain of meshes + contains a subdivided mesh. + + Note that vertex maps only support one subdivision in a chain of mesh + to parent meshes. + """ + # TODO will require nxn matrix if subdivided (catmull-clark module) + + if not hasattr(self, "parent") or not self.parent: + return None + + if not hasattr(self, "_inverse_parent_map"): + mapping = np.arange(self.parent.getVertexCount(), dtype=np.int32) + else: + mapping = self._inverse_parent_map + + # Traverse to parent + pmap = self.parent.inverse_parent_map + if pmap is None: + return mapping + else: + # Combine mappings to topmost parent + if len(mapping.shape) > 1: + shape = (len(pmap), mapping.shape[1]) + else: + shape = len(pmap) + result = -np.ones(shape, dtype=np.int32) + idx = np.where(pmap > -1) + result[idx] = mapping[pmap[idx]] + return result + + def filterMaskedVerts(self, other, update=True): + """ + Set the vertices, faces and vertex attributes of other object to the + vertices and faces of this mesh object, with the hidden faces and + vertices filtered out. + + The other mesh contains a parent_map which maps vertex indices from + the other to its original mesh and inverse_parent_map which maps vertex + indexes from original to other (-1 if removed). + + other.parent is set to the original mesh. + """ + # TODO or build a chain of parents? + if hasattr(self, "parent") and self.parent: + other.parent = self.parent + else: + other.parent = self + + # Forward vertex mapping: + # _parent_map[idx] = mIdx: other.coord[idx] -> self.coord[mIdx] + other._parent_map = np.unique(self.getVerticesForFaceMask(self.face_mask)) + + # Reverse vertex mapping: + # _inverse_parent_map[idx] = mIdx: self.coord[idx] -> other.coord[mIdx] + other._inverse_parent_map = -np.ones(self.getVertexCount(), dtype=np.int32) + other._inverse_parent_map[other.parent_map] = np.arange( + len(other._parent_map), dtype=np.int32 + ) + # other._inverse_parent_map = np.ma.masked_less(other._inverse_parent_map, 0) # TODO might be useful + + other.setCoords(self.coord[other.parent_map]) + other.setColor(self.color[other.parent_map]) + + # Filter out and remap masked faces + fvert = self.fvert[self.face_mask] + for i in range(self.vertsPerPrimitive): + fvert[:, i] = other._inverse_parent_map[fvert[:, i]] + + # Filter out and remap unused UVs + fuvs = self.fuvs[self.face_mask] + uv_idx = np.unique(fuvs.reshape(-1)) + inverse_uv_idx = -np.ones(self.texco.shape[0], dtype=np.int32) + inverse_uv_idx[uv_idx] = np.arange(uv_idx.shape[0], dtype=np.int32) + for i in range(self.vertsPerPrimitive): + fuvs[:, i] = inverse_uv_idx[fuvs[:, i]] + + other.setUVs(self.texco[uv_idx]) + + other.setFaces(fvert, fuvs, self.group[self.face_mask]) + + if update: + other.calcNormals() + other.updateIndexBuffer() + + def getCenter(self): + """ + Get center position of mesh using center of its bounding box. + """ + bBox = self.calcBBox() + bmin = np.asarray(bBox[0], dtype=np.float32) + bmax = np.asarray(bBox[1], dtype=np.float32) + return -(bmin + ((bmax - bmin) / 2)) + + def calcFaceNormals(self, ix=None): + """ + Calculate the face normals. A right-handed coordinate system is assumed, + which requires mesh faces to be defined with counter clock-wise vertex order. + Face normals are not normalized. + + Faces are treated as if they were triangles (using only the 3 first verts), + so for quads with non co-planar points, inaccuracies may occur (even though + those have a high change of being corrected by neighbouring faces). + """ + # We assume counter clock-wise winding order + # (if your normals are inversed, you're using clock-wise winding order) + if ix is None: + ix = np.s_[:] + fvert = self.coord[self.fvert[ix]] + v1 = fvert[:, 0, :] + v2 = fvert[:, 1, :] + v3 = fvert[:, 2, :] + va = v1 - v2 + vb = v2 - v3 + self.fnorm[ix] = np.cross(va, vb) + + def calcVertexNormals(self, ix=None): + """ + Calculate per-vertex normals from the face normals for smooth shading + the model. Requires face normals to be calculated first. + """ + self.markCoords(ix, norm=True) + if ix is None: + ix = np.s_[:] + + vface = self.vface[ix] + norms = self.fnorm[vface] + norms *= np.arange(self.MAX_FACES)[None, :, None] < self.nfaces[ix][:, None, None] + norms = np.sum(norms, axis=1) + norms /= np.sqrt(np.sum(norms**2, axis=-1))[:, None] + self.vnorm[ix] = norms + + def calcVertexTangents(self, ix=None): + """ + Calculate vertex tangents using Lengyel’s Method. + """ + if not self.has_uv: + return + self.markCoords(ix, norm=True) + if ix is None: + ix = np.s_[:] + xLen = self.getVertexCount() + f_ix = np.s_[:] + else: + xLen = len(ix) + f_ix = np.unique(self.vface[ix]) + + # This implementation is based on + # http://www.terathon.com/code/tangent.html + + tan = np.zeros((xLen, 2, 3), dtype=np.float32) + + fvert = self.coord[self.fvert[f_ix]] + v1 = fvert[:, 0, :] + v2 = fvert[:, 1, :] + v3 = fvert[:, 2, :] + + x1 = v2[:, 0] - v1[:, 0] + x2 = v3[:, 0] - v1[:, 0] + y1 = v2[:, 1] - v1[:, 1] + y2 = v3[:, 1] - v1[:, 1] + z1 = v2[:, 2] - v1[:, 2] + z2 = v3[:, 2] - v1[:, 2] + + fuv = self.texco[self.fuvs[f_ix]] + w1 = fuv[:, 0, :] + w2 = fuv[:, 1, :] + w3 = fuv[:, 2, :] + + s1 = w2[:, 0] - w1[:, 0] + s2 = w3[:, 0] - w1[:, 0] + t1 = w2[:, 1] - w1[:, 1] + t2 = w3[:, 1] = w1[:, 1] + + # Prevent NANs because of borked up UV coordinates # TODO perhaps remove this + s1[np.argwhere(np.equal(s1, 0.0))] = 0.0000001 + s2[np.argwhere(np.equal(s2, 0.0))] = 0.0000001 + t1[np.argwhere(np.equal(t1, 0.0))] = 0.0000001 + t2[np.argwhere(np.equal(t2, 0.0))] = 0.0000001 + + r = np.repeat(1.0, len(s1)) / ((s1 * t2) - (s2 * t1)) + sdir = np.zeros((self.getFaceCount(), 3), dtype=np.float32) + tdir = np.zeros((self.getFaceCount(), 3), dtype=np.float32) + sdir[f_ix] = np.column_stack( + [((t2 * x1) - (t1 * x2)) * r, ((t2 * y1) - (t1 * y2)) * r, ((t2 * z1) - (t1 * z2)) * r] + ) + tdir[f_ix] = np.column_stack( + [((s1 * x2) - (s2 * x1)) * r, ((s1 * y2) - (s2 * y1)) * r, ((s1 * z2) - (s2 * z1)) * r] + ) + + tan[:, 0] = np.sum(sdir[self.vface[ix]]) + tan[:, 1] = np.sum(tdir[self.vface[ix]]) + + # Gramm-Schmidt orthogonalize + dotP = dot_v3(self.vnorm[ix], tan[:, 0]) + # Duplicate dot product value in 3 columns because scalar * (n x 3) + # does not work + dotP = np.tile(dotP, (3, 1)).transpose().reshape(len(tan), 3) + self.vtang[ix, :3] = tan[:, 0] - dotP * self.vnorm[ix] + # Normalize + self.vtang[ix, :3] /= np.sqrt(np.sum(self.vtang[ix, :3] ** 2, axis=-1))[:, None] + + # Determine Handedness as w parameter + self.vtang[ix, 3] = 1.0 + indx = np.argwhere(np.less(dot_v3(np.cross(self.vnorm[ix], tan[:, 0]), tan[:, 1]), 0.0)) + self.vtang[ix, 3][indx] = -1.0 + + def getObject(self): + if self.__object: + return self.__object() + else: + return None + + def setObject(self, value): + if value is None: + self.__object = None + else: + # Ensure link to gui3d.object is weak to avoid circular references (which break garbage collection) + self.__object = weakref.ref(value) + + object = property(getObject, setObject) + + @property + def faceGroups(self): + return iter(self._faceGroups) + + @property + def faceGroupCount(self): + return len(self._faceGroups) + + def clear(self): + """ + Clears both local and remote data to repurpose this object + """ + + # Clear remote data + self._faceGroups = [] + + self._transparentPrimitives = 0 + + # Reference to vertex attributes (coordinate, normal, color, tang) that form the faces (idx = face idx) + self.fvert = [] + # Stores the face normal of the faces (idx = face idx) + self.fnorm = [] + # References to UVs at the verts of the faces (idx = face idx) + # (NOTE: UVs are not tied to vertex IDs, and are not necessarily + # uniform per vertex, like the other attributes!) + self.fuvs = [] + self.group = [] # Determines facegroup per face (idx = face idx) + self.face_mask = [] # Determines visibility per face (idx = face idx) + + self.texco = [] # UV coordinates (idx = uv idx) + + self.coord = [] # Vertex coordinates (positions) (idx = vertex idx) + self.vnorm = [] # Vertex normals (idx = vertex idx) + self.vtang = [] # Vertex tangents (idx = vertex idx) + self.color = [] # Vertex colors (idx = vertex idx) + self.vface = ( + [] + ) # References the faces that a vertex belongs to (limited to MAX_FACES) (idx = vertex idx) + self.nfaces = 0 # Polycount + + self.ucoor = False # Update flags for updating to OpenGL renderbuffers + self.unorm = False + self.utang = False + self.ucolr = False + self.utexc = False + + self.has_uv = False + + if hasattr(self, "index"): + del self.index + if hasattr(self, "grpix"): + del self.grpix + + # Maps unwelded vertices back to original welded ones (idx = unwelded vertex idx) + self.vmap = None + # Maps unwelded vertex texture (UV) coordinates back to original ones (idx = unwelded vertex idx) + self.tmap = None + + # Cached inverse of vmap: maps original welded vert idx (coord) to one or multiple unwelded + # vert idxs (r_coord) + self._inverse_vmap = None + + # Unwelded vertex buffers used by OpenGL + if hasattr(self, "r_coord"): + del self.r_coord + if hasattr(self, "r_texco"): + del self.r_texco + if hasattr(self, "r_vnorm"): + del self.r_vnorm + if hasattr(self, "r_vtang"): + del self.r_vtang + if hasattr(self, "r_color"): + del self.r_color + if hasattr(self, "r_faces"): + del self.r_faces + + def setCoords(self, coords): + nverts = len(coords) + self.coord = np.asarray(coords, dtype=np.float32) + self.vnorm = np.zeros((nverts, 3), dtype=np.float32) + self.vtang = np.zeros((nverts, 4), dtype=np.float32) + self.color = np.zeros((nverts, 4), dtype=np.uint8) + 255 + self.vface = np.zeros((nverts, self.MAX_FACES), dtype=np.uint32) + self.nfaces = np.zeros(nverts, dtype=np.uint8) + + self.orig_coord = self.coord.copy() # Keep a copy of the original coordinates + + self.ucoor = True + self.unorm = True + self.utang = True + self.ucolr = True + + self.markCoords(None, True, True, True) + + def getVertexCount(self, excludeMaskedVerts=False): + # return len(self.vface) + if excludeMaskedVerts: + return np.count_nonzero(self.getVertexMaskForFaceMask(self.getFaceMask())) + return len(self.coord) + + def getCoords(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.coord[indices] + + def getNormals(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.vnorm[indices] + + def markCoords(self, indices=None, coor=False, norm=False, colr=False): + if isinstance(indices, tuple): + indices = indices[0] + + nverts = len(self.coord) + + if coor: + if indices is None: + self.ucoor = True + else: + if self.ucoor is False: + self.ucoor = np.zeros(nverts, dtype=bool) + if self.ucoor is not True: + self.ucoor[indices] = True + + if norm: + if indices is None: + self.unorm = self.utang = True + else: + if self.unorm is False: + self.unorm = self.utang = np.zeros(nverts, dtype=bool) + if self.unorm is not True: + self.unorm[indices] = True + self.utang[indices] = True + + if colr: + if indices is None: + self.ucolr = True + else: + if self.ucolr is False: + self.ucolr = np.zeros(nverts, dtype=bool) + if self.ucolr is not True: + self.ucolr[indices] = True + + def changeCoords(self, coords, indices=None): + self.markCoords(indices, coor=True) + + if indices is None: + indices = np.s_[...] + self.coord[indices] = coords + + def setUVs(self, uvs): + self.texco = np.asarray(uvs, dtype=np.float32) + self.utexc = True + + def getUVCount(self): + return len(self.texco) + + def getUVs(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.texco[indices] + + def markUVs(self, indices=None): + if isinstance(indices, tuple): + indices = indices[0] + + ntexco = len(self.texco) + + if indices is None: + self.utexc = True + else: + if self.utexc is False: + self.utexc = np.zeros(ntexco, dtype=bool) + if self.utexc is not True: + self.utexc[indices] = True + + def setFaces(self, verts, uvs=None, groups=None, skipUpdate=False): + nfaces = len(verts) + + self.fvert = np.empty((nfaces, self.vertsPerPrimitive), dtype=np.uint32) + self.fnorm = np.zeros((nfaces, 3), dtype=np.float32) + self.fuvs = np.zeros(self.fvert.shape, dtype=np.uint32) + self.group = np.zeros(nfaces, dtype=np.uint16) + self.face_mask = np.ones(nfaces, dtype=bool) + + # preset number of vertices for all export formats to 4 + self.vertsPerFaceForExport = 4 + if nfaces != 0: + + # evaluate if we have 3 or 4 vertices per face, do it here so it works for npz or obj file + if len(verts[0]) == 4 and verts[0][0] == verts[0][3]: + self.vertsPerFaceForExport = 3 + + self.fvert[...] = verts + if uvs is not None: + self.fuvs[...] = uvs + if groups is not None: + self.group[...] = groups + + self.has_uv = uvs is not None + + if not skipUpdate: + self._update_faces(resize=True) + + def changeFaceMask(self, mask, indices=None): + if indices is None: + indices = np.s_[...] + self.face_mask[indices] = mask + + def getFaceMask(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.face_mask[indices] + + def hasUVs(self): + return self.has_uv + + def getFaceCount(self, excludeMaskedFaces=False): + if excludeMaskedFaces: + return np.count_nonzero(self.getFaceMask()) + return len(self.fvert) + + def getFaceVerts(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.fvert[indices] + + def getFaceUVs(self, indices=None): + if indices is None: + indices = np.s_[...] + return self.fuvs[indices] + + @property + def inverse_vmap(self): + """ + The inverse of vmap: a mapping of original welded (relating to UVs) + vertex (coord indices) to a set of unwelded vertices that represent the + same coordinate (r_coord indices). + """ + if self._inverse_vmap is None: + # TODO this loop is quite slow and could benefit from numpy optimization + originalToUnweldedMap = {} + for unweldedIdx, originalIdx in enumerate(self.vmap): + if originalIdx not in originalToUnweldedMap: + originalToUnweldedMap[originalIdx] = [] + originalToUnweldedMap[originalIdx].append(unweldedIdx) + self._inverse_vmap = originalToUnweldedMap + return self._inverse_vmap + + def _update_faces(self, resize=False): + # + # this procedure is only called, when geometry is not taken from npz-file + # + # create an 2dim array self.vface for each vertex + # which contains an array of faces, where the vertex belongs to, maximum of + # the array inside is self.MAX_FACES + # + # use self.nfaces as counter for the inner array, it is needed afterwards + + nverts = len(self.coord) + self.nfaces = np.zeros(nverts, dtype=np.uint8) # reset the counters to zero + + for idx, vert in enumerate(self.fvert): # contains faces with vertex number + for i in range( + 0, min(len(vert), self.vertsPerFaceForExport) + ): # use minimum of attached vertices, works for less than 3 also + vn = vert[i] # vertex.number + if self.nfaces[vn] >= self.MAX_FACES: + log.error( + "Failed to index faces of mesh %s, you are probably loading a mesh " + "with mixed nb of verts per face (do not mix tris and quads). " + "Or your mesh has too many faces attached to one vertex " + "(the maximum is %s-poles). In the second case, either increase " + "MAX_FACES for this mesh, or improve the mesh topology.", + self.name, + self.MAX_FACES, + ) + raise RuntimeError("Incompatible mesh topology.") + return + self.vface[vn, self.nfaces[vn]] = ( + idx # add the index of the face to the array, row given by vn, column by counter nfaces[vn] + ) + self.nfaces[vn] += 1 # now increment the counter + + imax = i + 1 # just needed later not to be recalculated. + + # in case this function is not called from catmull-clark function resize the self.vface to a minimum + if resize is True: + # now lets recalculate the maximum number of faces belonging to a plane + newmax = np.max(self.nfaces) + if ( + newmax > 2 + ): # Avoid the information when function calculates internal objects not to confuse users + log.debug("Calculated maximum number of faces for one vertex: %d", newmax) + + # unfortunately catmull-clark expects maxpoles and not maxfaces, so we need + # also to calculate max-poles + # + # for each vertex we have to check all attached faces, these faces are already collected in self.vface + # when we check all the faces we will collect the neighbors using a modulo function, the vertices are + # entered clockwise ... modulo allows us to use neighbor of 1 and 3 when index is 0 + # if not already existent, the neighbors are added to an array and the maximum size of this array will + # be maxpole + + maxpole = 0 + for vn, row in enumerate(self.vface): + noticed = np.zeros(self.MAX_FACES * 4, dtype=int) + m = 0 + for j in range(0, self.nfaces[vn]): + # collect neighbors + face = row[j] + for ix, v2 in enumerate(self.fvert[face]): + if v2 == vn: + ln = self.fvert[face][(ix - 1) % imax] + rn = self.fvert[face][(ix + 1) % imax] + if ln not in noticed: + noticed[m] = ln + m += 1 + if rn not in noticed: + noticed[m] = rn + m += 1 + break + if m > maxpole: + maxpole = m + + if ( + maxpole > 2 + ): # Avoid the information when function calculates internal objects not to confuse users + log.debug("Calculated maximum number of poles for one vertex: %d", maxpole) + + if newmax < maxpole: + newmax = maxpole + + # keep in mind that a maximum number of neighboring faces lower than 4 will crash subdiv + # (try to load a cube) + if newmax < 4: + newmax = 4 + + if newmax != self.MAX_FACES: + # it is different so resize vface + self.vface = np.delete(self.vface, np.s_[newmax::], 1) + self.MAX_FACES = newmax + + def getVertexWeights(self, parentWeights): + """ + Map armature weights mapped to the root parent (original mesh) to this + child mesh. Returns parentWeights unaltered if this mesh has no parent. + If this is a proxy mesh, parentWeights should be the weights mapped + through the proxy.getVertexWeights() method first. + + This particular vertex weights mapping is only used for exporting rigged + meshes, as in MakeHuman only unsubdivided meshes are posed, and then + smoothed in their posed state if required. Vertices are not removed + within MH when faces are hidden, either. + """ + if not hasattr(self, "parent") or not self.parent: + return parentWeights + + vmap = self.inverse_parent_map + vwmap = self.parent_map_weights + + from collections import OrderedDict + + weights = OrderedDict() + for bname, (verts, wghts) in list(parentWeights.data.items()): + vgroup = [] + empty = True + for v, wt in zip(verts, wghts): + mvs = vmap[v] + if isinstance(mvs, (int, np.int32)): + mvs = [mvs] + for mv in mvs: + w = vwmap[mv] + if mv > -1: + vgroup.append((mv, w * wt)) + empty = False + if not empty: + weights[bname] = vgroup + + return parentWeights.create(weights, self.getVertexCount()) + + def updateIndexBuffer(self): + self.updateIndexBufferVerts() + self.updateIndexBufferFaces() + + def updateIndexBufferVerts(self): + packed = self.fvert.astype(np.uint64) << 32 + packed |= self.fuvs + packed = packed.reshape(-1) + + u, rev = np.unique(packed, return_inverse=True) + del packed + + unwelded = u[:, None] >> np.array([[32, 0]], dtype=np.uint64) + unwelded = unwelded.astype(np.uint32) + nverts = len(unwelded) + iverts = rev.reshape(self.fvert.shape) + del rev, u + + self.vmap = unwelded[:, 0] + self.tmap = unwelded[:, 1] + self._inverse_vmap = None + del unwelded + + self.r_coord = np.empty((nverts, 3), dtype=np.float32) + self.r_texco = np.empty((nverts, 2), dtype=np.float32) + self.r_vnorm = np.zeros((nverts, 3), dtype=np.float32) + self.r_vtang = np.zeros((nverts, 4), dtype=np.float32) + self.r_color = np.zeros((nverts, 4), dtype=np.uint8) + 255 + + self.r_faces = np.array(iverts, dtype=np.uint32) + + def updateIndexBufferFaces(self): + index = self.r_faces[self.face_mask] + group = self.group[self.face_mask] + + if len(group) > 0: + order = np.argsort(group) + group = group[order] + index = index[order] + + group, start = np.unique(group, return_index=True) + count = np.empty(len(start), dtype=np.uint32) + count[:-1] = start[1:] - start[:-1] + count[-1] = len(index) - start[-1] + + grpix = np.zeros((max(self.group) + 1, 2), dtype=np.uint32) + grpix[group, 0] = start + grpix[group, 1] = count + else: + grpix = np.zeros((0, 2), dtype=np.uint32) + + self.index = index + self.grpix = grpix + + self.ucoor = True + self.unorm = True + self.utang = True + self.ucolr = True + self.utexc = True + self.sync_all() + + def sync_coord(self): + if self.ucoor is False: + return + if self.vmap is None or len(self.vmap) == 0: + return + if self.ucoor is True: + self.r_coord[...] = self.coord[self.vmap] + else: + self.r_coord[self.ucoor[self.vmap]] = self.coord[self.vmap][self.ucoor[self.vmap]] + self.ucoor = False + + def sync_norms(self): + if self.unorm is False: + return + if self.vmap is None or len(self.vmap) == 0: + return + if self.unorm is True: + self.r_vnorm[...] = self.vnorm[self.vmap] + else: + self.r_vnorm[self.unorm[self.vmap]] = self.vnorm[self.vmap][self.unorm[self.vmap]] + self.unorm = False + + def sync_tangents(self): + if not self.has_uv: + return + if self.utang is False: + return + if self.vmap is None or len(self.vmap) == 0: + return + if self.utang is True: + self.r_vtang[...] = self.vtang[self.vmap] + else: + self.r_vtang[self.utang[self.vmap]] = self.vtang[self.vmap][self.utang[self.vmap]] + self.utang = False + + def sync_color(self): + if self.ucolr is False: + return + if self.vmap is None or len(self.vmap) == 0: + return + if self.ucolr is True: + self.r_color[...] = self.color[self.vmap] + else: + self.r_color[self.ucolr[self.vmap]] = self.color[self.vmap][self.ucolr[self.vmap]] + self.ucolr = False + self._r_color_diff = None + + def sync_texco(self): + if not self.has_uv: + return + if self.utexc is False: + return + if self.tmap is None or len(self.tmap) == 0: + return + if self.utexc is True: + self.r_texco[...] = self.texco[self.tmap] + else: + self.r_texco[self.utexc[self.tmap]] = self.texco[self.tmap][self.utexc[self.tmap]] + self.utexc = False + + def sync_all(self): + self.sync_coord() + self.sync_norms() + if self.calculateTangents: + self.sync_tangents() + self.sync_color() + self.sync_texco() + + def createFaceGroup(self, name): + """ + Creates a new module3d.FaceGroup with the given name. + + :param name: The name for the face group. + :type name: [float, float, float] + :return: The new face group. + :rtype: :py:class:`module3d.FaceGroup` + """ + idx = len(self._faceGroups) + fg = FaceGroup(self, name, idx) + self._groups_rev[name] = fg + self._faceGroups.append(fg) + return fg + + def setColor(self, color): + """ + Sets the vertex colors for the entire object. + + :param color: The color in rgba. + :type color: [byte, byte, byte, byte] + """ + color = np.asarray(color, dtype=np.uint8) + if len(color.shape) == 1: # One color for all vertices + if len(color) == 3: + # Add alpha component to simple RGB color + color = list(color) + [255] + self.color[:] = color + else: + self.color[...] = color[None, :] + self.markCoords(colr=True) + self.sync_color() + + @property + def r_color_diff(self): + """ + Vertex colors multiplied with the diffuse material color. + """ + if self._r_color_diff is None: + self._r_color_diff = np.zeros(self.r_color.shape, dtype=np.uint8) + self._old_diff = None + + diff = self.material.diffuseColor.values + [self.material.opacity] + if diff != self._old_diff: + # Update diffuse * vertex colors + self._r_color_diff[:] = diff * self.r_color + self._old_diff = diff + return self._r_color_diff + + @property + def visibility(self): + return self._visibility + + def setVisibility(self, visible): + self.visibility = visible + + @visibility.setter + def visibility(self, visible): + """ + This method sets the visibility of the object. + + :param visible: Whether or not the object is visible. + :type visible: Boolean + """ + self._visibility = visible + + def setPickable(self, pickable): + """ + This method sets the pickable flag of the object. + + :param pickable: Whether or not the object is pickable. + :type pickable: Boolean + """ + + self.pickable = pickable + + def getPriority(self): + """ + The rendering priority of this object. + Objects with higher priorities will be drawn later. + + Common priorities used: + file description 2D/3D priority + + core/mhmain.py background-gradient 2D -200 + 0_modeling_background.py background 2D -90 + core/mhmain.py human 3D 0 + 3_libraries_clothes_chooser.py clothing 3D 10 + 3_libraries_polygon_hair_chooser.py hair 3D 20 + 2_posing_armature.py armature 3D 30 + 0_modeling_a_measurement.py measureMesh 2D 50 + 5_settings_censor.py censor rectangles 2D 80 + apps/gui/guifiles.py black frame 2D 90 + """ + return self._priority + + def setPriority(self, priority): + """ + Set the rendering priority of this object. + Objects with higher priorities will be drawn later. + + Common priorities used: + file description 2D/3D priority + + 0_modeling_background.py background 2D -90 + core/mhmain.py human 3D 0 + 3_libraries_clothes_chooser.py clothing 3D 10 + 3_libraries_polygon_hair_chooser.py hair 3D 20 + 2_posing_armature.py armature 3D 30 + 0_modeling_a_measurement.py measureMesh 2D 50 + 5_settings_censor.py censor rectangles 2D 80 + apps/gui/guifiles.py black frame 2D 90 + """ + self._priority = priority + + priority = property(getPriority, setPriority) + + @property + def material(self): + if not self.object and hasattr(self, "parent"): + return self.parent.material + return self.object.material + + def setTransparentPrimitives(self, transparentPrimitives): + """ + This method is used to specify the amount of transparent faces. + This property is overridden if self.material.transparent is set to True. + + :param transparentPrimitives: The amount of transparent faces. + :type transparentPrimitives: int + """ + self._transparentPrimitives = transparentPrimitives + + def getTransparentPrimitives(self): + # Object allows transparency rendering of only a subset (starting from + # the first face) of faces of the mesh, but material property transparent + # set to True overrides this and makes all faces render with transparency technique + if self.material.transparent: + return len(self.fvert) + else: + return self._transparentPrimitives + + transparentPrimitives = property(getTransparentPrimitives, setTransparentPrimitives) + + def getFaceGroup(self, name): + """ + This method searches the list of FaceGroups held for this object, and + returns the FaceGroup with the specified name. If no FaceGroup is found + for that name, this method returns None. + + :param name: The name of the FaceGroup to retrieve. + :type name: str + :return: The FaceGroup if found, None otherwise. + :rtype: :py:class:`module3d.FaceGroup` + """ + return self._groups_rev.get(name, None) + + def getFaceGroups(self): + """ + The names of the facegroups available on this mesh. + """ + return list(self._groups_rev.keys()) + + def getGroupMaskForGroups(self, groupNames): + groups = np.zeros(len(self._faceGroups), dtype=bool) + for name in groupNames: + groups[self._groups_rev[name].idx] = True + return groups + + def getFaceMaskForGroups(self, groupNames): + groups = self.getGroupMaskForGroups(groupNames) + face_mask = groups[self.group] + return face_mask + + def getFacesForGroups(self, groupNames): + face_mask = self.getFaceMaskForGroups(groupNames) + faces = np.argwhere(face_mask)[..., 0] + return faces + + def getVertexMaskForGroups(self, groupNames): + face_mask = self.getFaceMaskForGroups(groupNames) + verts = self.fvert[face_mask] + vert_mask = np.zeros(len(self.coord), dtype=bool) + vert_mask[verts] = True + return vert_mask + + def getVerticesForGroups(self, groupNames): + vert_mask = self.getVertexMaskForGroups(groupNames) + verts = np.argwhere(vert_mask)[..., 0] + return verts + + def getVerticesForFaceMask(self, face_mask): + verts = self.fvert[face_mask] + verts = verts.reshape(-1) + return verts + + def getVertexMaskForFaceMask(self, face_mask): + verts = self.getVerticesForFaceMask(face_mask) + vert_mask = np.zeros(len(self.coord), dtype=bool) + vert_mask[verts] = True + return vert_mask + + def getVertexAndFaceMasksForGroups(self, groupNames): + face_mask = self.getFaceMaskForGroups(groupNames) + verts = self.fvert[face_mask] + vert_mask = np.zeros(len(self.coord), dtype=bool) + vert_mask[verts] = True + return vert_mask, face_mask + + def getFaceMaskForVertices(self, verts): + """ + Get mask that selects all faces that are connected to the specified + vertices. + """ + mask = np.zeros(len(self.fvert), dtype=bool) + valid = ( + np.arange(self.MAX_FACES)[None, :] < self.nfaces[verts][:, None] + ) # Mask that filters out unused slots for faces connected to a vert + vface = self.vface[verts] + faces = vface[valid] + mask[faces] = True + return mask + + def getFacesForVertices(self, verts): + return np.argwhere(self.getFaceMaskForVertices(verts))[..., 0] + + def setCameraProjection(self, cameraMode): + """ + This method sets the camera mode used to visualize this object (fixed or movable). + The 3D engine has two camera modes (both perspective modes). + The first is moved by the mouse, while the second is fixed. + The first is generally used to model 3D objects (a human, clothes, + etc.), while the second is used for 3D GUI controls. + + :param cameraMode: The camera mode. 0 = movable camera (modelCamera); + 1 = static camera (guiCamera). + :type cameraMode: int + """ + + self.cameraMode = cameraMode + + def getCamera(self): + """ + The camera with which this mesh is rendered. + """ + from core import G + + return G.cameras[self.cameraMode] + + def update(self): + """ + This method is used to call the update methods on each of a list of vertices + or all vertices that form part of this object. Passes the current mesh state + to the OpenGL buffers. + """ + self.sync_all() + + def calcNormals( + self, recalcVertexNormals=1, recalcFaceNormals=1, verticesToUpdate=None, facesToUpdate=None + ): + """ + Updates the given vertex and/or face normals. + + :param recalcVertexNormals: A flag to indicate whether or not the vertex normals should be recalculated. + :type recalcVertexNormals: Boolean + :param recalcFaceNormals: A flag to indicate whether or not the face normals should be recalculated. + :type recalcFaceNormals: Boolean + :param verticesToUpdate: The list of vertices to be updated, if None all vertices are updated. + :type verticesToUpdate: list of :py:class:`module3d.Vert` + :param facesToUpdate: The list of faces to be updated, if None all faces are updated. + :type facesToUpdate: list of :py:class:`module3d.Face` + """ + + if recalcFaceNormals: + self.calcFaceNormals(facesToUpdate) + + if recalcVertexNormals: + self.calcVertexNormals(verticesToUpdate) + + if recalcFaceNormals or recalcVertexNormals and self.calculateTangents: + self.calcVertexTangents(verticesToUpdate) + + def calcBBox(self, ix=None, onlyVisible=True, fixedFaceMask=None): + """ + Calculates the axis aligned bounding box of this object in the object's coordinate system. + """ + # TODO maybe cache bounding box + if ix is None: + ix = np.s_[:] + if fixedFaceMask is not None: + verts = self.getVerticesForFaceMask(fixedFaceMask) + coord = self.coord[verts] + elif onlyVisible: + verts = self.getVerticesForFaceMask(self.getFaceMask()) + coord = self.coord[verts] + else: + coord = self.coord[ix] + if len(coord) == 0: + return np.zeros((2, 3), dtype=np.float32) + v0 = np.amin(coord, axis=0) + v1 = np.amax(coord, axis=0) + return np.vstack((v0, v1)) + + def __str__(self): + return "object3D Mesh named: %s, nverts: %s, nfaces: %s" % ( + self.name, + self.getVertexCount(), + self.getFaceCount(), + ) + + +def dot_v3(v3_arr1, v3_arr2): + """ + Numpy Ufunc'ed implementation of a series of dot products of two vector3 + objects. + """ + return ( + (v3_arr1[:, 0] * v3_arr2[:, 0]) + + (v3_arr1[:, 1] * v3_arr2[:, 1]) + + (v3_arr2[:, 2] * v3_arr1[:, 2]) + ) diff --git a/gr00t_wbc/control/teleop/gui/core/selection.py b/gr00t_wbc/control/teleop/gui/core/selection.py new file mode 100644 index 0000000..3400aab --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/core/selection.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +TODO +""" + + +class SelectionColorMap: + """ + The objects support the use of a technique called *Selection Using Unique + Color IDs*, that internally uses color-coding of components within the + scene to support the selection of objects by the user using the mouse. + + This technique generates a sequence of colors (color IDs), assigning a + unique color to each uniquely selectable object or component in the scene. + These colors are not displayed, but are used by MakeHuman to generates an + unseen image of the various selectable elements. This image uses the same + camera settings currently being used for the actual, on-screen image. + When the mouse is clicked, the position of the mouse is used with the + unseen image to retrieve a color. MakeHuman uses this color as an ID to + identify which object or component the user clicked with the mouse. + + This technique uses glReadPixels() to read the single pixel at the + current mouse location, using the unseen, color-coded image. + + For further information on this technique, see: + + - http://www.opengl.org/resources/faq/technical/selection.htm and + - http://wiki.gamedev.net/index.php/OpenGL_Selection_Using_Unique_Color_IDs + + **Note.** Because the 3D engine uses glDrawElements in a highly opimized + way and each vertex can have only one color ID, there there is a known + problem with selecting individual faces with very small FaceGroups using + this technique. However, this is not a major problem for MakeHuman, which + doesn't use such low polygon groupings. + + - **self.colorIDToFaceGroup**: *Dictionary of colors IDs* A dictionary of the color IDs used for + selection (see MakeHuman Selectors, above). + - **self.colorID**: *float list* A progressive color ID. + + The attributes *self.colorID* and *self.colorIDToFaceGroup* + support a technique called *Selection Using Unique Color IDs* to make each + FaceGroup independently clickable. + + The attribute *self.colorID* stores a progressive color that is incremented for each successive + FaceGroup added to the scene. + The *self.colorIDToFaceGroup* attribute contains a list that serves as a directory to map + each color back to the corresponding FaceGroup by using its color ID. + """ + + def __init__(self): + + self.colorIDToFaceGroup = {} + self.colorID = 0 + + def assignSelectionID(self, obj): + """ + This method generates a new, unique color ID for each FaceGroup, + within a particular Object3D object, that forms a part of this scene3D + object. This color ID can subsequently be used in a non-displayed + image map to determine the FaceGroup that a mouse click was made in. + + This method loops through the FaceGroups, assigning the next color + in the sequence to each subsequent FaceGroup. The color value is + written into a 'dictionary' to serve as a color ID that can be + translated back into the corresponding FaceGroup name when a mouse + click is detected. + This is part of a technique called *Selection Using Unique Color IDs* + to make each FaceGroup independently clickable. + + :param obj: The object3D object for which color dictionary entries need to be generated. + :type obj: module3d.Object 3D + """ + for g in obj.faceGroups: + self.colorID += 1 + + # 555 to 24-bit rgb + + idR = (self.colorID % 32) * 8 + idG = ((self.colorID >> 5) % 32) * 8 + idB = ((self.colorID >> 10) % 32) * 8 + + g.colorID = (idR, idG, idB) + + self.colorIDToFaceGroup[self.colorID] = g + + def getSelectedFaceGroup(self, picked): + """ + This method uses a non-displayed image containing color-coded faces + to return the index of the FaceGroup selected by the user with the mouse. + This is part of a technique called *Selection Using Unique Color IDs* to make each + FaceGroup independently clickable. + + :return: The selected face group. + :rtype: :py:class:`module3d.FaceGroup` + """ + # Force integer divide + IDkey = picked[0] // 8 | picked[1] // 8 << 5 | picked[2] // 8 << 10 # 555 + + # print "DEBUG COLOR PICKED: %s,%s,%s %s"%(picked[0], picked[1], picked[2], IDkey) + + try: + groupSelected = self.colorIDToFaceGroup[IDkey] + except Exception: + print("Color %s (%s) not found" % (IDkey, picked)) + # print (groupSelected.name) + # this print should only come on while debugging color picking + # print ('Color %s (%s) not found' % (IDkey, picked)) + groupSelected = None + return groupSelected + + def getSelectedFaceGroupAndObject(self, picked): + """ + This method determines whether a FaceGroup or a non-selectable zone has been + clicked with the mouse. It returns a tuple, showing the FaceGroup and the parent + Object3D object, or None. + If no object is picked, this method will simply print \"no clickable zone.\" + + :return: The selected face group and object. + :rtype: (:py:class:`module3d.FaceGroup`, :py:class:`module3d.Object3d`) + """ + + facegroupPicked = self.getSelectedFaceGroup(picked) + if facegroupPicked: + objPicked = facegroupPicked.parent + return (facegroupPicked, objPicked) + else: + # this print should only be made while debugging picking + # print ('not a clickable zone') + return None + + +selectionColorMap = SelectionColorMap() diff --git a/gr00t_wbc/control/teleop/gui/library/getpath.py b/gr00t_wbc/control/teleop/gui/library/getpath.py new file mode 100755 index 0000000..d116e18 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/getpath.py @@ -0,0 +1,446 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Jonas Hauquier, Glynn Clements, Joel Palmius, Marc Flerackers + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Utility module for finding the user home path. +""" + +import os +import sys + +from library.xdg_parser import XDG_PATHS + +__home_path = None + +# Search for an optional configuration file, providing another location for the home folder. +# The encoding of the file must be utf-8 and an absolute path is expected. + +if sys.platform.startswith("linux"): + + configFile = os.path.expanduser("~/.config/makehuman.conf") + +elif sys.platform.startswith("darwin"): + + configFile = os.path.expanduser("~/Library/Application Support/MakeHuman/makehuman.conf") + +elif sys.platform.startswith("win32"): + + configFile = os.path.join(os.getenv("LOCALAPPDATA", ""), "makehuman.conf") + +else: + + configFile = "" + +configPath = "" + +if os.path.isfile(configFile): + with open(configFile, "r", encoding="utf-8") as f: + configPath = f.readline().strip() + + if os.path.isdir(configPath): + __home_path = os.path.normpath(configPath).replace("\\", "/") + + +def pathToUnicode(path): + """ + Unicode representation of the filename. + Bytes is decoded with the codeset used by the filesystem of the operating + system. + Unicode representations of paths are fit for use in GUI. + """ + + if isinstance(path, bytes): + # Approach for bytes string type + try: + return str(path, "utf-8") + except UnicodeDecodeError: + pass + try: + return str(path, sys.getfilesystemencoding()) + except UnicodeDecodeError: + pass + try: + return str(path, sys.getdefaultencoding()) + except UnicodeDecodeError: + pass + try: + import locale + + return str(path, locale.getpreferredencoding()) + except UnicodeDecodeError: + return path + else: + return path + + +def formatPath(path): + if path is None: + return None + return pathToUnicode(os.path.normpath(path).replace("\\", "/")) + + +def canonicalPath(path): + """ + Return canonical name for location specified by path. + Useful for comparing paths. + """ + return formatPath(os.path.realpath(path)) + + +def localPath(path): + """ + Returns the path relative to the MH program directory, + i.e. the inverse of canonicalPath. + """ + path = os.path.realpath(path) + root = os.path.realpath(getSysPath()) + return formatPath(os.path.relpath(path, root)) + + +def getHomePath(): + """ + Find the user home path. + Note: If you are looking for MakeHuman data, you probably want getPath()! + """ + # Cache the home path + global __home_path + + # The environment variable MH_HOME_LOCATION will supersede any other settings for the home folder. + alt_home_path = os.environ.get("MH_HOME_LOCATION", "") + if os.path.isdir(alt_home_path): + __home_path = formatPath(alt_home_path) + + if __home_path is not None: + return __home_path + + # Windows + if sys.platform.startswith("win"): + import winreg + + keyname = r"Software\Microsoft\Windows\CurrentVersion\Explorer\User Shell Folders" + # name = 'Personal' + with winreg.OpenKey(winreg.HKEY_CURRENT_USER, keyname) as k: + try: + value, type_ = winreg.QueryValueEx(k, "Personal") + except FileNotFoundError: + value, type_ = r"%USERPROFILE%\Documents", winreg.REG_EXPAND_SZ + if type_ == winreg.REG_EXPAND_SZ: + __home_path = formatPath(winreg.ExpandEnvironmentStrings(value)) + return __home_path + elif type_ == winreg.REG_SZ: + __home_path = formatPath(value) + return __home_path + else: + raise RuntimeError("Couldn't determine user folder") + + # Linux + elif sys.platform.startswith("linux"): + doc_folder = XDG_PATHS.get("DOCUMENTS", "") + if os.path.isdir(doc_folder): + __home_path = doc_folder + else: + __home_path = pathToUnicode(os.path.expanduser("~")) + + # Darwin + else: + __home_path = os.path.expanduser("~") + + return __home_path + + +def getPath(subPath=""): + """ + Get MakeHuman folder that contains per-user files, located in the user home + path. + """ + path = getHomePath() + + # Windows + if sys.platform.startswith("win"): + path = os.path.join(path, "makehuman") + + # MAC OSX + elif sys.platform.startswith("darwin"): + path = os.path.join(path, "Documents") + path = os.path.join(path, "MakeHuman") + + # Unix/Linux + else: + path = os.path.join(path, "makehuman") + + path = os.path.join(path, "v1py3") + + if subPath: + path = os.path.join(path, subPath) + + return formatPath(path) + + +def getDataPath(subPath=""): + """ + Path to per-user data folder, should always be the same as getPath('data'). + """ + if subPath: + path = getPath(os.path.join("data", subPath)) + else: + path = getPath("data") + return formatPath(path) + + +def getSysDataPath(subPath=""): + """ + Path to the data folder that is installed with MakeHuman system-wide. + NOTE: do NOT assume that getSysPath("data") == getSysDataPath()! + """ + if subPath: + path = getSysPath(os.path.join("data", subPath)) + else: + path = getSysPath("data") + return formatPath(path) + + +def getSysPath(subPath=""): + """ + Path to the system folder where MakeHuman is installed (it is possible that + data is stored in another path). + Writing to this folder or modifying this data usually requires admin rights, + contains system-wide data (for all users). + """ + if subPath: + path = os.path.join(".", subPath) + else: + path = "." + return formatPath(path) + + +def _allnamesequal(name): + return all(n == name[0] for n in name[1:]) + + +def commonprefix(paths, sep="/"): + """ + Implementation of os.path.commonprefix that works as you would expect. + + Source: http://rosettacode.org/wiki/Find_Common_Directory_Path#Python + """ + from itertools import takewhile + + bydirectorylevels = list(zip(*[p.split(sep) for p in paths])) + return sep.join(x[0] for x in takewhile(_allnamesequal, bydirectorylevels)) + + +def isSubPath(subpath, path): + """ + Verifies whether subpath is within path. + """ + subpath = canonicalPath(subpath) + path = canonicalPath(path) + return commonprefix([subpath, path]) == path + + +def isSamePath(path1, path2): + """ + Determines whether two paths point to the same location. + """ + return canonicalPath(path1) == canonicalPath(path2) + + +def getRelativePath(path, relativeTo=[getDataPath(), getSysDataPath()], strict=False): + """ + Return a relative file path, relative to one of the specified search paths. + First valid path is returned, so order in which search paths are given matters. + """ + if not isinstance(relativeTo, list): + relativeTo = [relativeTo] + + relto = None + for p in relativeTo: + if isSubPath(path, p): + relto = p + if relto is None: + if strict: + return None + else: + return path + + relto = os.path.abspath(os.path.realpath(relto)) + path = os.path.abspath(os.path.realpath(path)) + rpath = os.path.relpath(path, relto) + return formatPath(rpath) + + +def findFile(relPath, searchPaths=[getDataPath(), getSysDataPath()], strict=False): + """ + Inverse of getRelativePath: find an absolute path from specified relative + path in one of the search paths. + First occurence is returned, so order in which search paths are given matters. + Note: does NOT treat the path as relative to the current working dir, unless + you explicitly specify '.' as one of the searchpaths. + """ + if not isinstance(searchPaths, list): + searchPaths = [searchPaths] + + for dataPath in searchPaths: + path = os.path.join(dataPath, relPath) + if os.path.isfile(path): + return formatPath(path) + + if strict: + return None + else: + return relPath + + +def thoroughFindFile(filename, searchPaths=[], searchDefaultPaths=True): + """ + Extensively search the data paths to find a file with matching filename in + as much cases as possible. If file is found, returns absolute filename. + If nothing is found return the most probable filename. + """ + # Ensure unix style path + filename.replace("\\", "/") + + if not isinstance(searchPaths, list): + searchPaths = [searchPaths] + + if searchDefaultPaths: + # Search in user / sys data, and user / sys root folders + searchPaths = list(searchPaths) + searchPaths.extend([getDataPath(), getSysDataPath(), getPath(), getSysPath()]) + + path = findFile(filename, searchPaths, strict=True) + if path: + return canonicalPath(path) + + # Treat as absolute path or search relative to application path + if os.path.isfile(filename): + return canonicalPath(filename) + + # Strip leading data/ folder if present (for the scenario where sysDataPath is not in sysPath) + if filename.startswith("data/"): + result = thoroughFindFile(filename[5:], searchPaths, False) + if os.path.isfile(result): + return result + + # Nothing found + return formatPath(filename) + + +def search(paths, extensions, recursive=True, mutexExtensions=False): + """ + Search for files with specified extensions in specified paths. + If mutexExtensions is True, no duplicate files with only differing extension + will be returned. Instead, only the file with highest extension precedence + (extensions occurs earlier in the extensions list) is kept. + """ + if isinstance(paths, str): + paths = [paths] + if isinstance(extensions, str): + extensions = [extensions] + extensions = [e[1:].lower() if e.startswith(".") else e.lower() for e in extensions] + + if mutexExtensions: + discovered = dict() + + def _aggregate_files_mutexExt(filepath): + basep, ext = os.path.splitext(filepath) + ext = ext[1:] + if basep in discovered: + if extensions.index(ext) < extensions.index(discovered[basep]): + discovered[basep] = ext + else: + discovered[basep] = ext + + if recursive: + for path in paths: + for root, dirs, files in os.walk(path): + for f in files: + ext = os.path.splitext(f)[1][1:].lower() + if ext in extensions: + if mutexExtensions: + _aggregate_files_mutexExt(os.path.join(root, f)) + else: + yield pathToUnicode(os.path.join(root, f)) + else: + for path in paths: + if not os.path.isdir(path): + continue + for f in os.listdir(path): + f = os.path.join(path, f) + if os.path.isfile(f): + ext = os.path.splitext(f)[1][1:].lower() + if ext in extensions: + if mutexExtensions: + _aggregate_files_mutexExt(f) + else: + yield pathToUnicode(f) + + if mutexExtensions: + for f in ["%s.%s" % (p, e) for p, e in list(discovered.items())]: + yield pathToUnicode(f) + + +def getJailedPath(filepath, relativeTo, jailLimits=[getDataPath(), getSysDataPath()]): + """ + Get a path to filepath, relative to relativeTo path, confined within the + jailLimits folders. Returns None if the path would fall outside of the jail. + This is a portable path which can be used for distributing eg. materials + (texture paths are portable). + Returns None if the filepath falls outside of the jail folders. Returns + a path to filename relative to relativeTo path if it is a subpath of it, + else returns a path relative to the jailLimits. + """ + + def _withinJail(path): + for j in jailLimits: + if isSubPath(path, j): + return True + return False + + # These paths may become messed up when using symlinks for user home. + # Make sure we use the same paths when calculating relative paths. + filepath = os.path.realpath(filepath) + + if relativeTo is str: + relativeTo = os.path.realpath(relativeTo) + + output = None + + if _withinJail(filepath): + relPath = getRelativePath(filepath, relativeTo, strict=True) + if relPath: + output = relPath + else: + output = getRelativePath(filepath, jailLimits) + return output diff --git a/gr00t_wbc/control/teleop/gui/library/image.py b/gr00t_wbc/control/teleop/gui/library/image.py new file mode 100644 index 0000000..31a4bdb --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/image.py @@ -0,0 +1,358 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Image class definition +====================== + +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +The image module contains the definition of the Image class, the container +that MakeHuman uses to handle images. + +Image only depends on the numpy library, except when image have to be loaded +or saved to disk, in which case one of the back-ends (Qt or PIL) will have to +be imported (import happens only when needed). +""" + +import time + +import numpy as np + +FILTER_NEAREST = 0 # Nearest neighbour resize filter (no real filtering) +FILTER_BILINEAR = 1 # Bi-linear filter +FILTER_BICUBIC = 2 # Bi-cubic filter (not supported with Qt, only PIL) + + +class Image(object): + """Container for handling images. + + It is equipped with the necessary methods that one needs for loading + and saving images and fetching their data, as well as many properties + providing information about the loaded image. + """ + + def __init__(self, path=None, width=0, height=0, bitsPerPixel=32, components=None, data=None): + """Image constructor. + + The Image can be constructed by an existing Image, a QPixmap, a + QImage, loaded from a file path, or created empty. + + To construct the Image by copying the data from the source, + pass the source as the first argument of the constructor. + ``dest = Image(source)`` + + To create the Image by sharing the data of another Image, pass + the source Image as the data argument. + ``dest = Image(data=sharedsource)`` + + The data argument can be a numpy array of 3 dimensions (w, h, c) which + will be used directly as the image's data, where w is the width, h is + the height, and c is the number of channels. + The data argument can also be a path to load. + + To create an empty Image, leave path=None, and specify the width and + height. You can then optionally adjust the new Image's channels by + setting bitsPerPixel = 8, 16, 24, 32, or components = 1, 2, 3, 4, + which are equivalent to W (Grayscale), WA (Grayscale with Alpha), + RGB, and RGBA respectively. + """ + import image_qt as image_lib + + if path is not None: + self._is_empty = False + if isinstance(path, Image): + # Create a copy of the image. + self._data = path.data.copy() + elif _isQPixmap(path): + qimg = path.toImage() + self._data = image_lib.load(qimg) + else: # Path string / QImage. + self._data = image_lib.load(path) + self.sourcePath = path + elif data is not None: + self._is_empty = False + if isinstance(data, Image): + # Share data between images. + self._data = data.data + elif isinstance(data, str): + self._data = image_lib.load(data) + else: # Data array. + self._data = data + else: + self._is_empty = True + if components is None: + if bitsPerPixel == 32: + components = 4 + elif bitsPerPixel == 24: + components = 3 + else: + raise NotImplementedError("bitsPerPixel must be 24 or 32") + self._data = np.empty((height, width, components), dtype=np.uint8) + self._data = np.ascontiguousarray(self._data) + + self.modified = time.time() + + @property + def size(self): + """Return the size of the Image as a (width, height) tuple.""" + h, w, c = self._data.shape + return (w, h) + + @property + def width(self): + """Return the width of the Image in pixels.""" + h, w, c = self._data.shape + return w + + @property + def height(self): + """Return the height of the Image in pixels.""" + h, w, c = self._data.shape + return h + + @property + def components(self): + """Return the number of the Image channels.""" + h, w, c = self._data.shape + return c + + @property + def bitsPerPixel(self): + """Return the number of bits per pixel used for the Image.""" + h, w, c = self._data.shape + return c * 8 + + @property + def data(self): + """Return the numpy ndarray that contains the Image data.""" + return self._data + + def save(self, path): + """Save the Image to a file.""" + import image_qt as image_lib + + image_lib.save(path, self._data) + + def toQImage(self): + """ + Get a QImage copy of this Image. + Useful when the image should be shown in a Qt GUI + """ + import image_qt + + # return image_qt.toQImage(self.data) + # ^ For some reason caused problems + if self.components == 1: + fmt = image_qt.QtGui.QImage.Format_RGB888 + h, w, c = self.data.shape + data = np.repeat(self.data[:, :, 0], 3).reshape((h, w, 3)) + elif self.components == 2: + fmt = image_qt.QtGui.QImage.Format_ARGB32 + h, w, c = self.data.shape + data = np.repeat(self.data[:, :, 0], 3).reshape((h, w, 3)) + data = np.insert(data, 3, values=self.data[:, :, 1], axis=2) + elif self.components == 3: + """ + fmt = image_qt.QtGui.QImage.Format_RGB888 + data = self.data + """ + # The above causes a crash or misaligned image raster. + # Quickhack solution: + fmt = image_qt.QtGui.QImage.Format_ARGB32 + _data = self.convert(components=4).data + # There appear to be channel mis-alignments, another hack: + data = np.zeros(_data.shape, dtype=_data.dtype) + data[:, :, :] = _data[:, :, [2, 1, 0, 3]] + else: + # components == 4 + fmt = image_qt.QtGui.QImage.Format_ARGB32 + data = self.data + return image_qt.QtGui.QImage(data.tostring(), data.shape[1], data.shape[0], fmt) + + def resized_(self, width, height, filter=FILTER_NEAREST): + if filter == FILTER_NEAREST: + dw, dh = width, height + sw, sh = self.size + xmap = np.floor((np.arange(dw) + 0.5) * sw / dw).astype(int) + ymap = np.floor((np.arange(dh) + 0.5) * sh / dh).astype(int) + return self._data[ymap, :][:, xmap] + else: + # NOTE: bi-cubic filtering is not supported by Qt, use bi-linear + import image_qt + + return image_qt.resized(self, width, height, filter=filter) + + def resized(self, width, height, filter=FILTER_NEAREST): + """Get a resized copy of the Image.""" + return Image(data=self.resized_(width, height, filter)) + + def resize(self, width, height, filter=FILTER_NEAREST): + """Resize the Image to a specified size.""" + self._data = self.resized_(width, height, filter) + self.modified = time.time() + + def blit(self, other, x, y): + """Copy the contents of an Image to another. + The target image may have a different size.""" + dh, dw, dc = self._data.shape + sh, sw, sc = other._data.shape + if sc != dc: + raise ValueError("source image has incorrect format") + sw = min(sw, dw - x) + sh = min(sh, dh - y) + self._data[y : y + sh, x : x + sw, :] = other._data + + self.modified = time.time() + + def flip_vertical(self): + """Turn the Image upside down.""" + return Image(data=self._data[::-1, :, :]) + + def flip_horizontal(self): + """Flip the Image in the left-right direction.""" + return Image(data=self._data[:, ::-1, :]) + + def __getitem__(self, xy): + """Get the color of a specified pixel by using the + square brackets operator. + + Example: my_color = my_image[(17, 42)]""" + if not isinstance(xy, tuple) or len(xy) != 2: + raise TypeError("tuple of length 2 expected") + + x, y = xy + + if not isinstance(x, int) or not isinstance(y, int): + raise TypeError("tuple of 2 ints expected") + + if x < 0 or x >= self.width or y < 0 or y >= self.height: + raise IndexError("element index out of range") + + pix = self._data[y, x, :] + if self.components == 4: + return (pix[0], pix[1], pix[2], pix[3]) + elif self.components == 3: + return (pix[0], pix[1], pix[2], 255) + elif self.components == 2: + return (pix[0], pix[0], pix[0], pix[1]) + elif self.components == 1: + return (pix[0], pix[0], pix[0], 255) + else: + return None + + def __setitem__(self, xy, color): + """Set the color of a pixel using the square brackets + operator. + + Example: my_image[(17, 42)] = (0, 255, 64, 255)""" + if not isinstance(xy, tuple) or len(xy) != 2: + raise TypeError("tuple of length 2 expected") + + x, y = xy + + if not isinstance(x, int) or not isinstance(y, int): + raise TypeError("tuple of 2 ints expected") + + if x < 0 or x >= self.width or y < 0 or y >= self.height: + raise IndexError("element index out of range") + + if not isinstance(color, tuple): + raise TypeError("tuple expected") + + self._data[y, x, :] = color + self.modified = time.time() + + def convert(self, components): + """Convert the Image to a different color format. + + 'components': The number of color channels the Image + will have after the conversion.""" + if self.components == components: + return self + + hasAlpha = self.components in (2, 4) + needAlpha = components in (2, 4) + + if hasAlpha: + alpha = self._data[..., -1] + color = self._data[..., :-1] + else: + alpha = None + color = self._data + + isMono = self.components in (1, 2) + toMono = components in (1, 2) + + if isMono and not toMono: + color = np.dstack((color, color, color)) + elif toMono and not isMono: + color = np.sum(color.astype(np.uint16), axis=-1) / 3 + color = color.astype(np.uint8)[..., None] + + if needAlpha and alpha is None: + alpha = np.zeros_like(color[..., :1]) + 255 + + if needAlpha: + data = np.dstack((color, alpha)) + else: + data = color + + return type(self)(data=data) + + def markModified(self): + """Mark the Image as modified.""" + self.modified = time.time() + self._is_empty = False + + @property + def isEmpty(self): + """ + Returns True if the Image is empty or new. + Returns False if the Image contains data or has been modified. + """ + return self._is_empty + + +def _isQPixmap(img): + """ + Test an image object for being a QPixmap instance if Qt libraries were + loaded in the application. + """ + import sys + + if "PyQt5" in list(sys.modules.keys()): + import image_qt + + return isinstance(img, image_qt.QtGui.QPixmap) + else: + return False diff --git a/gr00t_wbc/control/teleop/gui/library/language.py b/gr00t_wbc/control/teleop/gui/library/language.py new file mode 100644 index 0000000..9935843 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/language.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Translation and localization module. + +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Code Home Page:** http://code.google.com/p/makehuman/ + +**Authors:** Joel Palmius, Marc Flerackers, Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Language file loading and translation. +""" + +from collections import OrderedDict +import json +import os + +from getpath import getDataPath, getSysDataPath +import log + + +class Language(object): + def __init__(self): + self.language = None + self.languageStrings = None + self.missingStrings = OrderedDict() + self.rtl = False + + def setLanguage(self, lang): + self.languageStrings = None + path = os.path.join(getSysDataPath("languages/"), lang + ".json") + if not os.path.isfile(path): + return + with open(path, "r", encoding="utf-8") as f: + try: + self.languageStrings = json.loads(f.read()) + except Exception: + print("Error in language file %s" % lang) + log.error("Error in language file %s", lang, exc_info=True) + self.languageStrings = None + self.language = lang + self.rtl = False + if self.languageStrings and "__options__" in self.languageStrings: + self.rtl = self.languageStrings["__options__"].get("rtl", False) + + def getLanguageString(self, string, appendData=None, appendFormat=None): + if not string: + return string + if string == "%%s": + return string + if isinstance(string, list): + if len(string) == 2: + appendData = string[1] + string = string[0] + else: + if len(string) == 3: + appendFormat = string[2] + appendData = string[1] + string = string[0] + + if self.languageStrings is None: + result = string + else: + result = self.languageStrings.get(string) + + if result is not None: + if appendData is not None: + if appendFormat is not None: + result = result + (appendFormat % appendData) + else: + result = result + appendData + return result + self.missingStrings[string] = None + return string + + def dumpMissingStrings(self): + if not self.language: + return + path = os.path.join(getSysDataPath("languages/"), self.language + ".missing") + pathdir = os.path.dirname(path) + if not os.path.isdir(pathdir): + os.makedirs(pathdir) + with open(path, "w", encoding="utf-8") as f: + for string in self.missingStrings.keys(): + if self.language == "master": + f.write( + '"%s": "%s",\n' + % ( + string.replace("\n", "\\n").encode("utf8"), + string.replace("\n", "\\n").encode("utf8"), + ) + ) + else: + f.write('"%s": "",\n' % string.replace("\n", "\\n").encode("utf8")) + + +def getLanguages(): + """ + The languages available on this MH installation, by listing all .json + files in the languages folder in user and system data path. + """ + langDirFiles = os.listdir(getSysDataPath("languages")) + try: + langDirFiles = langDirFiles + os.listdir(getDataPath("languages")) + except Exception: + print("No user data path") + return ["english"] + [ + os.path.basename(filename).replace(".json", "") + for filename in sorted(langDirFiles) + if filename.split(os.extsep)[-1] == "json" + ] + + +language = Language() diff --git a/gr00t_wbc/control/teleop/gui/library/log.py b/gr00t_wbc/control/teleop/gui/library/log.py new file mode 100644 index 0000000..0a9ffd5 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/log.py @@ -0,0 +1,283 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Logging. + +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Logging component. To be used instead of print statements. +""" + +import code +import logging +from logging import CRITICAL, DEBUG, ERROR, INFO, WARNING +import logging.config +import os + +from library.getpath import getPath, getSysDataPath +from library.universal import G + +NOTICE = 25 +MESSAGE = logging.INFO + +LEVEL_TO_STR = { + DEBUG: "debug", + INFO: "info", + WARNING: "warning", + ERROR: "error", + CRITICAL: "critical", + NOTICE: "notice", +} + + +def logLevelToStr(levelCode): + if levelCode in LEVEL_TO_STR: + return LEVEL_TO_STR[levelCode] + else: + levels = sorted(LEVEL_TO_STR.keys()) + i = 0 + while i < len(levels) and levelCode < levels[i]: + i += 1 + i = min(i, len(levels) - 1) + return levels[i].upper() + + +def _toUnicode(msg, *args): + """ + Unicode representation of the formatted message. + String is decoded with the codeset used by the filesystem of the operating + system. + """ + try: + msg_ = msg % args + except TypeError: + # Also allow dict with keywords in format string, passed as first arg + if len(args) == 1 and isinstance(args[0], dict): + msg_ = msg % args[0] + else: + raise + + if isinstance(msg_, bytes): + return str(msg_, encoding="utf-8") + else: + return msg_ + + +def debug(msg, *args, **kwargs): + try: + logging.debug(msg, *args, **kwargs) + except UnicodeError: + msg_ = _toUnicode(msg, args) + logging.debug(msg_, kwargs) + + +def warning(msg, *args, **kwargs): + try: + logging.warning(msg, *args, **kwargs) + except UnicodeError: + msg_ = _toUnicode(msg, args) + logging.warning(msg_, kwargs) + + +def error(msg, *args, **kwargs): + try: + logging.error(msg, *args, **kwargs) + except UnicodeError: + msg_ = _toUnicode(msg, args) + logging.error(msg_, kwargs) + + +def message(msg, *args, **kwargs): + try: + logging.info(msg, *args, **kwargs) + except UnicodeError: + msg_ = _toUnicode(msg, args) + logging.info(msg_, kwargs) + + +# We have to make notice() appear to have been defined in the logging module +# so that logging.findCaller() finds its caller, not notice() itself +# This is required for the pathname, filename, module, funcName and lineno +# members of the LogRecord refer to the caller rather than to notice() itself. + +_notice_src = r""" +def notice(format, *args, **kwargs): + logging.log(NOTICE, format, *args, **kwargs) +""" +try: + exec(code.compile_command(_notice_src, logging.info.__code__.co_filename)) +except Exception: + print("Error defining notice() function in log.py") + + def notice(format, *args, **kwargs): + logging.log(NOTICE, format, *args, **kwargs) + + +logging.addLevelName(NOTICE, "NOTICE") +logging.addLevelName(MESSAGE, "MESSAGE") + + +def _splitpath(path): + + head, tail = os.path.split(path) + if tail == "": + return [head] + return _splitpath(head) + [tail] + + +class NoiseFilter(logging.Filter): + def filter(self, record): + try: + if record.msg.endswith(":\n%s"): + record.msg = record.msg[:-4] + record.args = record.args[:-1] + except Exception as e: + print("Error in NoiseFilter", e) + import traceback + + traceback.print_exc() + return True + + +class DowngradeFilter(logging.Filter): + def __init__(self, level): + super(DowngradeFilter, self).__init__() + self.level = level + + def filter(self, record): + try: + if record.levelno > self.level: + record.levelno = self.level + record.levelname = logging.getLevelName(record.levelno) + except Exception as e: + print("Error in DowngradeFilter", e) + import traceback + + traceback.print_exc() + return True + + +_logLevelColors = { + DEBUG: "grey", + NOTICE: "blue", + INFO: "blue", + WARNING: "darkorange", + ERROR: "red", + CRITICAL: "red", +} + + +def getLevelColor(logLevel): + global _logLevelColors + if logLevel not in _logLevelColors: + warning("Unknown log level color %s (%s)" % (logLevel, logLevelToStr(logLevel))) + return _logLevelColors.get(logLevel, "red") + + +class SplashLogHandler(logging.Handler): + def emit(self, record): + if G.app is not None and G.app.splash is not None: + G.app.splash.logMessage(self.format(record).split("\n", 1)[0] + "\n") + + +class StatusLogHandler(logging.Handler): + def emit(self, record): + if G.app is not None and G.app.statusBar is not None and record.levelno >= ERROR: + msg = "An ERROR Occurred. Check Utilities/Logs For More Information! Error Message: {:s}".format( + record.getMessage() + ) + G.app.statusBar.temporaryMessage("%s", msg, msec=10000) + + +class ApplicationLogHandler(logging.Handler): + def emit(self, record): + if G.app is not None and G.app.log_window is not None: + G.app.addLogMessage(self.format(record), record.levelno) + + +_logger_notice_src = r""" +def _logger_notice(self, msg, *args, **kwargs): + self.log(NOTICE, msg, *args, **kwargs) +""" +try: + exec(code.compile_command(_logger_notice_src, logging.info.__code__.co_filename)) +except Exception as e: + print("Error defining _logger_notice() function in log.py", e) + + def _logger_notice(self, format, *args, **kwargs): + self.log(NOTICE, format, *args, **kwargs) + + +class Logger(logging.Logger): + message = logging.Logger.info + notice = _logger_notice + + +def init(): + def config(): + userDir = getPath("") + defaults = dict(mhUserDir=userDir.replace("\\", "/")) + + try: + filename = os.path.join(userDir, "logging.ini") + if os.path.isfile(filename): + logging.config.fileConfig(filename, defaults) + return + except Exception: + pass + + try: + logging.config.fileConfig(getSysDataPath("logging.ini"), defaults) + return + except Exception: + pass + + try: + logging.basicConfig(level=logging.DEBUG) + return + except Exception: + pass + + logging.setLoggerClass(Logger) + + config() + + logging.captureWarnings(True) + + try: + logging.getLogger("OpenGL.formathandler").addFilter(NoiseFilter()) + logging.getLogger("OpenGL.extensions").addFilter(DowngradeFilter(logging.DEBUG)) + except Exception: + import traceback + + traceback.print_exc() diff --git a/gr00t_wbc/control/teleop/gui/library/matrix.py b/gr00t_wbc/control/teleop/gui/library/matrix.py new file mode 100644 index 0000000..82c3340 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/matrix.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Numpy powered matrix transformations. +""" + +import math + +import numpy as np + + +def transform(m, v): + return np.asarray(m * np.asmatrix(v).T)[:, 0] + + +def transform3(m, v): + x, y, z = v + v = np.asmatrix((x, y, z, 1)).T + v = np.asarray(m * v)[:, 0] + return v[:3] / v[3] + + +def magnitude(v): + return math.sqrt(np.sum(v**2)) + + +def normalize(v): + m = magnitude(v) + if m == 0: + return v + return v / m + + +def ortho(l1, r1, b1, t1, n1, f1): + dx = r1 - l1 + dy = t1 - b1 + dz = f1 - n1 + rx = -(r1 + l1) / (r1 - l1) + ry = -(t1 + b1) / (t1 - b1) + rz = -(f1 + n1) / (f1 - n1) + return np.matrix( + [[2.0 / dx, 0, 0, rx], [0, 2.0 / dy, 0, ry], [0, 0, -2.0 / dz, rz], [0, 0, 0, 1]] + ) + + +def perspective(fovy, aspect, n, f): + s = 1.0 / math.tan(math.radians(fovy) / 2.0) + sx, sy = s / aspect, s + zz = (f + n) / (n - f) + zw = 2 * f * n / (n - f) + return np.matrix([[sx, 0, 0, 0], [0, sy, 0, 0], [0, 0, zz, zw], [0, 0, -1, 0]]) + + +def frustum(x0, x1, y0, y1, z0, z1): + a = (x1 + x0) / (x1 - x0) + b = (y1 + y0) / (y1 - y0) + c = -(z1 + z0) / (z1 - z0) + d = -2 * z1 * z0 / (z1 - z0) + sx = 2 * z0 / (x1 - x0) + sy = 2 * z0 / (y1 - y0) + return np.matrix([[sx, 0, a, 0], [0, sy, b, 0], [0, 0, c, d], [0, 0, -1, 0]]) + + +def translate(xyz): + x, y, z = xyz + return np.matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]]) + + +def scale(xyz): + x, y, z = xyz + return np.matrix([[x, 0, 0, 0], [0, y, 0, 0], [0, 0, z, 0], [0, 0, 0, 1]]) + + +def _sincos(a): + a = math.radians(a) + return math.sin(a), math.cos(a) + + +def rotate(a, xyz): + x, y, z = normalize(xyz) + s, c = _sincos(a) + nc = 1 - c + return np.matrix( + [ + [x * x * nc + c, x * y * nc - z * s, x * z * nc + y * s, 0], + [y * x * nc + z * s, y * y * nc + c, y * z * nc - x * s, 0], + [x * z * nc - y * s, y * z * nc + x * s, z * z * nc + c, 0], + [0, 0, 0, 1], + ] + ) + + +def rotx(a): + s, c = _sincos(a) + return np.matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) + + +def roty(a): + s, c = _sincos(a) + return np.matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) + + +def rotz(a): + s, c = _sincos(a) + return np.matrix([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) + + +def lookat(eye, target, up): + F = target[:3] - eye[:3] + f = normalize(F) + U = normalize(up[:3]) + s = np.cross(f, U) + u = np.cross(s, f) + M = np.matrix(np.identity(4)) + M[:3, :3] = np.vstack([s, u, -f]) + T = translate(-eye) + return M * T + + +def viewport(x, y, w, h): + x, y, w, h = list(map(float, (x, y, w, h))) + return np.matrix( + [[w / 2, 0, 0, x + w / 2], [0, h / 2, 0, y + h / 2], [0, 0, 0.5, 0.5], [0, 0, 0, 1]] + ) diff --git a/gr00t_wbc/control/teleop/gui/library/mh.py b/gr00t_wbc/control/teleop/gui/library/mh.py new file mode 100644 index 0000000..b561857 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/mh.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +Python compatibility layer replacing the old C functions of MakeHuman. +""" + +from library.universal import G + +cameras = G.cameras + + +def setClearColor(r, g, b, a): + G.clearColor = (r, g, b, a) + + +def setCaption(caption): + G.app.mainwin.setWindowTitle(caption) + + +def changeCategory(category): + G.app.mainwin.tabs.changeTab(category) + + +def changeTask(category, task): + if not G.app.mainwin.tabs.findTab(category): + return + changeCategory(category) + G.app.mainwin.tabs.findTab(category).child.changeTab(task) + + +def refreshLayout(): + G.app.mainwin.refreshLayout() + + +def addPanels(): + return G.app.mainwin.addPanels() + + +def showPanels(left, right): + return G.app.mainwin.showPanels(left, right) + + +def addTopWidget(widget, *args, **kwargs): + return G.app.mainwin.addTopWidget(widget, *args, **kwargs) + + +def removeTopWidget(widget): + return G.app.mainwin.removeTopWidget(widget) + + +def addToolBar(name): + return G.app.mainwin.addToolBar(name) + + +def redraw(): + G.app.redraw() + + +def getKeyModifiers(): + return int(G.app.keyboardModifiers()) + + +def addTimer(milliseconds, callback): + return G.app.addTimer(milliseconds, callback) + + +def removeTimer(id): + G.app.removeTimer(id) + + +def callAsync(func, *args, **kwargs): + G.app.callAsync(func, *args, **kwargs) + + +def getSetting(setting_name): + return G.app.getSetting(setting_name) + + +def setSetting(setting_name, value): + G.app.setSetting(setting_name, value) diff --git a/gr00t_wbc/control/teleop/gui/library/profiler.py b/gr00t_wbc/control/teleop/gui/library/profiler.py new file mode 100644 index 0000000..e35e7ab --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/profiler.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +TODO +""" + +import atexit +from cProfile import Profile + +_sort = "cumulative" +_profiler = None +_show = None + + +def run(cmd, globals, locals): + prof = Profile() + try: + prof.runctx(cmd, globals, locals) + finally: + show(prof) + + +def active(): + return _profiler is not None + + +def start(): + global _profiler + if active(): + return + _profiler = Profile() + + +def stop(): + global _profiler + if not active(): + return + show(_profiler) + _profiler = None + + +def accum(cmd, globals, locals): + if not active(): + return + _profiler.runctx(cmd, globals, locals) + + +def show(prof): + try: + if _show is not None: + _show(prof) + else: + prof.print_stats(_sort) + except TypeError: + pass + + +def set_sort(sort): + global _sort + _sort = sort + + +def set_show(show): + global _show + _show = show + + +atexit.register(stop) diff --git a/gr00t_wbc/control/teleop/gui/library/qtgui.py b/gr00t_wbc/control/teleop/gui/library/qtgui.py new file mode 100644 index 0000000..26a42e2 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/qtgui.py @@ -0,0 +1,190 @@ +from PyQt6 import QtCore, QtGui, QtWidgets +from PyQt6.QtCore import Qt + + +class ListItem(QtWidgets.QListWidgetItem): + def __init__(self, label): + super(ListItem, self).__init__(label) + self.__hasCheckbox = False + + @property + def hasCheckbox(self): + return self.__hasCheckbox + + def setUserData(self, data): + self.setData(Qt.ItemDataRole.UserRole, data) + + def getUserData(self): + return self.data(Qt.ItemDataRole.UserRole) + + def text(self): + return str(super(ListItem, self).text()) + + def enableCheckbox(self): + self.__hasCheckbox = True + self.setFlags(self.flags() | Qt.ItemFlag.ItemIsUserCheckable) + self.setCheckState(QtCore.Qt.CheckState.Unchecked) + self.checkedState = False + + def setChecked(self, checked): + if not self.hasCheckbox: + self.setSelected(checked) + return + + if checked: + self.setCheckState(QtCore.Qt.CheckState.Checked) + else: + self.setCheckState(QtCore.Qt.CheckState.Unchecked) + self.checkedState = self.checkState() + + def isChecked(self): + return self.hasCheckbox and self.checkState() != QtCore.Qt.CheckState.Unchecked + + def _clicked(self): + # owner = self.listWidget() + if self.hasCheckbox: + if self.checkState() != self.checkedState: + self.checkedState = self.checkState() + if self.checkState(): + print("Item checked") + else: + print("Item unchecked") + return True + return False + + +class ListView(QtWidgets.QListWidget): + def __init__(self): + super(ListView, self).__init__() + # Widget.__init__(self) + self._vertical_scrolling = True + self.itemActivated[QtWidgets.QListWidgetItem].connect(self._activate) + self.itemClicked[QtWidgets.QListWidgetItem].connect(self._clicked) + + def _activate(self, item): + print("Item activated") + + def _clicked(self, item): + if item._clicked(): + return + if self.allowsMultipleSelection(): + if item.isSelected(): + print("Item selected") + else: + print("Item deselected") + else: + pass + + def onActivate(self, event): + pass + + def onClicked(self, event): + pass + + def setData(self, items): + self.clear() + for item in items: + self.addItem(item) + + def setVerticalScrollingEnabled(self, enabled): + self._vertical_scrolling = enabled + if enabled: + self.setVerticalScrollBarPolicy(QtCore.Qt.ScrollBarPolicy.ScrollBarAsNeeded) + else: + self.setVerticalScrollBarPolicy(QtCore.Qt.ScrollBarPolicy.ScrollBarAlwaysOff) + self.updateGeometry() + + def rowCount(self): + return len([item for item in self.getItems() if item is not None and not item.isHidden()]) + + def sizeHint(self): + if self._vertical_scrolling: + return super(ListView, self).sizeHint() + else: + rows = self.rowCount() + if rows > 0: + rowHeight = self.sizeHintForRow(0) + rowHeight = max(rowHeight, self.iconSize().height()) + else: + rowHeight = 0 + height = rowHeight * rows + + size = super(ListView, self).sizeHint() + size.setHeight(height) + return size + + _brushes = {} + + @classmethod + def getBrush(cls, color): + if color not in cls._brushes: + cls._brushes[color] = QtGui.QBrush(QtGui.QColor(color)) + return cls._brushes[color] + + def addLogItem(self, text, color=None, data=None, checkbox=False, pos=None): + item = ListItem(text) + item.setText(text) + if color is not None: + item.setForeground(self.getBrush(color)) + if data is not None: + item.setUserData(data) + if checkbox: + item.enableCheckbox() + return self.addItemObject(item, pos) + + def addItemObject(self, item, pos=None): + if pos is not None: + super(ListView, self).insertItem(pos, item) + else: + super(ListView, self).addItem(item) + if not self._vertical_scrolling: + self.updateGeometry() + return item + + def getSelectedItem(self): + items = self.selectedItems() + if len(items) > 0: + return items[0].text + return None + + def getSelectedItems(self): + return [item.text for item in self.selectedItems()] + + def getItemData(self, row): + item = self.item(row) + if item is not None: + return item.getUserData() + + def setItemColor(self, row, color): + item = self.item(row) + if item is not None: + item.setForeground(self.getBrush(color)) + + def showItem(self, row, state): + item = self.item(row) + if item is not None: + item.setHidden(not state) + + def allowMultipleSelection(self, allow): + self.setSelectionMode( + QtWidgets.QAbstractItemView.SelectionMode.MultiSelection + if allow + else QtWidgets.QAbstractItemView.SelectionMode.SingleSelection + ) + + def allowsMultipleSelection(self): + return self.selectionMode() == QtWidgets.QAbstractItemView.SelectionMode.MultiSelection + + def getItems(self): + return [self.item(row) for row in range(self.count())] + + def clearSelection(self): + super(ListView, self).clearSelection() + + for item in self.getItems(): + if item is None: + continue + if item.isSelected(): + item.setSelected(False) + + self.callEvent("onClearSelection", None) diff --git a/gr00t_wbc/control/teleop/gui/library/universal.py b/gr00t_wbc/control/teleop/gui/library/universal.py new file mode 100644 index 0000000..6b97ad7 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/universal.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Github Code Home Page:** https://github.com/makehumancommunity/ + +**Authors:** Glynn Clements + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman Community (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- + +TODO +""" + +import json +import os +from typing import Any + +from library.getpath import getPath + +_PRE_STARTUP_KEYS = ["useHDPI", "noShaders", "noSampleBuffers"] + + +class Globals(object): + def __init__(self): + self.app: Any = None + self.args = {} + self.world = [] + self.cameras = [] + self.canvas = None + self.windowHeight = 600 + self.windowWidth = 800 + self.clearColor = (0.0, 0.0, 0.0, 0.0) + self.preStartupSettings = dict() + self._preStartupConfigScan() + + def _preStartupConfigScan(self): + """Run a very primitive scan in order to pick up settings which has + to be known before we launch the QtApplication object.""" + iniPath = getPath("settings.ini") + data = None + if os.path.exists(iniPath): + with open(iniPath, encoding="utf-8") as f: + data = json.load(f) + if data is None: + for key in _PRE_STARTUP_KEYS: + self.preStartupSettings[key] = None + else: + for key in _PRE_STARTUP_KEYS: + if key in data: + self.preStartupSettings[key] = data[key] + # Would be nice to log this, but log has not been initialized yet + print("PRE STARTUP SETTING: " + key + " = " + str(data[key])) + else: + self.preStartupSettings[key] = None + + +G = Globals() diff --git a/gr00t_wbc/control/teleop/gui/library/xdg_parser.py b/gr00t_wbc/control/teleop/gui/library/xdg_parser.py new file mode 100644 index 0000000..0445b7c --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/library/xdg_parser.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +**Project Name:** MakeHuman + +**Product Home Page:** http://www.makehumancommunity.org/ + +**Code Home Page:** https://github.com/makehumancommunity/makehuman + +**Authors:** Aranuvir + +**Copyright(c):** MakeHuman Team 2001-2020 + +**Licensing:** AGPL3 + + This file is part of MakeHuman (www.makehumancommunity.org). + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + +Abstract +-------- +This module provides the "well known" user directories from a Linux Desktop. Common +directories are 'DESKTOP', 'DOCUMENTS', 'DOWNLOAD', 'MUSIC', 'PICTURES', ' +PUBLICSHARE', 'TEMPLATES' and 'VIDEOS'. The predefined directories and their +paths are stored in the dictionary XDG_PATHS, which will be None on platforms other +than Linux.""" + + +import os +import sys + +isLinux = sys.platform.startswith("linux") + +if isLinux: + + # Default path to xdg configuration file + CONFIG_PATH = os.path.expanduser("~/.config/user-dirs.dirs") + + def get_userdirs(path=CONFIG_PATH): + """This function parses a configuration file to retrieve user directories and their configured paths. + Environment variables are replaced by their values""" + + dirs = dict() + + if os.path.isfile(path): + with open(path, "r", encoding="utf-8") as file: + for line in file: + if line and line.startswith("XDG_"): + line = line.strip() + key, value = line.split("=") + key = key.split("_")[1] + value = os.path.expandvars(value.strip('"')) + if os.path.isdir(value): + dirs[key] = value + + return dirs + +else: + + def get_userdirs(path=""): + """Dummy function for use on other Platform than Linux""" + return {} + + +XDG_PATHS = get_userdirs() + +if XDG_PATHS: + XDG_DIRS = sorted(XDG_PATHS.keys()) +else: + XDG_DIRS = [] + + +# Main Function ############################################################# +if __name__ == "__main__": + + if isLinux and XDG_DIRS: + + print("Found XDG_DIRS:", ", ".join(XDG_DIRS), "\n") + + max_len = max([len(d) for d in XDG_DIRS]) + f_str = "{0:" + str(max_len) + "s} ---> {1}" + + for d in XDG_DIRS: + print(f_str.format(d, XDG_PATHS.get(d))) + + else: + + print("This module should be used on Linux platforms only!") diff --git a/gr00t_wbc/control/teleop/gui/main.py b/gr00t_wbc/control/teleop/gui/main.py new file mode 100644 index 0000000..a46af87 --- /dev/null +++ b/gr00t_wbc/control/teleop/gui/main.py @@ -0,0 +1,549 @@ +import json +import os +import signal +import subprocess +import sys +from typing import Dict + +import library.log as log +import library.qtgui as qtgui +from library.universal import G +from PyQt6.QtCore import QProcess, QSettings, Qt +from PyQt6.QtWidgets import ( + QApplication, + QCheckBox, + QComboBox, + QFormLayout, + QGridLayout, + QHBoxLayout, + QLabel, + QLineEdit, + QMainWindow, + QPushButton, + QVBoxLayout, + QWidget, +) + +import gr00t_wbc + +signal.signal(signal.SIGINT, signal.SIG_DFL) + + +GR00T_TELEOP_DATA_ROOT = os.path.join( + os.path.dirname(gr00t_wbc.__file__), "./external/teleop/data" +) + + +class MainWindow(QMainWindow): + def __init__( + self, + fix_pick_up=True, + fix_pointing=False, + object_A_options=[ + "small cup", + "dragon fruit", + "orange", + "apple", + "mango", + "star fruit", + "rubiks cube", + "lemon", + "lime", + "red can", + "blue can", + "green can", + "cucumber", + "bottled water", + "big cup", + "mayo", + "mustard", + "bok choy", + "grapes", + "soup can", + "mouse", + "water apple", + "corn", + "mug", + "orange cup", + "bitter gourd", + "banana", + "mangosteen", + "marker", + "coffee pod", + "plastic cup", + "grapes", + "small mug", + "condiment bottles", + "corn", + "tools", + "pear", + "eggplant", + "canned beans", + "potato", + ], + object_from_options=[ + "cutting board", + "pan", + "plate", + "bowl", + "tray", + "desk", + "placemat", + "table", + "mesh cup", + "shelf", + ], + object_to_options=[ + "cutting board", + "pan", + "plate", + "bowl", + "tray", + "microwave", + "basket", + "drawer", + "placemat", + "clear bin", + "mesh cup", + "yellow bin", + "shelf", + ], + ): + super().__init__() + # Fix the pick up and place object type + self.fix_pick_up = fix_pick_up + self.fix_pointing = fix_pointing + + # window settings + self.setWindowTitle("Gr00t Capture Test") + self.setFixedWidth(800) + self.setMinimumHeight(1000) + + self.main_layout = QVBoxLayout() + self.main_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + + # stop button + self.stop_button = QPushButton("Emergency Stop", self) + self.stop_button.setStyleSheet( + "background-color: red; color: white; font-size: 20px; font-weight: bold; height: 60px;" + ) + # self.stop_button.setEnabled(False) + self.stop_button.clicked.connect(self.stop_process) + self.main_layout.addWidget(self.stop_button) + + # property label + self.property_label = QLabel("Settings") + self.property_label.setStyleSheet( + "font-size: 20px; font-weight: bold; color: #333; max-hedescription.jight: 30px;" + ) + self.main_layout.addWidget(self.property_label) + + # operator form + settings_form_layout = QFormLayout() + self.operator_input = QLineEdit("Zu") + # self.operator_input.textChanged.connect(self.property_label.setText) + settings_form_layout.addRow(QLabel("Operator Name:"), self.operator_input) + + # collector form + self.collector_input = QLineEdit("Zu") + settings_form_layout.addRow(QLabel("Collector Name:"), self.collector_input) + + # description form + self.description_input = QLineEdit("3D pick up") + + # object A menu + self.object_A_input = QComboBox() + self.object_A_input.addItems(object_A_options) + + # object A menu + self.object_from_input = QComboBox() + self.object_from_input.addItems(object_from_options) + + # object B menu + self.object_to_input = QComboBox() + self.object_to_input.addItems(object_to_options) + + box_layout = QHBoxLayout() + box_layout.setAlignment(Qt.AlignmentFlag.AlignLeft) + if self.fix_pick_up: + # box_layout.addWidget(QLabel("Pour Object:")) + box_layout.addWidget(QLabel("Pick up Object:")) + box_layout.addWidget(self.object_A_input) + box_layout.addWidget(QLabel("from:")) + box_layout.addWidget(self.object_from_input) + box_layout.addWidget(QLabel("To:")) + box_layout.addWidget(self.object_to_input) + elif self.fix_pointing: + box_layout.addWidget(QLabel("Pointing Object:")) + box_layout.addWidget(self.object_A_input) + else: + box_layout.addWidget(self.description_input) + + # add a button to fix the pick up and place object + self.save_description_button = QPushButton("Save Description") + self.save_description_button.setMinimumWidth(100) + self.save_description_button.clicked.connect(self.save_settings) + box_layout.addWidget(self.save_description_button) + + settings_form_layout.addRow(QLabel("Task description:"), box_layout) + + # robot form + self.robot_input = QLineEdit("gr00t002") + settings_form_layout.addRow(QLabel("Robot Name:"), self.robot_input) + + # vive keyword form + self.vive_keyword_input = QComboBox() + self.vive_keyword_input.addItems(["elbow", "knee", "wrist", "shoulder", "foot"]) + settings_form_layout.addRow(QLabel("Vive keyword:"), self.vive_keyword_input) + + settings_form_layout.addRow(QLabel("-" * 300)) + self.vive_ip_input = QLineEdit("192.168.0.182") + settings_form_layout.addRow(QLabel("Vive ip:"), self.vive_ip_input) + + self.vive_port_input = QLineEdit("5555") + settings_form_layout.addRow(QLabel("Vive port:"), self.vive_port_input) + self.manus_port_input = QLineEdit("5556") + settings_form_layout.addRow(QLabel("Manus port:"), self.manus_port_input) + + # manus form + # self.manus_input = QLineEdit("foot") + # settings_form_layout.addRow(QLabel("Manus name:"), self.manus_input) + + self.main_layout.addLayout(settings_form_layout) + + # testing buttons + self.testing_button_layout = QGridLayout() + + # vive button + self.test_vive_button = QPushButton("Test Vive") + self.test_vive_button.setCheckable(True) + self.test_vive_button.toggled.connect(self.test_vive) + self.test_vive_button.setMaximumHeight(30) + self.testing_button_layout.addWidget(self.test_vive_button, 0, 1) + self.vive_checkbox = QCheckBox("Vive Ready") + self.testing_button_layout.addWidget(self.vive_checkbox, 0, 2) + + # start manus button + self.server_manus_button = QPushButton("Start Manus") + self.server_manus_button.setCheckable(True) + self.server_manus_button.toggled.connect(self.test_manus_server) + self.server_manus_button.setMaximumHeight(30) + self.testing_button_layout.addWidget(self.server_manus_button, 1, 0) + + # manus button + self.client_manus_button = QPushButton("Test Manus") + self.client_manus_button.setCheckable(True) + self.client_manus_button.toggled.connect(self.test_manus_client) + self.client_manus_button.setMaximumHeight(30) + self.testing_button_layout.addWidget(self.client_manus_button, 1, 1) + + self.manus_checkbox = QCheckBox("Manus Ready") + self.manus_checkbox.setEnabled(False) + self.testing_button_layout.addWidget(self.manus_checkbox, 1, 2) + + # self.testing_button_layout.addWidget() + self.main_layout.addLayout(self.testing_button_layout) + + container = QWidget() + container.setLayout(self.main_layout) + + # Set the central widget of the Window. + self.setCentralWidget(container) + + # Process + # QProcess for running scripts + self.process = QProcess(self) + # self.process.readyRead.connect(self.handle_stdout) + self.process.readyReadStandardOutput.connect(self.handle_stdout) + self.process.readyReadStandardError.connect(self.handle_stderr) + self.process.errorOccurred.connect(self.process_error) + self.process.finished.connect(self.process_finished) + self.current_process_name = "" + + # QProcess for running scripts + self.manus_process = QProcess(self) + # self.process.readyRead.connect(self.handle_stdout) + self.manus_process.readyReadStandardOutput.connect(self.handle_stdout) + self.manus_process.readyReadStandardError.connect(self.handle_stderr) + self.manus_process.finished.connect(self.process_finished) + + # load history + self.load_settings() + + # record all buttons + self.test_buttons: Dict[str, QPushButton] = { + "test_vive": self.test_vive_button, + "test_manus": self.client_manus_button, + } + + def load_settings(self): + settings = QSettings("Gear", "Teleop") + self.vive_ip_input.setText(settings.value("vive_ip", "192.168.0.182")) + self.vive_keyword_input.setCurrentIndex(int(settings.value("vive_keyword", 0))) + self.robot_input.setText(settings.value("robot_input", "")) + self.operator_input.setText(settings.value("operator", "")) + self.collector_input.setText(settings.value("data_collector", "")) + self.description_input.setText(settings.value("description", "")) + self.object_A_input.setCurrentIndex(int(settings.value("object", 0))) + self.object_from_input.setCurrentIndex(int(settings.value("object_from", 0))) + self.object_to_input.setCurrentIndex(int(settings.value("object_to", 0))) + + def save_settings(self): + # Create QSettings object (You can provide your app name and organization for persistent storage) + settings = QSettings("Gear", "Teleop") + + # Save the text from the QLineEdit + settings.setValue("vive_ip", self.vive_ip_input.text()) + settings.setValue("vive_keyword", self.vive_keyword_input.currentIndex()) + settings.setValue("robot_input", self.robot_input.text()) + settings.setValue("operator", self.operator_input.text()) + settings.setValue("data_collector", self.collector_input.text()) + + if self.fix_pick_up: + description = ( + f"pick {self.object_A_input.currentText()} " + f"{self.object_from_input.currentText()}->" + f"{self.object_to_input.currentText()}" + ) + elif self.fix_pointing: + description = f"point at {self.object_A_input.currentText()}" + else: + description = self.description_input.text() + + settings.setValue("description", description) + settings.setValue("object", self.object_A_input.currentIndex()) + settings.setValue("object_from", self.object_from_input.currentIndex()) + settings.setValue("object_to", self.object_to_input.currentIndex()) + + # Save the settings + descrition_file = os.path.join(GR00T_TELEOP_DATA_ROOT, "description.json") + with open(descrition_file, "w") as f: + json.dump( + { + "operator": self.operator_input.text(), + "data_collector": self.collector_input.text(), + "description": description, + }, + f, + ) + print("Settings saved to {}".format(descrition_file)) + + ################################# Process ################################# + def handle_stdout(self): + data = self.process.readAllStandardOutput().data() + stdout = data.decode("utf-8") + G.app.log_window.addLogMessage(stdout, log.DEBUG) + + data = self.manus_process.readAllStandardOutput().data() + stdout = data.decode("utf-8") + G.app.log_window.addLogMessage(stdout, log.DEBUG) + + def handle_stderr(self): + data = self.process.readAllStandardError().data() + stderr = data.decode("utf-8") + G.app.log_window.addLogMessage(stderr, log.ERROR) + + data = self.manus_process.readAllStandardError().data() + stderr = data.decode("utf-8") + G.app.log_window.addLogMessage(stderr, log.ERROR) + + def process_finished(self, exit_code, exit_status): + if exit_status == QProcess.ExitStatus.NormalExit: + G.app.log_window.addLogMessage( + f"Process finished successfully with exit code {exit_code}.", log.INFO + ) + else: + G.app.log_window.addLogMessage( + f"Process finished with error code {exit_code}.", log.ERROR + ) + + G.app.log_window.addLogMessage("---------------------------------------------", log.INFO) + + # toggle the button for the current task + print("current_process_name", self.current_process_name, self.test_buttons.keys()) + if self.current_process_name in self.test_buttons.keys(): + if self.test_buttons[self.current_process_name].isChecked(): + self.test_buttons[self.current_process_name].click() + + def process_error(self, error): + G.app.log_window.addLogMessage(f"Process Error (or stopped manually): {error}", log.ERROR) + G.app.log_window.addLogMessage("---------------------------------------------", log.INFO) + + ################################# Timer ################################# + + def update_log(self): + print("update log") + # G.app.log_window.addLogMessage("Update log", log.DEBUG) + returnBool = self.process.waitForFinished(1000) + print("returnBool", returnBool) + if not returnBool: + data = self.process.readAllStandardOutput().data() + stdout = data.decode("utf8") + if stdout: + G.app.log_window.addLogMessage(stdout, log.DEBUG) + G.app.log_window.updateView() + + def test_vive(self, checked): + print("checked", checked) + if not checked: + self.test_vive_button.setText("Test Vive") + self.stop_process() + self.current_process_name = "" + else: + self.current_process_name = "test_vive" + self.test_vive_button.setText("Stop Test") + self.stop_button.setEnabled(True) + G.app.log_window.addLogMessage("Testing Vive", log.DEBUG) + script = "python" + args = [ + "gr00t_wbc/control/teleop/main/test_vive.py", + "--keyword", + self.vive_keyword_input.currentText(), + "--ip", + self.vive_ip_input.text(), + "--port", + self.vive_port_input.text(), + ] + self.process.start(script, args) + + def test_manus_server(self, checked): + if not checked: # 2 + self.server_manus_button.setText("Start Manus") + self.stop_manus_server() + self.manus_checkbox.setEnabled(False) + else: # 1 + self.server_manus_button.setText("Stop Manus") + self.manus_checkbox.setEnabled(True) + self.start_manus_server() + + def start_manus_server(self): + G.app.log_window.addLogMessage("Starting manus server", log.WARNING) + script = "python" + args = ["gr00t_wbc/control/teleop/device/manus.py", "--port", self.manus_port_input.text()] + self.manus_process.start(script, args) + + def stop_manus_server(self): + G.app.log_window.addLogMessage("Stopping manus server", log.WARNING) + self.manus_process.terminate() + self.manus_process.waitForFinished(2000) + if self.manus_process.state() != QProcess.ProcessState.NotRunning: + self.manus_process.kill() + self.stop_button.setEnabled(False) + G.app.log_window.addLogMessage("Manus stopped", log.DEBUG) + + def test_manus_client(self, checked): + if not checked: + self.client_manus_button.setText("Test Manus") + self.stop_process() + self.current_process_name = "" + else: + self.current_process_name = "test_manus" + self.client_manus_button.setText("Stop Test") + self.stop_button.setEnabled(True) + G.app.log_window.addLogMessage("Testing", log.DEBUG) + print("Testing manus") + script = "python" + args = [ + "gr00t_wbc/control/teleop/main/test_manus.py", + "--port", + self.manus_port_input.text(), + ] + self.process.start(script, args) + + ############################### Stop Process ################################# + + def stop_process(self): + G.app.log_window.addLogMessage("Stopping process", log.WARNING) + # disable all buttons + for button in self.test_buttons.values(): + button.setCheckable(False) + button.setChecked(False) + button.setEnabled(False) + + # kill the process + if self.process.state() == QProcess.ProcessState.Running: + # os.kill(self.process.processId(), signal.SIGINT) + self.process.waitForFinished(2000) # Wait for up to 2 seconds for termination + if self.process.state() != QProcess.ProcessState.NotRunning: + self.process.kill() + # self.stop_button.setEnabled(False) + G.app.log_window.addLogMessage("Script stopped", log.DEBUG) + + if self.current_process_name in ["test_oak", "record_demonstration", "test_robot"]: + command = ( + f"ps aux | grep {self.current_process_name}" + + " | grep -v grep | awk '{print $2}' | xargs kill" + ) + + # Execute the command with subprocess.Popen, capturing both stdout and stderr + sub_process = subprocess.Popen( + command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE + ) + # Get the output and errors (if any) + stdout, stderr = sub_process.communicate() + # Ensure the process is successfully terminated + sub_process.wait() + + # enable all buttons + for button in self.test_buttons.values(): + button.setCheckable(True) + button.setEnabled(True) + + # check the button if the button was checked + if self.current_process_name in self.test_buttons.keys(): + if self.test_buttons[self.current_process_name].isChecked(): + self.test_buttons[self.current_process_name].setChecked(False) + + +class LogWindow(qtgui.ListView): + def __init__(self): + super(LogWindow, self).__init__() + self.level = log.DEBUG + self.allowMultipleSelection(True) + + def setLevel(self, level): + self.level = level + self.updateView() + + def keyPressEvent(self, e): + e.ignore() + + def updateView(self): + for i in range(self.count()): + ilevel = self.getItemData(i) + self.showItem(i, ilevel >= self.level) + self.setItemColor(i, log.getLevelColor(ilevel)) + + def addLogMessage(self, text, level=log.INFO): + index = self.count() + color = log.getLevelColor(level) + self.addLogItem(text, color, level) + self.showItem(index, level >= self.level) + + +class GCApplication(QApplication): + def __init__(self, argv): + super(GCApplication, self).__init__(argv) + + # application + if G.app is not None: + raise RuntimeError("MHApplication is a singleton") + G.app = self + + self.mainwin = MainWindow() + self.log_window = LogWindow() + self.mainwin.main_layout.addWidget(self.log_window) + self.mainwin.show() + # self.log_window.show() + + +# Function to terminate the subprocess gracefully +def terminate_subprocess(proc): + if proc.poll() is None: # Check if the process is still running + proc.terminate() # Terminate the process + print("Subprocess terminated.") + + +if __name__ == "__main__": + app = GCApplication(sys.argv) + sys.exit(app.exec()) diff --git a/gr00t_wbc/control/teleop/main/test_iphone.py b/gr00t_wbc/control/teleop/main/test_iphone.py new file mode 100644 index 0000000..2b02607 --- /dev/null +++ b/gr00t_wbc/control/teleop/main/test_iphone.py @@ -0,0 +1,306 @@ +import argparse +import time + +import matplotlib.pyplot as plt +import numpy as np + +from gr00t_wbc.control.teleop.device.iphone.iphone import IPhoneDevice + + +class IphonePublisher: + def __init__(self, port: int = 5555, silent: bool = True): + self._device = IPhoneDevice(port=port, silent=silent) + + # record the initial transform + self._init_transform = None + self._init_transform_inverse = None + + # record the latest transform and timestamp + self._latest_transform = None + self._latest_timestamp = None + + # publishing variables + self._velocity = [0, 0, 0] + self._position = [0, 0, 0] + + # visualization + self._fig = None + self._ax = None + self._setup_visualization() + + def _draw_axes(self, ax): + """Draw coordinate axes, labels, and legend on the given axes object.""" + origin = [0, 0, 0] + axis_length = 0.3 + # X axis (right) + ax.quiver( + origin[0], + origin[1], + origin[2], + axis_length, + 0, + 0, + color="r", + arrow_length_ratio=0.1, + linewidth=2, + ) + # Y axis (out/forward) + ax.quiver( + origin[0], + origin[1], + origin[2], + 0, + axis_length, + 0, + color="b", + arrow_length_ratio=0.1, + linewidth=2, + ) + # Z axis (up) + ax.quiver( + origin[0], + origin[1], + origin[2], + 0, + 0, + axis_length, + color="g", + arrow_length_ratio=0.1, + linewidth=2, + ) + ax.text(axis_length, 0, 0, "X", color="red") + ax.text(0, axis_length, 0, "Y", color="blue") + ax.text(0, 0, axis_length, "Z", color="green") + from matplotlib.lines import Line2D + + legend_elements = [ + Line2D([0], [0], color="r", lw=2, label="X axis (right)"), + Line2D([0], [0], color="g", lw=2, label="Y axis (out)"), + Line2D([0], [0], color="b", lw=2, label="Z axis (up)"), + ] + ax.legend(handles=legend_elements, loc="upper right") + + def _draw_cube(self, ax): + """Draw a 1x1x1 meter cube centered at the origin on the given axes object.""" + r = [-0.5, 0.5] + corners = [ + [r[0], r[0], r[0]], + [r[0], r[0], r[1]], + [r[0], r[1], r[0]], + [r[0], r[1], r[1]], + [r[1], r[0], r[0]], + [r[1], r[0], r[1]], + [r[1], r[1], r[0]], + [r[1], r[1], r[1]], + ] + edges = [ + (0, 1), + (0, 2), + (0, 4), + (1, 3), + (1, 5), + (2, 3), + (2, 6), + (3, 7), + (4, 5), + (4, 6), + (5, 7), + (6, 7), + ] + for i, j in edges: + x = [corners[i][0], corners[j][0]] + y = [corners[i][1], corners[j][1]] + z = [corners[i][2], corners[j][2]] + ax.plot3D(x, y, z, "gray") + + def _setup_visualization(self): + """Setup matplotlib figure for 6D pose visualization""" + plt.ion() # Enable interactive mode + self._fig = plt.figure(figsize=(8, 8)) + self._ax = self._fig.add_subplot(111, projection="3d") + self._ax.set_xlim([-0.5, 0.5]) + self._ax.set_ylim([-0.5, 0.5]) + self._ax.set_zlim([-0.5, 0.5]) + self._ax.set_xlabel("X (right)") + self._ax.set_ylabel("Y (out)") + self._ax.set_zlabel("Z (up)") + self._ax.set_title("iPhone 6D Pose") + self._ax.view_init(elev=30, azim=-45) + self._draw_cube(self._ax) + self._draw_axes(self._ax) + plt.tight_layout() + self._fig.canvas.draw() + plt.pause(0.001) + + def start(self): + self._device.start() + + def stop(self): + self._device.stop() + if self._fig is not None: + plt.close(self._fig) + + def reset_transform(self, *args, timeout=5.0, poll_interval=0.1): + """ + Wait until device returns data, then set the initial transform. + + Args: + timeout (float): Maximum time to wait in seconds. + poll_interval (float): Time between checks in seconds. + """ + start_time = time.time() + while True: + data = self._device.get_cmd() + if data: + self._init_transform = np.array(data.get("transformMatrix")) + self._init_transform_inverse = np.linalg.inv(self._init_transform) + print( + "initial position", [round(v, 4) for v in self._init_transform[:3, 3].tolist()] + ) + self._latest_transform = self._init_transform.copy() + self._latest_timestamp = data.get("timestamp") + print("Initial transform set.") + break + elif time.time() - start_time > timeout: + print("Timeout: Failed to get initial transform data after waiting.") + break + else: + time.sleep(poll_interval) + + return {"success": True, "message": "Triggered!"} + + def update_transfrom(self) -> dict: + data = self._device.get_cmd() + + if data: + transform_matrix = np.array(data.get("transformMatrix")) + position = transform_matrix[:3, 3] + rotation = transform_matrix[:3, :3] + + # Create a single multiline string with position and rotation data + output = f"\r\033[KPosition: {[round(v, 4) for v in position.tolist()]}\n" + output += "\033[KRotation matrix:\n" + for i in range(3): + row = [round(v, 4) for v in rotation[i].tolist()] + output += f"\033[K {row}\n" + + # Move cursor up 5 lines (position + "rotation matrix:" + 3 rows) + # and print the entire output at once + print(f"\033[5A{output}", end="", flush=True) + + # draw 6d pose + self._visualize_pose(position, rotation) + + if data: + current_transform = np.array(data.get("transformMatrix")) @ self._init_transform_inverse + # print("current_transform", [round(v, 4) for v in current_transform.flatten().tolist()]) + current_timestamp = data.get("timestamp") + + # Check if the current timestamp is the same as the latest timestamp (No update or disconnect) + if current_timestamp == self._latest_timestamp: + return { + "transform_matrix": self._latest_transform, + "velocity": [0, 0, 0], + "position": self._latest_transform[:3, 3].tolist(), + "timestamp": self._latest_timestamp, + } + + if self._latest_transform is not None: + current_position = current_transform[:3, 3] + current_velocity = (current_position - self._latest_transform[:3, 3]) / ( + current_timestamp - self._latest_timestamp + ) + self._velocity = current_velocity.tolist() + self._position = current_position.tolist() + + self._latest_transform = current_transform.copy() + self._latest_timestamp = current_timestamp + + return { + "transform_matrix": self._latest_transform, + "velocity": self._velocity, + "position": self._position, + "timestamp": self._latest_timestamp, + } + + def _visualize_pose(self, position, rotation): + """Visualize the 6D pose using matplotlib""" + if self._fig is None or not plt.fignum_exists(self._fig.number): + self._setup_visualization() + self._ax.clear() + self._ax.set_xlim([-0.5, 0.5]) + self._ax.set_ylim([-0.5, 0.5]) + self._ax.set_zlim([-0.5, 0.5]) + self._ax.set_xlabel("X (right)") + self._ax.set_ylabel("Y (out)") + self._ax.set_zlabel("Z (up)") + self._ax.set_title("iPhone 6D Pose") + self._ax.view_init(elev=30, azim=-45) + self._draw_cube(self._ax) + self._draw_axes(self._ax) + # Draw position as a marker + pos = np.clip(position, -0.5, 0.5) + self._ax.scatter(pos[0], pos[1], pos[2], color="red", s=100, marker="o") + # Draw orientation axes + length = 0.2 + colors = ["r", "b", "g"] # X=red, Y=blue, Z=green + for i in range(3): + direction = rotation[:, i] * length + self._ax.quiver( + pos[0], + pos[1], + pos[2], + direction[0], + direction[1], + direction[2], + color=colors[i], + arrow_length_ratio=0.2, + linewidth=2, + ) + self._fig.canvas.draw() + plt.pause(0.001) + + +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser() + parser.add_argument("--fps", type=int, default=20, help="Frames per second (default: 20)") + parser.add_argument( + "--timeout", + type=int, + default=30, + help="Timeout for waiting for the device to connect (default: 30 seconds)", + ) + parser.add_argument("--debug", action="store_true", help="Enable debug mode") + args = parser.parse_args() + fps = args.fps + timeout = args.timeout + + # Initialize the iPhone publisher + iphone_publisher = IphonePublisher(port=5555, silent=False) + iphone_publisher.start() + print("Waiting for device to connect...press the reset button on the device to start") + iphone_publisher.reset_transform( + timeout=timeout + ) # wait x seconds for the device to return data + + def update_and_visualize(): + # Update the transform and get the latest data + data = iphone_publisher.update_transfrom() + + # print the data for debugging + if args.debug and np.random.rand() < 1 / fps: + print("data", data) + + try: + while True: + update_and_visualize() + time.sleep(1.0 / fps) + except KeyboardInterrupt: + print("Stopping device...") + finally: + iphone_publisher.stop() + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/teleop/main/test_manus.py b/gr00t_wbc/control/teleop/main/test_manus.py new file mode 100644 index 0000000..81f6967 --- /dev/null +++ b/gr00t_wbc/control/teleop/main/test_manus.py @@ -0,0 +1,102 @@ +import pickle +import time + +import click +import matplotlib.pyplot as plt +import numpy as np +import zmq + + +def plot_fingertips(data, ax): + """Plot specific fingertip positions of left and right fingers in 3D space.""" + + left_fingertips = data["left_fingers"]["position"] + right_fingertips = data["right_fingers"]["position"] + + # Extract X, Y, Z positions of fingertips from the transformation matrices + left_positions = np.array([finger[:3, 3] for finger in left_fingertips]) + right_positions = np.array([finger[:3, 3] for finger in right_fingertips]) + + # Ensure the positions are 2D arrays (N, 3) + left_positions = np.reshape(left_positions, (-1, 3)) # Ensure 2D array with shape (N, 3) + right_positions = np.reshape(right_positions, (-1, 3)) # Ensure 2D array with shape (N, 3) + + # Create a 3D plot + ax.cla() # Clear the previous plot + + # Plot selected left fingertips (use red color) + ax.scatter( + left_positions[:, 0], + left_positions[:, 1], + left_positions[:, 2], + c="r", + label="Left Fingers", + ) + ax.scatter( + right_positions[:, 0], + right_positions[:, 1], + right_positions[:, 2], + c="b", + label="Right Fingers", + ) + # ax.scatter(avp_left_fingers[:, 0], avp_left_fingers[:, 1], avp_left_fingers[:, 2], c='b', label='AVP') + + # Set plot labels + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") + + ax.set_xlim([-0.3, 0.3]) # Set X-axis limits, adjust to your range + ax.set_ylim([-0.3, 0.3]) # Set Y-axis limits, adjust to your range + ax.set_zlim([-0.3, 0.3]) # Set Z-axis limits, adjust to your range + + # Add a legend + ax.legend() + + # Display the plot (update it) + plt.draw() + plt.pause(0.00001) + + +class ManusVisClient: + def __init__(self, port=5556): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REQ) # Use REQ instead of SUB + self.socket.connect(f"tcp://localhost:{port}") + + def request_data(self): + """Request the latest data from the server.""" + # Send request to the server + self.socket.send(b"request_data") # Send a request message + + # Wait for the server's response + message = self.socket.recv() # Receive response + data = pickle.loads(message) # Deserialize the data + + return data + + +@click.command() +@click.option("--port", type=int, default=5556) +def main(port): + print("==>start test manus") + plt.ion() + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + + client = ManusVisClient(port=port) + + iter_idx = 0 + while True: + data = client.request_data() + plot_fingertips(data, ax) + + time.sleep(0.1) + iter_idx += 1 + if iter_idx > 100: + break + + +# Run the client +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/teleop/main/test_vive.py b/gr00t_wbc/control/teleop/main/test_vive.py new file mode 100644 index 0000000..274a800 --- /dev/null +++ b/gr00t_wbc/control/teleop/main/test_vive.py @@ -0,0 +1,82 @@ +import click +import matplotlib.animation as animation +import matplotlib.pyplot as plt +import numpy as np + +from gr00t_wbc.control.teleop.streamers.vive_streamer import ViveStreamer + + +@click.command() +@click.option("--ip", default="192.168.0.182", help="IP address of the Vive Tracker") +@click.option("--port", default=5555, help="Port number of the Vive Tracker") +@click.option("--keyword", default="elbow", help="Keyword to filter the tracker data") +def main(ip, port, keyword): + print("==>start test vive") + streamer = ViveStreamer(ip=ip, port=port, fps=20, keyword=keyword) + streamer.start_streaming() + + # Create figure and 3D axis + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + + pos_data = {} + pose_data_init = {} + ax_line = {} + for dir in ["left", "right"]: + pose_data_init[dir] = None + pos_data[dir] = list() + # Initialize a line object that will be updated in the animation + color = "r" if dir == "left" else "b" + ax_line[dir] = ax.plot([], [], [], f"{color}-", marker="o")[0] + + # Set plot limits (adjust as needed) + ax.set_xlim(-1, 1) + ax.set_ylim(-1, 1) + ax.set_zlim(-1, 1) + + # Set axis labels + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") + + # Define the initialization function for the animation + def init(): + for dir in ["left", "right"]: + ax_line[dir].set_data([], []) + ax_line[dir].set_3d_properties([]) + + return ax_line["left"], ax_line["right"] + + # Define the update function that will update the trajectory in real-time + def update(num): + streamer_output = streamer.get() + + # Handle case where no data is available yet + if not streamer_output or not streamer_output.ik_data: + return ax_line["left"], ax_line["right"] + + for dir in ["left", "right"]: + wrist_key = f"{dir}_wrist" + if wrist_key not in streamer_output.ik_data: + continue + + raw_pose = streamer_output.ik_data[wrist_key] + if pose_data_init[dir] is None: + pose_data_init[dir] = raw_pose + relative_pose = np.linalg.inv(pose_data_init[dir]) @ raw_pose + pos_data[dir].append(relative_pose[:3, 3]) + pos_data_np = np.array(pos_data[dir]) + ax_line[dir].set_data(pos_data_np[:, 0], pos_data_np[:, 1]) + ax_line[dir].set_3d_properties(pos_data_np[:, 2]) + return ax_line["left"], ax_line["right"] + + # Create the animation + _ = animation.FuncAnimation(fig, update, init_func=init, frames=20, interval=50, blit=True) + + # Show the plot + plt.show(block=False) + plt.pause(5) + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/teleop/pre_processor/fingers/fingers.py b/gr00t_wbc/control/teleop/pre_processor/fingers/fingers.py new file mode 100644 index 0000000..7c11db6 --- /dev/null +++ b/gr00t_wbc/control/teleop/pre_processor/fingers/fingers.py @@ -0,0 +1,16 @@ +from gr00t_wbc.control.teleop.pre_processor.pre_processor import PreProcessor + + +class FingersPreProcessor(PreProcessor): + """Dummy class just takes out the fingers from the data.""" + + def __init__(self, side: str): + super().__init__() + self.side = side + + def __call__(self, data): + return data[f"{self.side}_fingers"] + + # TODO: calibrate max and min + def calibrate(self, data, control_device): + pass diff --git a/gr00t_wbc/control/teleop/pre_processor/pre_processor.py b/gr00t_wbc/control/teleop/pre_processor/pre_processor.py new file mode 100644 index 0000000..76c5d6f --- /dev/null +++ b/gr00t_wbc/control/teleop/pre_processor/pre_processor.py @@ -0,0 +1,13 @@ +import abc + + +class PreProcessor(abc.ABC): + def __init__(self, **kwargs): + pass + + def register(self, robot): + self.robot = robot + + @abc.abstractmethod + def __call__(self, data) -> dict: + pass diff --git a/gr00t_wbc/control/teleop/pre_processor/wrists/wrists.py b/gr00t_wbc/control/teleop/pre_processor/wrists/wrists.py new file mode 100644 index 0000000..a0c67ae --- /dev/null +++ b/gr00t_wbc/control/teleop/pre_processor/wrists/wrists.py @@ -0,0 +1,110 @@ +from copy import deepcopy + +import numpy as np + +from gr00t_wbc.control.teleop.pre_processor.pre_processor import PreProcessor + +RIGHT_HAND_ROTATION = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) + + +class WristsPreProcessor(PreProcessor): + def __init__(self, motion_scale: float, **kwargs): + super().__init__(**kwargs) + self.motion_scale = motion_scale # scale factor for robot motion + self.calibration_ee_pose = {} # poses to calibrate the robot + self.ee_name_list = ( + [] + ) # name of the end-effector "link_head_pitch", "link_LArm7", "link_RArm7" + self.robot_world_T_init_ee = {} # initial end-effector pose of the robot + self.init_teleop_T_teleop_world = {} # initial transformation matrix + self.init_teleop_T_init_ee = {} # alignment of end effector and local teleop frames + + self.latest_data = None + + def calibrate(self, data, control_device): + left_elbow_joint_name = self.robot.supplemental_info.joint_name_mapping["elbow_pitch"][ + "left" + ] + right_elbow_joint_name = self.robot.supplemental_info.joint_name_mapping["elbow_pitch"][ + "right" + ] + if "left_wrist" in data: + self.ee_name_list.append(self.robot.supplemental_info.hand_frame_names["left"]) + self.calibration_ee_pose[left_elbow_joint_name] = ( + self.robot.supplemental_info.elbow_calibration_joint_angles["left"] + ) + if "right_wrist" in data: + self.ee_name_list.append(self.robot.supplemental_info.hand_frame_names["right"]) + self.calibration_ee_pose[right_elbow_joint_name] = ( + self.robot.supplemental_info.elbow_calibration_joint_angles["right"] + ) + + if self.calibration_ee_pose: + q = deepcopy(self.robot.q_zero) + # set pose + for joint, degree in self.calibration_ee_pose.items(): + joint_idx = self.robot.joint_to_dof_index[joint] + q[joint_idx] = np.deg2rad(degree) + self.robot.cache_forward_kinematics(q, auto_clip=False) + calibration_ee_poses = [ + self.robot.frame_placement(ee_name).np for ee_name in self.ee_name_list + ] + self.robot.reset_forward_kinematics() + else: + calibration_ee_poses = [ + self.robot.frame_placement(ee_name).np for ee_name in self.ee_name_list + ] + + for ee_name in self.ee_name_list: + self.robot_world_T_init_ee[ee_name] = deepcopy( + calibration_ee_poses[self.ee_name_list.index(ee_name)] + ) + self.init_teleop_T_teleop_world[ee_name] = ( + np.linalg.inv(deepcopy(data["left_wrist"])) + if ee_name == self.robot.supplemental_info.hand_frame_names["left"] + else np.linalg.inv(deepcopy(data["right_wrist"])) + ) + + # Initial teleop local frame is aligned with the initial end effector + # local frame with hardcoded rotations since we don't have a common + # reference frame for teleop and robot. + self.init_teleop_T_init_ee[ee_name] = np.eye(4) + if control_device == "pico": + # TODO: add pico wrist calibration respect to the headset frame + pass + else: + if ee_name == self.robot.supplemental_info.hand_frame_names["left"]: + self.init_teleop_T_init_ee[ee_name][ + :3, :3 + ] = self.robot.supplemental_info.hand_rotation_correction + else: + self.init_teleop_T_init_ee[ee_name][:3, :3] = ( + RIGHT_HAND_ROTATION @ self.robot.supplemental_info.hand_rotation_correction + ) + + def __call__(self, data) -> dict: + processed_data = {} + for ee_name in self.ee_name_list: + # Select wrist data based on ee_name + teleop_world_T_final_teleop = ( + data["left_wrist"] + if ee_name == self.robot.supplemental_info.hand_frame_names["left"] + else data["right_wrist"] + ) + init_teleop_T_final_teleop = ( + self.init_teleop_T_teleop_world[ee_name] @ teleop_world_T_final_teleop + ) + # End effector differential transform is teleop differential transform expressed + # in the end effector frame. + init_ee_T_final_ee = ( + np.linalg.inv(self.init_teleop_T_init_ee[ee_name]) + @ init_teleop_T_final_teleop + @ self.init_teleop_T_init_ee[ee_name] + ) + # Translation scaling + init_ee_T_final_ee[:3, 3] = self.motion_scale * init_ee_T_final_ee[:3, 3] + robot_world_T_final_ee = self.robot_world_T_init_ee[ee_name] @ init_ee_T_final_ee + processed_data[ee_name] = robot_world_T_final_ee + + self.latest_data = processed_data + return processed_data diff --git a/gr00t_wbc/control/teleop/solver/body/body_ik_solver.py b/gr00t_wbc/control/teleop/solver/body/body_ik_solver.py new file mode 100644 index 0000000..3127019 --- /dev/null +++ b/gr00t_wbc/control/teleop/solver/body/body_ik_solver.py @@ -0,0 +1,179 @@ +from typing import Dict + +import numpy as np +import pink +from pink import solve_ik +from pink.tasks import FrameTask, PostureTask +import pinocchio as pin +import qpsolvers + +from gr00t_wbc.control.teleop.solver.body.body_ik_solver_settings import BodyIKSolverSettings +from gr00t_wbc.control.teleop.solver.solver import Solver + + +class WeightedPostureTask(PostureTask): + def __init__( + self, cost: float, weights: np.ndarray, lm_damping: float = 0.0, gain: float = 1.0 + ) -> None: + r"""Create weighted posture task. + + Args: + cost: value used to cast joint angle differences to a homogeneous + cost, in :math:`[\mathrm{cost}] / [\mathrm{rad}]`. + weights: vector of weights for each joint. + lm_damping: Unitless scale of the Levenberg-Marquardt (only when + the error is large) regularization term, which helps when + targets are unfeasible. Increase this value if the task is too + jerky under unfeasible targets, but beware that too large a + damping can slow down the task. + gain: Task gain :math:`\alpha \in [0, 1]` for additional low-pass + filtering. Defaults to 1.0 (no filtering) for dead-beat + control. + """ + super().__init__(cost=cost, lm_damping=lm_damping, gain=gain) + self.weights = weights + + def compute_error(self, configuration): + error = super().compute_error(configuration) + return self.weights * error + + def compute_jacobian(self, configuration): + J = super().compute_jacobian(configuration) + # breakpoint() + return self.weights[:, np.newaxis] * J + + def __repr__(self): + """Human-readable representation of the weighted posture task.""" + return ( + "WeightedPostureTask(" + f"cost={self.cost}, " + f"weights={self.weights}, " + f"gain={self.gain}, " + f"lm_damping={self.lm_damping})" + ) + + +class BodyIKSolver(Solver): + def __init__(self, ik_solver_settings: BodyIKSolverSettings): + self.dt = ik_solver_settings.dt + self.num_step_per_frame = ik_solver_settings.num_step_per_frame + self.amplify_factor = ik_solver_settings.amplify_factor + self.link_costs = ik_solver_settings.link_costs + self.posture_weight = ik_solver_settings.posture_weight + self.posture_cost = ik_solver_settings.posture_cost + self.posture_lm_damping = ik_solver_settings.posture_lm_damping + self.robot = None + + def register_robot(self, robot): + self.robot = robot + self.initialize() + + def initialize(self): + self.solver = qpsolvers.available_solvers[0] + if "quadprog" in qpsolvers.available_solvers: + self.solver = "quadprog" + else: + self.solver = qpsolvers.available_solvers[0] + + q_default = self.robot.q_zero.copy() + q_default[self.robot.joint_to_dof_index["left_shoulder_roll_joint"]] = 0.2 + q_default[self.robot.joint_to_dof_index["right_shoulder_roll_joint"]] = -0.2 + + self.configuration = pink.Configuration( + self.robot.pinocchio_wrapper.model, + self.robot.pinocchio_wrapper.data, + q_default, + ) + self.configuration.model.lowerPositionLimit = self.robot.lower_joint_limits + self.configuration.model.upperPositionLimit = self.robot.upper_joint_limits + + # initialize tasks + self.tasks = {} + for link_name, weight in self.link_costs.items(): + assert link_name != "posture", "posture is a reserved task name" + + # Map robot-agnostic link names to robot-specific names + if link_name == "hand": + # Use hand_frame_names from supplemental info + for side in ["left", "right"]: + frame_name = self.robot.supplemental_info.hand_frame_names[side] + task = FrameTask( + frame_name, + **weight, + ) + self.tasks[frame_name] = task + else: + # For other links, use the name directly + task = FrameTask( + link_name, + **weight, + ) + self.tasks[link_name] = task + + # add posture task + if self.posture_weight is not None: + weight = np.ones(self.robot.num_dofs) + + # Map robot-agnostic joint types to specific robot joint names using supplemental info + for joint_type, posture_weight in self.posture_weight.items(): + if joint_type not in self.robot.supplemental_info.joint_name_mapping: + print(f"Warning: Unknown joint type {joint_type}") + continue + + # Get the joint name mapping for this type + joint_mapping = self.robot.supplemental_info.joint_name_mapping[joint_type] + + # Handle both single joint names and left/right mappings + if isinstance(joint_mapping, str): + # Single joint (e.g., waist joints) + if joint_mapping in self.robot.joint_to_dof_index: + joint_idx = self.robot.joint_to_dof_index[joint_mapping] + weight[joint_idx] = posture_weight + else: + # Left/right mapping (e.g., arm joints) + for side in ["left", "right"]: + joint_name = joint_mapping[side] + if joint_name in self.robot.joint_to_dof_index: + joint_idx = self.robot.joint_to_dof_index[joint_name] + weight[joint_idx] = posture_weight + + self.tasks["posture"] = WeightedPostureTask( + cost=self.posture_cost, + weights=weight, + lm_damping=self.posture_lm_damping, + ) + else: + self.tasks["posture"] = PostureTask( + cost=self.posture_cost, lm_damping=self.posture_lm_damping + ) + for task in self.tasks.values(): + task.set_target_from_configuration(self.configuration) + + def __call__(self, target_pose: Dict): + for link_name, pose in target_pose.items(): + if link_name not in self.tasks: + continue + pose = pin.SE3(pose[:3, :3], pose[:3, 3]) + self.tasks[link_name].set_target(pose) + + for _ in range(self.num_step_per_frame): + velocity = solve_ik( + self.configuration, + self.tasks.values(), + dt=self.dt, + solver=self.solver, + ) + self.configuration.q = self.robot.clip_configuration( + self.configuration.q + velocity * self.dt * self.amplify_factor + ) + self.configuration.update() + self.robot.cache_forward_kinematics(self.configuration.q) + + return self.configuration.q.copy() + + def update_weights(self, weights): + for link_name, weight in weights.items(): + if "position_cost" in weight: + self.tasks[link_name].set_position_cost(weight["position_cost"]) + if "orientation_cost" in weight: + self.tasks[link_name].set_orientation_cost(weight["orientation_cost"]) diff --git a/gr00t_wbc/control/teleop/solver/body/body_ik_solver_settings.py b/gr00t_wbc/control/teleop/solver/body/body_ik_solver_settings.py new file mode 100644 index 0000000..b8453f5 --- /dev/null +++ b/gr00t_wbc/control/teleop/solver/body/body_ik_solver_settings.py @@ -0,0 +1,35 @@ +from dataclasses import dataclass + + +@dataclass +class BodyIKSolverSettings: + def __init__(self): + self.dt = 0.05 + self.num_step_per_frame = 3 + self.amplify_factor = 1.0 + self.posture_cost = 0.01 + self.posture_lm_damping = 1.0 + self.link_costs = { + "hand": { + "orientation_cost": 2.0, + "position_cost": 8.0, + "lm_damping": 3.0, + } + } + self.posture_weight = { + "waist_pitch": 10.0, + "waist_yaw": 2.0, + "waist_roll": 10.0, + "shoulder_pitch": 4.0, + "shoulder_roll": 3.0, + "shoulder_yaw": 0.1, + "elbow_pitch": 3.0, + "wrist_pitch": 1.0, + "wrist_yaw": 0.1, + } + # These joint limits override the joint limits in the robot model for the IK solver + self.ik_joint_limits = { + # Increase waist pitch limit since lower body policy can use hip joints to pitch + "waist_pitch": [-0.52, 0.9], + "elbow_pitch": [-1.0472, 1.4], + } diff --git a/gr00t_wbc/control/teleop/solver/hand/g1_gripper_ik_solver.py b/gr00t_wbc/control/teleop/solver/hand/g1_gripper_ik_solver.py new file mode 100644 index 0000000..455f2bc --- /dev/null +++ b/gr00t_wbc/control/teleop/solver/hand/g1_gripper_ik_solver.py @@ -0,0 +1,137 @@ +import numpy as np + +from gr00t_wbc.control.teleop.solver.solver import Solver + + +###################################### +### Define your solver here +###################################### +class G1GripperInverseKinematicsSolver(Solver): + def __init__(self, side) -> None: + self.side = "L" if side.lower() == "left" else "R" + + def register_robot(self, robot): + pass + + def __call__(self, finger_data): + q_desired = np.zeros(7) + + # manus data + fingertips = finger_data["position"] + + # Extract X, Y, Z positions of fingertips from the transformation matrices + positions = np.array([finger[:3, 3] for finger in fingertips]) + + # Ensure the positions are 2D arrays (N, 3) + positions = np.reshape(positions, (-1, 3)) # Ensure 2D array with shape (N, 3) + + thumb_pos = positions[4, :] + index_pos = positions[4 + 5, :] + middle_pos = positions[4 + 10, :] + ring_pos = positions[4 + 15, :] + pinky_pos = positions[4 + 20, :] + + index_dist = np.linalg.norm(thumb_pos - index_pos) + middle_dist = np.linalg.norm(thumb_pos - middle_pos) + ring_dist = np.linalg.norm(thumb_pos - ring_pos) + pinky_dist = np.linalg.norm(thumb_pos - pinky_pos) + dist_threshold = 0.05 + + index_close = index_dist < dist_threshold + middle_close = middle_dist < dist_threshold + ring_close = ring_dist < dist_threshold + pinky_close = pinky_dist < dist_threshold + + if index_close: + q_desired = self._get_index_close_q_desired() + elif middle_close: + q_desired = self._get_middle_close_q_desired() + elif ring_close: + q_desired = self._get_ring_close_q_desired() + elif pinky_close: + q_desired = self._get_pinky_close_q_desired() + + return q_desired + + def _get_index_close_q_desired(self): + q_desired = np.zeros(7) + + amp0 = 0.5 + if self.side == "L": + q_desired[0] -= amp0 + else: + q_desired[0] += amp0 + + amp = 0.7 + + q_desired[1] += amp + q_desired[2] += amp + + ampA1 = 1.5 + ampB1 = 1.5 + ampA2 = 0.6 + ampB2 = 1.5 + + q_desired[3] -= ampA1 + q_desired[4] -= ampB1 + q_desired[5] -= ampA2 + q_desired[6] -= ampB2 + + return q_desired if self.side == "L" else -q_desired + + def _get_middle_close_q_desired(self): + q_desired = np.zeros(7) + + amp0 = 0.0 + if self.side == "L": + q_desired[0] -= amp0 + else: + q_desired[0] += amp0 + + amp = 0.7 + + q_desired[1] += amp + q_desired[2] += amp + + ampA1 = 1.0 + ampB1 = 1.5 + ampA2 = 1.0 + ampB2 = 1.5 + + q_desired[3] -= ampA1 + q_desired[4] -= ampB1 + q_desired[5] -= ampA2 + q_desired[6] -= ampB2 + + return q_desired if self.side == "L" else -q_desired + + def _get_ring_close_q_desired(self): + q_desired = np.zeros(7) + + amp0 = -0.5 + if self.side == "L": + q_desired[0] -= amp0 + else: + q_desired[0] += amp0 + + amp = 0.7 + + q_desired[1] += amp + q_desired[2] += amp + + ampA1 = 0.6 + ampB1 = 1.5 + ampA2 = 1.5 + ampB2 = 1.5 + + q_desired[3] -= ampA1 + q_desired[4] -= ampB1 + q_desired[5] -= ampA2 + q_desired[6] -= ampB2 + + return q_desired if self.side == "L" else -q_desired + + def _get_pinky_close_q_desired(self): + q_desired = np.zeros(7) + + return q_desired if self.side == "L" else -q_desired diff --git a/gr00t_wbc/control/teleop/solver/hand/instantiation/g1_hand_ik_instantiation.py b/gr00t_wbc/control/teleop/solver/hand/instantiation/g1_hand_ik_instantiation.py new file mode 100644 index 0000000..23ec9d5 --- /dev/null +++ b/gr00t_wbc/control/teleop/solver/hand/instantiation/g1_hand_ik_instantiation.py @@ -0,0 +1,8 @@ +from gr00t_wbc.control.teleop.solver.hand.g1_gripper_ik_solver import G1GripperInverseKinematicsSolver + + +# initialize hand ik solvers for g1 robot +def instantiate_g1_hand_ik_solver(): + left_hand_ik_solver = G1GripperInverseKinematicsSolver(side="left") + right_hand_ik_solver = G1GripperInverseKinematicsSolver(side="right") + return left_hand_ik_solver, right_hand_ik_solver diff --git a/gr00t_wbc/control/teleop/solver/solver.py b/gr00t_wbc/control/teleop/solver/solver.py new file mode 100644 index 0000000..58fa508 --- /dev/null +++ b/gr00t_wbc/control/teleop/solver/solver.py @@ -0,0 +1,17 @@ +from abc import ABC, abstractmethod +from typing import Any + + +class Solver(ABC): + def __init__(self): + pass + + def register_robot(self, robot): + pass + + def calibrate(self, data): + pass + + @abstractmethod + def __call__(self, target) -> Any: + pass diff --git a/gr00t_wbc/control/teleop/streamers/base_streamer.py b/gr00t_wbc/control/teleop/streamers/base_streamer.py new file mode 100644 index 0000000..3da36d8 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/base_streamer.py @@ -0,0 +1,46 @@ +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +import time +from typing import Any, Dict + + +@dataclass +class StreamerOutput: + """Clean separation of different data types""" + + # Data that needs IK processing (ik_keys) + ik_data: Dict[str, Any] = field(default_factory=dict) + + # Commands that pass directly to robot control loop (control_keys) + control_data: Dict[str, Any] = field(default_factory=dict) + + # Commands used internally by teleop policy (teleop_keys) + teleop_data: Dict[str, Any] = field(default_factory=dict) + + # Commands used for data collection (data_collection_keys) + data_collection_data: Dict[str, Any] = field(default_factory=dict) + + # Metadata + timestamp: float = field(default_factory=time.time) + source: str = "" + + +class BaseStreamer(ABC): + def __init__(self, *args, **kwargs): + pass + + def reset_status(self): + pass + + @abstractmethod + def start_streaming(self): + pass + + @abstractmethod + def get(self) -> StreamerOutput: + """Return StreamerOutput with structured data""" + pass + + @abstractmethod + def stop_streaming(self): + pass diff --git a/gr00t_wbc/control/teleop/streamers/dummy_streamer.py b/gr00t_wbc/control/teleop/streamers/dummy_streamer.py new file mode 100644 index 0000000..b3febdc --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/dummy_streamer.py @@ -0,0 +1,81 @@ +import numpy as np + +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput +from gr00t_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber + + +class DummyStreamer(BaseStreamer): + """A dummy streamer that returns hardcoded structured data for testing.""" + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.is_streaming = False + self.keyboard_listener = KeyboardListenerSubscriber() + self.left_fingertips = np.zeros((25, 4, 4)) + self.right_fingertips = np.zeros((25, 4, 4)) + self.left_fingertips[4, 0, 3] = 1.0 # open + self.right_fingertips[4, 0, 3] = 1.0 # open + + def start_streaming(self): + self.is_streaming = True + print("Dummy streamer started with hardcoded IK data") + + def get(self) -> StreamerOutput: + """Return hardcoded dummy data with proper IK format.""" + if not self.is_streaming: + return StreamerOutput() + + # Hardcoded dummy data - identity matrices and zeros + left_wrist_pose = np.eye(4) + right_wrist_pose = np.eye(4) + left_fingers, right_fingers = self._generate_finger_data() + + return StreamerOutput( + ik_data={ + "left_wrist": left_wrist_pose, + "right_wrist": right_wrist_pose, + "left_fingers": {"position": left_fingers}, + "right_fingers": {"position": right_fingers}, + }, + control_data={ + "navigate_cmd": [0.0, 0.0, 0.0], # No movement + "toggle_stand_command": False, + "base_height_command": 0.78, # Default standing height + }, + teleop_data={ + "toggle_activation": False, + "freeze_upper_body": False, + }, + source="dummy", + ) + + def stop_streaming(self): + self.is_streaming = False + print("Dummy streamer stopped") + + def _generate_finger_data(self): + """Generate finger position data similar to iPhone streamer.""" + + # Control thumb based on shoulder button state (index 4 is thumb tip) + key = self.keyboard_listener.read_msg() + index = 5 + middle = 10 + ring = 15 + if key is not None: + if key == "b": + if self.left_fingertips[4 + index, 0, 3] == 1.0: + self.left_fingertips[4 + index, 0, 3] = 0.0 # open + else: + self.left_fingertips[4 + index, 0, 3] = 1.0 # closed + if key == "n": + if self.left_fingertips[4 + middle, 0, 3] == 1.0: + self.left_fingertips[4 + middle, 0, 3] = 0.0 # open + else: + self.left_fingertips[4 + middle, 0, 3] = 1.0 # closed + if key == "m": + if self.left_fingertips[4 + ring, 0, 3] == 1.0: + self.left_fingertips[4 + ring, 0, 3] = 0.0 # open + else: + self.left_fingertips[4 + ring, 0, 3] = 1.0 # closed + + return self.left_fingertips, self.right_fingertips diff --git a/gr00t_wbc/control/teleop/streamers/iphone_streamer.py b/gr00t_wbc/control/teleop/streamers/iphone_streamer.py new file mode 100644 index 0000000..9c20fc5 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/iphone_streamer.py @@ -0,0 +1,150 @@ +import concurrent.futures +import time + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.teleop.device.iphone.iphone import IPhoneDevice +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput + + +def get_data_with_timeout(obj, timeout=0.02): + with concurrent.futures.ThreadPoolExecutor() as executor: + future = executor.submit(obj.data_collect.request_vive_data) + try: + combined_data = future.result(timeout=timeout) + return combined_data + except concurrent.futures.TimeoutError: + print("Data request timed out.") + return None + + +def get_transformation(vive_raw_data): + """ + Turn the raw data from the Vive tracker into a transformation matrix. + """ + position = np.array( + [ + vive_raw_data["position"]["x"], + vive_raw_data["position"]["y"], + vive_raw_data["position"]["z"], + ] + ) + quat = np.array( + [ + vive_raw_data["orientation"]["x"], + vive_raw_data["orientation"]["y"], + vive_raw_data["orientation"]["z"], + vive_raw_data["orientation"]["w"], + ] + ) + T = np.identity(4) + T[:3, :3] = R.from_quat(quat).as_matrix() + T[:3, 3] = position + return T + + +class IphoneStreamer(BaseStreamer): + def __init__(self): + self.left_device = IPhoneDevice(port=5557) + self.right_device = IPhoneDevice(port=5558) + self.left_prev_button_states = {"Reset": False, "Close": False} + self.right_prev_button_states = {"Reset": False, "Close": False} + + def start_streaming(self): + self.left_device.start() + self.right_device.start() + + def __del__(self): + self.stop_streaming() + + def get(self): + """Request combined data and return transformations as StreamerOutput.""" + # Initialize data groups + ik_data = {} # For pose and joint data (ik_keys) + control_data = {} # For robot control commands (control_keys) + teleop_data = {} # For internal policy commands (teleop_keys) + + try: + # Request combined data from the server and wait until we get data + left_combined_data = None + right_combined_data = None + while not left_combined_data: + left_combined_data = self.left_device.get_cmd() + if not left_combined_data: + print("Waiting for left iPhone data...") + while not right_combined_data: + right_combined_data = self.right_device.get_cmd() + if not right_combined_data: + print("Waiting for right iPhone data...") + + # IK data - wrist poses and finger positions (ik_keys) + ik_data["left_wrist"] = np.array(left_combined_data.get("transformMatrix")) + ik_data["right_wrist"] = np.array(right_combined_data.get("transformMatrix")) + + # left button states + current_left_reset = left_combined_data.get("buttonStates").get("Reset") + current_left_close = left_combined_data.get("buttonStates").get("Close") + + # Trigger logic: only True when button transitions from False to True + left_reset = current_left_reset and not self.left_prev_button_states["Reset"] + + # Store current button states for next iteration + self.left_prev_button_states["Reset"] = current_left_reset + self.left_prev_button_states["Close"] = current_left_close + + # left fingers - IK data (ik_keys) + fingertips = np.zeros([25, 4, 4]) + positions = fingertips[:, :3, 3] + if current_left_close: + positions[4, 0] = 0 # closed + else: + positions[4, 0] = 1 # open + ik_data["left_fingers"] = {"position": fingertips} + + # right button states + current_right_reset = right_combined_data.get("buttonStates").get("Reset") + current_right_close = right_combined_data.get("buttonStates").get("Close") + + # Trigger logic: only True when button transitions from False to True + right_reset = current_right_reset and not self.right_prev_button_states["Reset"] + + # Store current button states for next iteration + self.right_prev_button_states["Reset"] = current_right_reset + self.right_prev_button_states["Close"] = current_right_close + + # right fingers - IK data (ik_keys) + fingertips = np.zeros([25, 4, 4]) + positions = fingertips[:, :3, 3] + if current_right_close: + positions[4, 0] = 0 # closed + else: + positions[4, 0] = 1 # open + ik_data["right_fingers"] = {"position": fingertips} + + # Teleop commands (teleop_keys) - used by TeleopPolicy for activation + teleop_data["toggle_activation"] = left_reset + + # Control commands (control_keys) - sent to robot + control_data["toggle_stand_command"] = right_reset + + except Exception as e: + print(f"Error while requesting iPhone data: {e}") + + # Return structured output + return StreamerOutput( + ik_data=ik_data, control_data=control_data, teleop_data=teleop_data, source="iphone" + ) + + def stop_streaming(self): + self.left_device.stop() + self.right_device.stop() + + +if __name__ == "__main__": + streamer = IphoneStreamer() + streamer.start_streaming() + while True: + data = streamer.get() + print(data) + time.sleep(0.1) diff --git a/gr00t_wbc/control/teleop/streamers/joycon_streamer.py b/gr00t_wbc/control/teleop/streamers/joycon_streamer.py new file mode 100644 index 0000000..7d40516 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/joycon_streamer.py @@ -0,0 +1,695 @@ +import time +from typing import Any, Dict + +from evdev import InputDevice, ecodes, list_devices +import numpy as np + +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput + + +class JoyConDevice: + """ + A class to handle a single Joy-Con controller using evdev. + Supports both individual Joy-Cons and their IMU sensors. + """ + + def __init__( + self, device_path: str = None, controller_type: str = "auto", silent: bool = False + ): + """ + Initialize the Joy-Con device. + + Args: + device_path: Path to the Joy-Con event device (e.g., /dev/input/event25) + controller_type: "left", "right", or "auto" + silent: Whether to suppress output messages + """ + self._silent = silent + self._device_path = device_path + self._controller_type = controller_type + self._device = None + self._prev_button_states = {} + + # Joy-Con button mappings (evdev key codes) + self._setup_mappings() + + # Current analog stick values + self._stick_x = 0.0 + self._stick_y = 0.0 + + # Axis codes (will be set in start() based on controller type) + self._x_axis_code = 0 # Default for left Joy-Con + self._y_axis_code = 1 # Default for left Joy-Con + + def _setup_mappings(self): + """Set up button and axis mappings for Joy-Con event codes.""" + # Left Joy-Con button mappings + self._left_button_mappings = { + # Left Joy-Con specific buttons + 544: "dpad_up", + 545: "dpad_down", + 546: "dpad_left", + 547: "dpad_right", + 309: "capture", + 314: "minus", + # Left Joy-Con shoulder/trigger buttons + 310: "L", # L button + 312: "ZL", # ZL trigger + 311: "SL", # SL side button + 313: "SR", # SR side button + } + + # Right Joy-Con button mappings + self._right_button_mappings = { + # Right Joy-Con specific buttons + 304: "a", + 305: "b", + 307: "x", + 308: "y", + 315: "plus", + 316: "home", + # Right Joy-Con shoulder/trigger buttons + 311: "R", # SL side button + 313: "ZR", # SR side button + 310: "SL", # R button + 312: "SR", # ZR trigger + } + + # Will be set in start() based on controller type + self._button_mappings = {} + self._prev_button_states = {} + + # Axis codes (set dynamically in start()) + self._x_axis_code = 0 + self._y_axis_code = 1 + + def _find_joycon_device(self): + """Find and return a Joy-Con device, waiting until one is found.""" + while True: + devices = [InputDevice(path) for path in list_devices()] + joycon_devices = [] + for device in devices: + if "Joy-Con" in device.name and "IMU" not in device.name: + joycon_devices.append(device) + + if not joycon_devices: + if not self._silent: + print("Warning: No Joy-Con devices found. Waiting for Joy-Con connection...") + time.sleep(1.0) # Wait 1 second before checking again + continue + + # Joy-Con devices found, proceed with selection + # If specific type requested, find matching device + if self._controller_type == "left": + for device in joycon_devices: + if "Joy-Con (L)" in device.name: + if not self._silent: + print(f"Found requested left Joy-Con: {device.name}") + return device + # If left Joy-Con not found but other Joy-Cons exist + if not self._silent: + print("Warning: Left Joy-Con requested but not found. Waiting...") + time.sleep(1.0) + continue + + elif self._controller_type == "right": + for device in joycon_devices: + if "Joy-Con (R)" in device.name: + if not self._silent: + print(f"Found requested right Joy-Con: {device.name}") + return device + # If right Joy-Con not found but other Joy-Cons exist + if not self._silent: + print("Warning: Right Joy-Con requested but not found. Waiting...") + time.sleep(1.0) + continue + + # Auto mode or fallback - return first available + if not self._silent: + print(f"Found Joy-Con in auto mode: {joycon_devices[0].name}") + return joycon_devices[0] + + def start(self): + """Start the Joy-Con device.""" + try: + if self._device_path: + # Use specific device path + self._device = InputDevice(self._device_path) + else: + # Auto-find Joy-Con device + self._device = self._find_joycon_device() + + # Determine actual controller type and axis codes from device name + if "Joy-Con (L)" in self._device.name: + self._controller_type = "left" + self._x_axis_code = 0 + self._y_axis_code = 1 + # Set up left Joy-Con specific button mappings + self._button_mappings = self._left_button_mappings.copy() + elif "Joy-Con (R)" in self._device.name: + self._controller_type = "right" + self._x_axis_code = 3 # Right Joy-Con uses ABS_RX (3) + self._y_axis_code = 4 # Right Joy-Con uses ABS_RY (4) + # Set up right Joy-Con specific button mappings + self._button_mappings = self._right_button_mappings.copy() + else: + # Unknown device, keep defaults + if not self._silent: + print(f"Warning: Unknown Joy-Con type: {self._device.name}") + self._button_mappings = self._left_button_mappings.copy() + + # Initialize button states + self._prev_button_states = {name: False for name in self._button_mappings.values()} + + if not self._silent: + print(f"Joy-Con connected: {self._device.name}") + print(f"Device path: {self._device.path}") + print(f"Controller type: {self._controller_type}") + print(f"Axis codes: X={self._x_axis_code}, Y={self._y_axis_code}") + print(f"Button mappings: {list(self._button_mappings.values())}") + print(f"Capabilities: {list(self._device.capabilities(verbose=True).keys())}") + + except Exception as e: + if not self._silent: + print(f"Failed to initialize Joy-Con: {e}") + raise e + + def stop(self): + """Stop the Joy-Con device.""" + if self._device: + try: + self._device.close() + except (OSError, AttributeError): + pass # Ignore errors during cleanup + if not self._silent: + print("Joy-Con disconnected.") + + def get_state(self) -> Dict[str, Any]: + """Get the current state of the Joy-Con by reading all available events.""" + if not self._device: + return {} + + try: + # Read all available events (non-blocking) + import select + + r, w, x = select.select([self._device], [], [], 0) # Non-blocking + + if r: + # Read ALL events available right now + events = self._device.read() + for event in events: + self._process_event(event) + + # Return current state + return self._build_state() + + except Exception as e: + if not self._silent: + print(f"Error reading Joy-Con state: {e}") + return {} + + def _process_event(self, event): + """Process a single input event from the Joy-Con.""" + if event.type == ecodes.EV_KEY: + # Button event + button_pressed = event.value == 1 + button_name = self._button_mappings.get(event.code) + if button_name: + self._prev_button_states[button_name] = button_pressed + + elif event.type == ecodes.EV_ABS: + # Analog stick event - use dynamic axis codes + if event.code == self._x_axis_code: + self._stick_x = event.value / 32767.0 + elif event.code == self._y_axis_code: + self._stick_y = -event.value / 32767.0 + + def _build_state(self): + """Build the current state dictionary.""" + if not self._device: + return {} + + # Get current button states and detect press events + buttons = {} + button_events = {} + + for button_name in self._button_mappings.values(): + current_state = self._prev_button_states.get(button_name, False) + buttons[button_name] = current_state + + # Detect button press events (transition from False to True) + prev_state = getattr(self, f"_prev_press_{button_name}", False) + button_events[f"{button_name}_pressed"] = current_state and not prev_state + setattr(self, f"_prev_press_{button_name}", current_state) + + # Package state + return { + "buttons": buttons, + "button_events": button_events, + "axes": {"stick_x": self._stick_x, "stick_y": self._stick_y}, + "timestamp": time.time(), + } + + +class JoyconStreamer(BaseStreamer): + """ + A streamer for Nintendo Joy-Con controllers following iPhone streamer pattern. + Supports two modes: locomotion and manipulation. + """ + + def __init__( + self, left_device_path: str = None, right_device_path: str = None, silent: bool = False + ): + """Initialize the Joy-Con streamer.""" + super().__init__() + + self._silent = silent + self.latest_data = {} + + # Mode management + self.reset_status() + + # Initialize devices + self.left_device = JoyConDevice(left_device_path, "left", silent) + self.right_device = JoyConDevice(right_device_path, "right", silent) + + def reset_status(self): + """Reset the cache of the streamer.""" + self.current_base_height = 0.74 # Initial base height, 0.74m (standing height) + self.stand_toggle_cooldown = 0.5 # prevent rapid stand toggling + self.last_stand_toggle_time = 0 + + def start_streaming(self): + """Start streaming from Joy-Con devices.""" + try: + if not self._silent: + print("Starting Joy-Con devices...") + + # Start devices and wait until both are connected + self.left_device.start() + self.right_device.start() + + # Wait until both devices are ready + max_wait_time = 10 # seconds + start_time = time.time() + while (time.time() - start_time) < max_wait_time: + left_state = self.left_device.get_state() + right_state = self.right_device.get_state() + + if left_state and right_state: + if not self._silent: + print("Both Joy-Con devices connected successfully!") + break + + if not self._silent: + print("Waiting for both Joy-Con devices to connect...") + time.sleep(0.5) + else: + print("Warning: Timeout waiting for both Joy-Con devices") + + if not self._silent: + print("Joy-Con streamer started in unified mode") + print("Controls:") + print(" ZL+ZR: Toggle stand command") + print(" D-pad Up/Down: Increase/Decrease base height") + print(" Capture (Left): Toggle locomotion policy (e-stop for lower body)") + print(" L/R shoulders: Left/Right finger open/close") + print(" Home button: Toggle activation") + print(" Left stick: Forward/backward and strafe movement") + print(" Right stick: Yaw rotation") + print(" A button: Toggle data collection") + print(" B button: Abort current episode") + + except Exception as e: + if not self._silent: + print(f"Failed to start Joy-Con streaming: {e}") + raise e + + def stop_streaming(self): + """Stop streaming from Joy-Con devices.""" + if self.left_device: + self.left_device.stop() + if self.right_device: + self.right_device.stop() + + if not self._silent: + print("Joy-Con streaming stopped") + + def _get_joycon_state(self): + """ + Get combined Joy-Con state with error handling. + DDA: Warning and wait until we got all left and right device data. + """ + left_state = self.left_device.get_state() if self.left_device else {} + right_state = self.right_device.get_state() if self.right_device else {} + + # Check if we have valid data from both devices + if not left_state or not right_state: + if not self._silent: + missing = [] + if not left_state: + missing.append("left") + if not right_state: + missing.append("right") + print(f"Warning: Missing Joy-Con data from {', '.join(missing)} device(s)") + + # Return empty data structure if either device is missing + return { + "left_button_states": {}, + "right_button_states": {}, + "left_stick_inputs": {}, + "right_stick_inputs": {}, + } + + # Combine states + combined_data = { + "left_button_states": left_state.get("buttons", {}), + "right_button_states": right_state.get("buttons", {}), + "left_stick_inputs": left_state.get("axes", {}), + "right_stick_inputs": right_state.get("axes", {}), + "left_button_events": left_state.get("button_events", {}), + "right_button_events": right_state.get("button_events", {}), + } + return combined_data + + def _handle_stand_toggle(self, joycon_data): + """Handle stand toggle command via ZL+ZR buttons.""" + current_time = time.time() + if (current_time - self.last_stand_toggle_time) > self.stand_toggle_cooldown: + # Use button states to check if both triggers are currently pressed + left_buttons = joycon_data.get("left_button_states", {}) + right_buttons = joycon_data.get("right_button_states", {}) + + # Use button events to detect if at least one trigger was just pressed + left_button_events = joycon_data.get("left_button_events", {}) + right_button_events = joycon_data.get("right_button_events", {}) + + # Check if both triggers are held AND at least one was just pressed + both_triggers_held = left_buttons.get("ZL", False) and right_buttons.get("ZR", False) + at_least_one_just_pressed = left_button_events.get( + "ZL_pressed", False + ) or right_button_events.get("ZR_pressed", False) + + if both_triggers_held and at_least_one_just_pressed: + self.last_stand_toggle_time = current_time + if not self._silent: + print("Stand toggle activated") + return True + return False + + def _apply_dead_zone(self, value, dead_zone): + """Apply dead zone and normalize.""" + if abs(value) < dead_zone: + return 0.0 + sign = 1 if value > 0 else -1 + # Normalize the output to be between -1 and 1 after dead zone + return sign * (abs(value) - dead_zone) / (1.0 - dead_zone) + + def _handle_height_adjustment(self, joycon_data): + """Handle base height adjustment via d-pad up/down.""" + left_button_states = joycon_data.get("left_button_states", {}) + + # Check d-pad button states + dpad_up = left_button_states.get("dpad_up", False) + dpad_down = left_button_states.get("dpad_down", False) + + # Incremental height adjustment + height_increment = 0.005 # Small step per call when button is pressed + + if dpad_up: + self.current_base_height += height_increment + elif dpad_down: + self.current_base_height -= height_increment + + # Clamp to bounds + self.current_base_height = np.clip(self.current_base_height, 0.2, 0.74) + + def _detect_stand_toggle(self, joycon_data): + """Detect stand/walk toggle command - triggered by ZL+ZR.""" + return self._handle_stand_toggle(joycon_data) + + def _detect_locomotion_policy_toggle(self, joycon_data): + """Detect locomotion policy toggle command using left Joy-Con capture button.""" + left_button_events = joycon_data.get("left_button_events", {}) + return left_button_events.get("capture_pressed", False) + + def _detect_emergency_stop(self, joycon_data): + """Detect emergency stop command (Placeholder - not implemented yet).""" + return False # Not implemented yet + + def _detect_data_collection_toggle(self, joycon_data): + """Detect data collection toggle command using right Joy-Con A button.""" + right_button_events = joycon_data.get("right_button_events", {}) + return right_button_events.get("a_pressed", False) + + def _detect_abort_toggle(self, joycon_data): + """Detect abort toggle command using right Joy-Con B button.""" + right_button_events = joycon_data.get("right_button_events", {}) + return right_button_events.get("b_pressed", False) + + def _generate_finger_data(self, shoulder_button_pressed): + """Generate finger position data similar to iPhone streamer.""" + fingertips = np.zeros([25, 4, 4]) + + # Set identity matrices for all finger joints + for i in range(25): + fingertips[i] = np.eye(4) + + # Control thumb based on shoulder button state (index 4 is thumb tip) + if shoulder_button_pressed: + fingertips[4, 0, 3] = 0.0 # closed + else: + fingertips[4, 0, 3] = 1.0 # open + + return fingertips + + def _generate_unified_raw_data(self, joycon_data): + """Generate unified raw_data combining navigation, finger control, and height adjustment.""" + + # Extract stick inputs + left_stick = joycon_data.get("left_stick_inputs", {}) + right_stick = joycon_data.get("right_stick_inputs", {}) + + # Extract button/trigger states for finger control + left_buttons = joycon_data.get("left_button_states", {}) + right_buttons = joycon_data.get("right_button_states", {}) + + # Handle d-pad height adjustment + self._handle_height_adjustment(joycon_data) + + # Map to velocity commands with dead zones and scaling + DEAD_ZONE = 0.1 + MAX_LINEAR_VEL = 0.2 # m/s + MAX_ANGULAR_VEL = 0.5 # rad/s + + # Left stick Y for forward/backward (lin_vel_x), X for strafe (lin_vel_y). + # Right stick X for yaw (ang_vel_z). Verify command signs. + fwd_bwd_input = left_stick.get("stick_y", 0.0) + strafe_input = -left_stick.get("stick_x", 0.0) # Flip sign for intuitive left/right + yaw_input = -right_stick.get("stick_x", 0.0) + + lin_vel_x = self._apply_dead_zone(fwd_bwd_input, DEAD_ZONE) * MAX_LINEAR_VEL + lin_vel_y = self._apply_dead_zone(strafe_input, DEAD_ZONE) * MAX_LINEAR_VEL # Strafe + ang_vel_z = self._apply_dead_zone(yaw_input, DEAD_ZONE) * MAX_ANGULAR_VEL + + # Extract home button press event for toggle activation + right_button_events = joycon_data.get("right_button_events", {}) + home_button_pressed = right_button_events.get("home_pressed", False) + + # Map L/R (shoulder buttons) to finger control + left_shoulder_pressed = left_buttons.get("L", False) + right_shoulder_pressed = right_buttons.get("R", False) + + # Generate finger data based on shoulder button states + left_fingers = self._generate_finger_data(left_shoulder_pressed) + right_fingers = self._generate_finger_data(right_shoulder_pressed) + + return StreamerOutput( + ik_data={ + "left_fingers": {"position": left_fingers}, + "right_fingers": {"position": right_fingers}, + }, + control_data={ + "base_height_command": self.current_base_height, + "navigate_cmd": [lin_vel_x, lin_vel_y, ang_vel_z], + "toggle_stand_command": self._detect_stand_toggle(joycon_data), + "toggle_policy_action": self._detect_locomotion_policy_toggle(joycon_data), + }, + teleop_data={ + "toggle_activation": home_button_pressed, + }, + data_collection_data={ + "toggle_data_collection": self._detect_data_collection_toggle(joycon_data), + "toggle_data_abort": self._detect_abort_toggle(joycon_data), + }, + source="joycon", + ) + + def get(self) -> StreamerOutput: + """ + Return StreamerOutput with unified control data. + """ + # Get current Joy-Con state + joycon_data = self._get_joycon_state() + + # Generate unified structured output + raw_data = self._generate_unified_raw_data(joycon_data) + return raw_data + + def __del__(self): + """Cleanup when the streamer is destroyed.""" + self.stop_streaming() + + +def debug_joycons(controller_type="both", duration=30): + """Debug function to show Joy-Con devices and capture real-time events.""" + print(f"\n=== JOY-CON DEBUG ({controller_type.upper()}) ===") + + # Show available devices + devices = [InputDevice(path) for path in list_devices()] + joycon_devices = [d for d in devices if "Joy-Con" in d.name and "IMU" not in d.name] + print(f"Found {len(joycon_devices)} Joy-Con devices:") + for device in joycon_devices: + print(f" {device.name} at {device.path}") + + if not joycon_devices: + print("No Joy-Con devices found!") + return + + # Select devices to monitor + devices_to_monitor = [] + if controller_type in ["left", "both"]: + devices_to_monitor.extend([d for d in joycon_devices if "Joy-Con (L)" in d.name]) + if controller_type in ["right", "both"]: + devices_to_monitor.extend([d for d in joycon_devices if "Joy-Con (R)" in d.name]) + + if not devices_to_monitor: + print(f"No {controller_type} Joy-Con found!") + return + + print(f"\nMonitoring events for {duration}s (Press Ctrl+C to stop)...") + + import select + + start_time = time.time() + try: + while (time.time() - start_time) < duration: + r, w, x = select.select(devices_to_monitor, [], [], 0.1) + for device in r: + events = device.read() + for event in events: + if event.type in [ecodes.EV_KEY, ecodes.EV_ABS]: + event_type = "KEY" if event.type == ecodes.EV_KEY else "ABS" + device_name = "L" if "Joy-Con (L)" in device.name else "R" + print(f"[{device_name}] {event_type}:{event.code}:{event.value}") + except KeyboardInterrupt: + print("\nStopped.") + finally: + for device in devices_to_monitor: + device.close() + + print("=== END DEBUG ===\n") + + +def test_unified_controls(): + """Test unified control system with all functionality available.""" + print("=== Testing Unified Controls ===") + print("Controls:") + print(" ZL+ZR: Toggle stand command") + print(" D-pad Up/Down: Increase/Decrease base height") + print(" Capture (Left): Toggle locomotion policy (e-stop for lower body)") + print(" Left stick: Forward/backward and strafe movement") + print(" Right stick: Yaw rotation") + print(" L/R shoulders: Left/Right finger open/close") + print(" Home button: Toggle activation") + print(" A button: Toggle data collection") + print(" B button: Abort current episode") + print("Press Ctrl+C to stop\n") + + streamer = JoyconStreamer(silent=False) + + try: + streamer.start_streaming() + + while True: + data = streamer.get() + + # Extract all control data + nav_cmd = data.control_data.get("navigate_cmd", [0, 0, 0]) + height_cmd = data.control_data.get("base_height_command", 0.78) + stand_toggle = data.control_data.get("toggle_stand_command", False) + policy_action = data.control_data.get("toggle_policy_action", False) + toggle_activation = data.teleop_data.get("toggle_activation", False) + toggle_data_collection = data.data_collection_data.get("toggle_data_collection", False) + toggle_data_abort = data.data_collection_data.get("toggle_data_abort", False) + + # Get finger states + left_fingers = data.ik_data.get("left_fingers", {}).get( + "position", np.zeros([25, 4, 4]) + ) + right_fingers = data.ik_data.get("right_fingers", {}).get( + "position", np.zeros([25, 4, 4]) + ) + left_closed = ( + left_fingers[4, 0, 3] == 0.0 if left_fingers.shape == (25, 4, 4) else False + ) + right_closed = ( + right_fingers[4, 0, 3] == 0.0 if right_fingers.shape == (25, 4, 4) else False + ) + + print( + f"UNIFIED - Nav:[{nav_cmd[0]:.2f},{nav_cmd[1]:.2f},{nav_cmd[2]:.2f}] " + f"Height:{height_cmd:.2f} L:{left_closed} R:{right_closed} " + f"StandToggle:{stand_toggle} PolicyAction:{policy_action} " + f"ToggleActivation:{toggle_activation} " + f"ToggleDataCollection:{toggle_data_collection} ToggleDataAbort:{toggle_data_abort}" + ) + + time.sleep(0.1) + + except KeyboardInterrupt: + print("\nStopping unified controls test...") + finally: + streamer.stop_streaming() + + +if __name__ == "__main__": + import sys + + # Check for debug mode + if len(sys.argv) > 1 and sys.argv[1] == "debug_both": + debug_joycons(controller_type="both", duration=60) + sys.exit(0) + + # Check for test mode + if len(sys.argv) > 1 and sys.argv[1] == "test_unified": + test_unified_controls() + sys.exit(0) + + # Default: show usage and run unified controls test + print("Joy-Con Teleop Streamer - Unified Mode") + print("Usage:") + print(" python joycon_streamer.py debug_both - Debug both Joy-Con devices") + print(" python joycon_streamer.py test_unified - Test unified controls") + print() + print("Unified Button Mapping:") + print("| Input | Function |") + print("|-------|----------|") + print("| Left Stick Y | Forward/Backward movement |") + print("| Left Stick X | Strafe Left/Right |") + print("| Right Stick X | Yaw rotation |") + print("| D-pad Up | Increase base height |") + print("| D-pad Down | Decrease base height |") + print("| L (Left Shoulder) | Left finger open/close |") + print("| R (Right Shoulder) | Right finger open/close |") + print("| Home (Right Joy-Con) | Toggle activation |") + print("| Capture (Left Joy-Con) | Toggle locomotion policy (e-stop) |") + print("| ZL+ZR (Triggers) | Toggle stand command |") + print("| A (Right Joy-Con) | Start/Stop Data Collection |") + print("| B (Right Joy-Con) | Abort Current Episode |") + print() + print("Running unified controls test by default...") + print() + + test_unified_controls() diff --git a/gr00t_wbc/control/teleop/streamers/leapmotion_streamer.py b/gr00t_wbc/control/teleop/streamers/leapmotion_streamer.py new file mode 100644 index 0000000..d01bd5b --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/leapmotion_streamer.py @@ -0,0 +1,289 @@ +from copy import deepcopy +import pickle +import queue +import threading +import time +from typing import Dict, List, Tuple + +import leap +import leap.events +import numpy as np +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput + + +def xyz2np_array(position: leap.datatypes.Vector) -> np.ndarray: + return np.array([position.x, position.y, position.z]) + + +def quat2np_array(quaternion: leap.datatypes.Quaternion) -> np.ndarray: + return np.array([quaternion.x, quaternion.y, quaternion.z, quaternion.w]) + + +def get_raw_finger_points(hand: leap.datatypes.Hand) -> np.ndarray: + target_points = np.array([np.eye(4) for _ in range(25)]) + target_points[0, :3, :3] = R.from_quat(quat2np_array(hand.palm.orientation)).as_matrix() + target_points[0, :3, 3] = xyz2np_array(hand.arm.next_joint) + target_points[1, :3, 3] = xyz2np_array(hand.thumb.bones[0].next_joint) + target_points[2, :3, 3] = xyz2np_array(hand.thumb.bones[1].next_joint) + target_points[3, :3, 3] = xyz2np_array(hand.thumb.bones[2].next_joint) + target_points[4, :3, 3] = xyz2np_array(hand.thumb.bones[3].next_joint) + target_points[5, :3, 3] = xyz2np_array(hand.index.bones[0].prev_joint) + target_points[6, :3, 3] = xyz2np_array(hand.index.bones[0].next_joint) + target_points[7, :3, 3] = xyz2np_array(hand.index.bones[1].next_joint) + target_points[8, :3, 3] = xyz2np_array(hand.index.bones[2].next_joint) + target_points[9, :3, 3] = xyz2np_array(hand.index.bones[3].next_joint) + target_points[10, :3, 3] = xyz2np_array(hand.middle.bones[0].prev_joint) + target_points[11, :3, 3] = xyz2np_array(hand.middle.bones[0].next_joint) + target_points[12, :3, 3] = xyz2np_array(hand.middle.bones[1].next_joint) + target_points[13, :3, 3] = xyz2np_array(hand.middle.bones[2].next_joint) + target_points[14, :3, 3] = xyz2np_array(hand.middle.bones[3].next_joint) + target_points[15, :3, 3] = xyz2np_array(hand.ring.bones[0].prev_joint) + target_points[16, :3, 3] = xyz2np_array(hand.ring.bones[0].next_joint) + target_points[17, :3, 3] = xyz2np_array(hand.ring.bones[1].next_joint) + target_points[18, :3, 3] = xyz2np_array(hand.ring.bones[2].next_joint) + target_points[19, :3, 3] = xyz2np_array(hand.ring.bones[3].next_joint) + target_points[20, :3, 3] = xyz2np_array(hand.pinky.bones[0].prev_joint) + target_points[21, :3, 3] = xyz2np_array(hand.pinky.bones[0].next_joint) + target_points[22, :3, 3] = xyz2np_array(hand.pinky.bones[1].next_joint) + target_points[23, :3, 3] = xyz2np_array(hand.pinky.bones[2].next_joint) + target_points[24, :3, 3] = xyz2np_array(hand.pinky.bones[3].next_joint) + + return target_points + + +def get_fake_finger_points_from_pinch(hand: leap.datatypes.Hand) -> np.ndarray: + # print(hand.pinch_strength) + target_points = np.array([np.eye(4) for _ in range(25)]) + for i in range(25): + target_points[i] = np.eye(4) + + # Control thumb based on shoulder button state (index 4 is thumb tip) + if hand.pinch_strength > 0.3: + target_points[4, 0, 3] = 0.0 # closed + else: + target_points[4, 0, 3] = 1000.0 # open, in mm + + return target_points + + +def get_raw_wrist_pose(hand: leap.datatypes.Hand) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + hand_palm_position = xyz2np_array(hand.palm.position) + hand_palm_normal = np.array([-hand.palm.normal.z, -hand.palm.normal.x, hand.palm.normal.y]) + hand_palm_direction = np.array( + [-hand.palm.direction.z, -hand.palm.direction.x, hand.palm.direction.y] + ) + + return hand_palm_position, hand_palm_normal, hand_palm_direction + + +def get_finger_transform(target_points: np.ndarray, hand_type: str) -> np.ndarray: + pose_palm_root = target_points[0, :3, 3].copy() + rot_palm = target_points[0, :3, :3].copy() + + if hand_type == "right": + rot_leap2base = R.from_euler("z", [180], degrees=True).as_matrix() + rot_reverse = np.array( + [[[0, 0, 1], [0, -1, 0], [1, 0, 0]]] + ) # due to the target_base_rotation in hand IK solver + else: + rot_leap2base = np.eye(3) + rot_reverse = np.array([[[0, 0, -1], [0, -1, 0], [-1, 0, 0]]]) + offset = (target_points[:, :3, 3] - pose_palm_root).copy() / 1000.0 # mm to m + offset = offset @ rot_palm @ rot_leap2base @ rot_reverse + + target_points[:, :3, 3] = offset + + return target_points + + +def get_wrist_transformation( + hand_palm_position: np.ndarray, + hand_palm_normal: np.ndarray, + hand_palm_direction: np.ndarray, + hand_type: str, + pos_sensitivity: float = 1.0 / 800.0, +) -> np.ndarray: + T = np.eye(4) + T[:3, 3] = hand_palm_position[[2, 0, 1]] * pos_sensitivity * np.array([-1, -1, 1]) + direction_np = hand_palm_direction + palm_normal_np = hand_palm_normal + if hand_type == "left": + transform = R.from_euler("y", -90, degrees=True).as_matrix() + lh_thumb_np = -np.cross(direction_np, palm_normal_np) + rotation_matrix = np.array([direction_np, -palm_normal_np, lh_thumb_np]).T + else: + transform = R.from_euler("xy", [-180, 90], degrees=True).as_matrix() + rh_thumb_np = np.cross(direction_np, palm_normal_np) + rotation_matrix = np.array([direction_np, palm_normal_np, rh_thumb_np]).T + T[:3, :3] = np.dot(rotation_matrix, transform) + return T + + +def get_raw_data( + hands: List[leap.datatypes.Hand], data: Dict[str, np.ndarray] = None +) -> Dict[str, np.ndarray]: + if data is None: + data = {} + for hand in hands: + hand_type = "left" if str(hand.type) == "HandType.Left" else "right" + data[hand_type + "_wrist"] = get_raw_wrist_pose(hand) + # @runyud: this is a hack to get the finger points from the pinch strength + data[hand_type + "_fingers"] = get_fake_finger_points_from_pinch(hand) + assert len(data) == 4, f"Leapmotiondata length should be 4, but got {len(data)}" + return data + + +def process_data(raw_data: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]: + data = {} + for hand_type in ["left", "right"]: + data[hand_type + "_wrist"] = get_wrist_transformation( + *raw_data[hand_type + "_wrist"], hand_type + ) + data[hand_type + "_fingers"] = { + "position": get_finger_transform(raw_data[hand_type + "_fingers"], hand_type) + } + return data + + +class LeapMotionListener(leap.Listener): + def __init__(self, verbose=False): + self.data = None + self.data_lock = threading.Lock() + self.verbose = verbose + + def on_connection_event(self, event): + print("Connected") + + def on_device_event(self, event: leap.events.DeviceEvent): + try: + with event.device.open(): + info = event.device.get_info() + except AttributeError: + # Handle the case where LeapCannotOpenDeviceError is not available + try: + info = event.device.get_info() + except Exception as e: + print(f"Error opening device: {e}") + info = None + + print(f"Found Leap Motion device {info.serial}") + + def on_tracking_event(self, event: leap.events.TrackingEvent): + if ( + len(event.hands) == 2 or self.data is not None + ): # only when two hands are detected, we update the data + with self.data_lock: + self.data: Dict[str, np.ndarray] = get_raw_data(event.hands, self.data) + if self.verbose: + for hand in event.hands: + hand_type = "left" if str(hand.type) == "HandType.Left" else "right" + print( + f"Hand id {hand.id} is a {hand_type} hand with position" + f"({hand.palm.position.x}, {hand.palm.position.y}, {hand.palm.position.z})." + ) + + def get_data(self): + with self.data_lock: + return deepcopy(self.data) + + def reset_status(self): + """Reset the cache of the streamer.""" + with self.data_lock: + self.data = None + + +class FakeLeapMotionListener: + def __init__(self, data_path=None): + self.data_lock = threading.Lock() + with open(data_path, "rb") as f: + self.data_list = pickle.load(f) + self.data_index = 0 + + def get_data(self) -> Dict[str, np.ndarray]: + with self.data_lock: + data = deepcopy(self.data_list[self.data_index % len(self.data_list)]) + self.data_index += 1 + return data + + +# Note currently it is a auto-polling based streamer. +# The connection will start another thread to continue polling data +# and the listener will continue to receive data from the connection. +class LeapMotionStreamer(BaseStreamer): + """LeapMotion streamer that provides hand tracking data.""" + + def __init__(self, verbose=False, record_data=False, **kwargs): + self.connection = leap.Connection() + self.listener = LeapMotionListener(verbose=verbose) + self.connection.add_listener(self.listener) + # self.listener = FakeLeapMotionListener(data_path="leapmotion_data_rot.pkl") + + self.connection.set_tracking_mode(leap.TrackingMode.Desktop) + + self.record_data = record_data + self.data_queue = queue.Queue() + + def reset_status(self): + """Reset the cache of the streamer.""" + self.listener.reset_status() + while not self.listener.get_data(): + time.sleep(0.1) + + def start_streaming(self): + self.connection.connect() + print("Waiting for the first data...") + time.sleep(0.5) + while not self.listener.get_data(): + time.sleep(0.1) + print("First data received!") + + def stop_streaming(self): + self.connection.disconnect() + + def get(self) -> StreamerOutput: + """Return hand tracking data as StreamerOutput.""" + # Get raw data and save if recording + raw_data = self.listener.get_data() + if self.record_data: + self.data_queue.put(raw_data) + + # Process raw data into transformations + processed_data = process_data(raw_data) + + # Initialize IK data (ik_keys) - LeapMotion provides hand/finger tracking + ik_data = {} + for hand_type in ["left", "right"]: + # Add wrist poses and finger positions to IK data + ik_data[f"{hand_type}_wrist"] = processed_data[f"{hand_type}_wrist"] + ik_data[f"{hand_type}_fingers"] = processed_data[f"{hand_type}_fingers"] + + # Return structured output - LeapMotion only provides IK data + return StreamerOutput( + ik_data=ik_data, + control_data={}, # No control commands from LeapMotion + teleop_data={}, # No teleop commands from LeapMotion + source="leapmotion", + ) + + def dump_data_to_file(self): + """Save recorded data to file if recording was enabled.""" + if not self.record_data: + return + + data_list = [] + while not self.data_queue.empty(): + data_list.append(self.data_queue.get()) + with open("leapmotion_data_trans.pkl", "wb") as f: + pickle.dump(data_list, f) + + +if __name__ == "__main__": + streamer = LeapMotionStreamer(verbose=True) + streamer.start_streaming() + for _ in range(100): + streamer.get() + time.sleep(0.1) + streamer.stop_streaming() + streamer.dump_data_to_file() diff --git a/gr00t_wbc/control/teleop/streamers/manus_streamer.py b/gr00t_wbc/control/teleop/streamers/manus_streamer.py new file mode 100644 index 0000000..284bdd6 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/manus_streamer.py @@ -0,0 +1,90 @@ +import pickle +import threading + +import zmq + +from gr00t_wbc.control.teleop.device.manus import Manus +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput + + +class ManusStreamer(BaseStreamer): + def __init__(self, port=5556): + self.port = port + self.context = None + self.socket = None + self.manus_server = None + self.server_thread = None + + def request_data(self): + """Request the latest data from the server.""" + if self.socket is None: + raise RuntimeError("ManusStreamer not started. Call start_streaming() first.") + + # Send request to the server + self.socket.send(b"request_data") # Send a request message + + # Wait for the server's response + message = self.socket.recv() # Receive response + data = pickle.loads(message) # Deserialize the data + + return data + + def _run_server(self): + """Run the manus server in a separate thread.""" + with self.manus_server.activate(): + print("Manus server activated") + self.manus_server.run() + + def start_streaming(self): + """Start the manus server and establish connection.""" + if self.manus_server is not None: + return + + print(f"Starting manus server on port {self.port}...") + + # Create manus server instance + self.manus_server = Manus(port=self.port) + + # Start server in separate thread + self.server_thread = threading.Thread(target=self._run_server, daemon=True) + self.server_thread.start() + + # Establish ZMQ connection + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REQ) + self.socket.connect(f"tcp://localhost:{self.port}") + + def get(self): + """Return hand tracking data as StreamerOutput.""" + raw_data = self.request_data() + + # Initialize IK data (ik_keys) - Manus provides hand/finger tracking + ik_data = {} + if isinstance(raw_data, dict): + # Extract finger and hand pose data + for key, value in raw_data.items(): + if "finger" in key.lower() or "hand" in key.lower(): + ik_data[key] = value + + # Return structured output - Manus only provides IK data + return StreamerOutput( + ik_data=ik_data, + control_data={}, # No control commands from Manus + teleop_data={}, # No teleop commands from Manus + source="manus", + ) + + def stop_streaming(self): + """Stop the manus server and close connections.""" + # Close ZMQ connection + if self.socket: + self.socket.close() + self.socket = None + + if self.context: + self.context.term() + self.context = None + + # Reset server references + self.manus_server = None + self.server_thread = None diff --git a/gr00t_wbc/control/teleop/streamers/pico_streamer.py b/gr00t_wbc/control/teleop/streamers/pico_streamer.py new file mode 100644 index 0000000..2734a44 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/pico_streamer.py @@ -0,0 +1,280 @@ +import subprocess +import time + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from gr00t_wbc.control.teleop.device.pico.xr_client import XrClient +from gr00t_wbc.control.teleop.streamers.base_streamer import BaseStreamer, StreamerOutput + +R_HEADSET_TO_WORLD = np.array( + [ + [0, 0, -1], + [-1, 0, 0], + [0, 1, 0], + ] +) + + +class PicoStreamer(BaseStreamer): + def __init__(self): + self.xr_client = XrClient() + self.run_pico_service() + + self.reset_status() + + def run_pico_service(self): + # Run the pico service + self.pico_service_pid = subprocess.Popen( + ["bash", "/opt/apps/roboticsservice/runService.sh"] + ) + print(f"Pico service running with pid {self.pico_service_pid.pid}") + + def stop_pico_service(self): + # find pid and kill it + if self.pico_service_pid: + subprocess.Popen(["kill", "-9", str(self.pico_service_pid.pid)]) + print(f"Pico service killed with pid {self.pico_service_pid.pid}") + else: + print("Pico service not running") + + def reset_status(self): + self.current_base_height = 0.74 # Initial base height, 0.74m (standing height) + self.toggle_policy_action_last = False + self.toggle_activation_last = False + self.toggle_data_collection_last = False + self.toggle_data_abort_last = False + + def start_streaming(self): + pass + + def stop_streaming(self): + self.xr_client.close() + + def get(self) -> StreamerOutput: + pico_data = self._get_pico_data() + + raw_data = self._generate_unified_raw_data(pico_data) + return raw_data + + def __del__(self): + pass + + def _get_pico_data(self): + pico_data = {} + + # Get the pose of the left and right controllers and the headset + pico_data["left_pose"] = self.xr_client.get_pose_by_name("left_controller") + pico_data["right_pose"] = self.xr_client.get_pose_by_name("right_controller") + pico_data["head_pose"] = self.xr_client.get_pose_by_name("headset") + + # Get key value of the left and right controllers + pico_data["left_trigger"] = self.xr_client.get_key_value_by_name("left_trigger") + pico_data["right_trigger"] = self.xr_client.get_key_value_by_name("right_trigger") + pico_data["left_grip"] = self.xr_client.get_key_value_by_name("left_grip") + pico_data["right_grip"] = self.xr_client.get_key_value_by_name("right_grip") + + # Get button state of the left and right controllers + pico_data["A"] = self.xr_client.get_button_state_by_name("A") + pico_data["B"] = self.xr_client.get_button_state_by_name("B") + pico_data["X"] = self.xr_client.get_button_state_by_name("X") + pico_data["Y"] = self.xr_client.get_button_state_by_name("Y") + pico_data["left_menu_button"] = self.xr_client.get_button_state_by_name("left_menu_button") + pico_data["right_menu_button"] = self.xr_client.get_button_state_by_name( + "right_menu_button" + ) + pico_data["left_axis_click"] = self.xr_client.get_button_state_by_name("left_axis_click") + pico_data["right_axis_click"] = self.xr_client.get_button_state_by_name("right_axis_click") + + # Get the timestamp of the left and right controllers + pico_data["timestamp"] = self.xr_client.get_timestamp_ns() + + # Get the hand tracking state of the left and right controllers + pico_data["left_hand_tracking_state"] = self.xr_client.get_hand_tracking_state("left") + pico_data["right_hand_tracking_state"] = self.xr_client.get_hand_tracking_state("right") + + # Get the joystick state of the left and right controllers + pico_data["left_joystick"] = self.xr_client.get_joystick_state("left") + pico_data["right_joystick"] = self.xr_client.get_joystick_state("right") + + # Get the motion tracker data + pico_data["motion_tracker_data"] = self.xr_client.get_motion_tracker_data() + + # Get the body tracking data + pico_data["body_tracking_data"] = self.xr_client.get_body_tracking_data() + + return pico_data + + def _generate_unified_raw_data(self, pico_data): + # Get controller position and orientation in z up world frame + left_controller_T = self._process_xr_pose(pico_data["left_pose"], pico_data["head_pose"]) + right_controller_T = self._process_xr_pose(pico_data["right_pose"], pico_data["head_pose"]) + + # Get navigation commands + DEAD_ZONE = 0.1 + MAX_LINEAR_VEL = 0.5 # m/s + MAX_ANGULAR_VEL = 1.0 # rad/s + + fwd_bwd_input = pico_data["left_joystick"][1] + strafe_input = -pico_data["left_joystick"][0] + yaw_input = -pico_data["right_joystick"][0] + + lin_vel_x = self._apply_dead_zone(fwd_bwd_input, DEAD_ZONE) * MAX_LINEAR_VEL + lin_vel_y = self._apply_dead_zone(strafe_input, DEAD_ZONE) * MAX_LINEAR_VEL + ang_vel_z = self._apply_dead_zone(yaw_input, DEAD_ZONE) * MAX_ANGULAR_VEL + + # Get base height command + height_increment = 0.01 # Small step per call when button is pressed + if pico_data["Y"]: + self.current_base_height += height_increment + elif pico_data["X"]: + self.current_base_height -= height_increment + self.current_base_height = np.clip(self.current_base_height, 0.2, 0.74) + + # Get gripper commands + left_fingers = self._generate_finger_data(pico_data, "left") + right_fingers = self._generate_finger_data(pico_data, "right") + + # Get activation commands + toggle_policy_action_tmp = pico_data["left_menu_button"] and ( + pico_data["left_trigger"] > 0.5 + ) + toggle_activation_tmp = pico_data["left_menu_button"] and (pico_data["right_trigger"] > 0.5) + + if self.toggle_policy_action_last != toggle_policy_action_tmp: + toggle_policy_action = toggle_policy_action_tmp + else: + toggle_policy_action = False + self.toggle_policy_action_last = toggle_policy_action_tmp + + if self.toggle_activation_last != toggle_activation_tmp: + toggle_activation = toggle_activation_tmp + else: + toggle_activation = False + self.toggle_activation_last = toggle_activation_tmp + + # Get data collection commands + toggle_data_collection_tmp = pico_data["A"] + toggle_data_abort_tmp = pico_data["B"] + + if self.toggle_data_collection_last != toggle_data_collection_tmp: + toggle_data_collection = toggle_data_collection_tmp + else: + toggle_data_collection = False + self.toggle_data_collection_last = toggle_data_collection_tmp + + if self.toggle_data_abort_last != toggle_data_abort_tmp: + toggle_data_abort = toggle_data_abort_tmp + else: + toggle_data_abort = False + self.toggle_data_abort_last = toggle_data_abort_tmp + + # print(f"toggle_data_collection: {toggle_data_collection}, toggle_data_abort: {toggle_data_abort}") + + return StreamerOutput( + ik_data={ + "left_wrist": left_controller_T, + "right_wrist": right_controller_T, + "left_fingers": {"position": left_fingers}, + "right_fingers": {"position": right_fingers}, + }, + control_data={ + "base_height_command": self.current_base_height, + "navigate_cmd": [lin_vel_x, lin_vel_y, ang_vel_z], + "toggle_policy_action": toggle_policy_action, + }, + teleop_data={ + "toggle_activation": toggle_activation, + }, + data_collection_data={ + "toggle_data_collection": toggle_data_collection, + "toggle_data_abort": toggle_data_abort, + }, + source="pico", + ) + + def _process_xr_pose(self, controller_pose, headset_pose): + # Convert controller pose to x, y, z, w quaternion + xr_pose_xyz = np.array(controller_pose)[:3] # x, y, z + xr_pose_quat = np.array(controller_pose)[3:] # x, y, z, w + + # Handle all-zero quaternion case by using identity quaternion + if np.allclose(xr_pose_quat, 0): + xr_pose_quat = np.array([0, 0, 0, 1]) # identity quaternion: x, y, z, w + + # Convert from y up to z up + xr_pose_xyz = R_HEADSET_TO_WORLD @ xr_pose_xyz + xr_pose_rotation = R.from_quat(xr_pose_quat).as_matrix() + xr_pose_rotation = R_HEADSET_TO_WORLD @ xr_pose_rotation @ R_HEADSET_TO_WORLD.T + + # Convert headset pose to x, y, z, w quaternion + headset_pose_xyz = np.array(headset_pose)[:3] + headset_pose_quat = np.array(headset_pose)[3:] + + if np.allclose(headset_pose_quat, 0): + headset_pose_quat = np.array([0, 0, 0, 1]) # identity quaternion: x, y, z, w + + # Convert from y up to z up + headset_pose_xyz = R_HEADSET_TO_WORLD @ headset_pose_xyz + headset_pose_rotation = R.from_quat(headset_pose_quat).as_matrix() + headset_pose_rotation = R_HEADSET_TO_WORLD @ headset_pose_rotation @ R_HEADSET_TO_WORLD.T + + # Calculate the delta between the controller and headset positions + xr_pose_xyz_delta = xr_pose_xyz - headset_pose_xyz + + # Calculate the yaw of the headset + R_headset_to_world = R.from_matrix(headset_pose_rotation) + headset_pose_yaw = R_headset_to_world.as_euler("xyz")[2] # Extract yaw (Z-axis rotation) + inverse_yaw_rotation = R.from_euler("z", -headset_pose_yaw).as_matrix() + + # Align with headset yaw to controller position delta and rotation + xr_pose_xyz_delta_compensated = inverse_yaw_rotation @ xr_pose_xyz_delta + xr_pose_rotation_compensated = inverse_yaw_rotation @ xr_pose_rotation + + xr_pose_T = np.eye(4) + xr_pose_T[:3, :3] = xr_pose_rotation_compensated + xr_pose_T[:3, 3] = xr_pose_xyz_delta_compensated + return xr_pose_T + + def _apply_dead_zone(self, value, dead_zone): + """Apply dead zone and normalize.""" + if abs(value) < dead_zone: + return 0.0 + sign = 1 if value > 0 else -1 + # Normalize the output to be between -1 and 1 after dead zone + return sign * (abs(value) - dead_zone) / (1.0 - dead_zone) + + def _generate_finger_data(self, pico_data, hand): + """Generate finger position data.""" + fingertips = np.zeros([25, 4, 4]) + + thumb = 0 + index = 5 + middle = 10 + ring = 15 + + # Control thumb based on shoulder button state (index 4 is thumb tip) + fingertips[4 + thumb, 0, 3] = 1.0 # open thumb + if not pico_data["left_menu_button"]: + if pico_data[f"{hand}_trigger"] > 0.5 and not pico_data[f"{hand}_grip"] > 0.5: + fingertips[4 + index, 0, 3] = 1.0 # close index + elif pico_data[f"{hand}_trigger"] > 0.5 and pico_data[f"{hand}_grip"] > 0.5: + fingertips[4 + middle, 0, 3] = 1.0 # close middle + elif not pico_data[f"{hand}_trigger"] > 0.5 and pico_data[f"{hand}_grip"] > 0.5: + fingertips[4 + ring, 0, 3] = 1.0 # close ring + + return fingertips + + +if __name__ == "__main__": + # from gr00t_wbc.control.utils.debugger import wait_for_debugger + # wait_for_debugger() + + streamer = PicoStreamer() + streamer.start_streaming() + while True: + raw_data = streamer.get() + print( + f"left_wrist: {raw_data.ik_data['left_wrist']}, right_wrist: {raw_data.ik_data['right_wrist']}" + ) + time.sleep(0.1) diff --git a/gr00t_wbc/control/teleop/streamers/vive_streamer.py b/gr00t_wbc/control/teleop/streamers/vive_streamer.py new file mode 100644 index 0000000..a1d0077 --- /dev/null +++ b/gr00t_wbc/control/teleop/streamers/vive_streamer.py @@ -0,0 +1,131 @@ +import concurrent.futures +import json + +import numpy as np +from scipy.spatial.transform import Rotation as R +import zmq + +from .base_streamer import BaseStreamer, StreamerOutput + + +def get_data_with_timeout(obj, timeout=0.02): + with concurrent.futures.ThreadPoolExecutor() as executor: + future = executor.submit(obj.data_collect.request_vive_data) + try: + combined_data = future.result(timeout=timeout) + return combined_data + except concurrent.futures.TimeoutError: + print("Data request timed out.") + return None + + +def get_transformation(vive_raw_data): + """ + Turn the raw data from the Vive tracker into a transformation matrix. + """ + position = np.array( + [ + vive_raw_data["position"]["x"], + vive_raw_data["position"]["y"], + vive_raw_data["position"]["z"], + ] + ) + quat = np.array( + [ + vive_raw_data["orientation"]["x"], + vive_raw_data["orientation"]["y"], + vive_raw_data["orientation"]["z"], + vive_raw_data["orientation"]["w"], + ] + ) + T = np.identity(4) + T[:3, :3] = R.from_quat(quat).as_matrix() + T[:3, 3] = position + return T + + +class DataCollectorClient: + def __init__(self, vive_tracker_address): + # Create a ZeroMQ context + self.context = zmq.Context() + + # Create a REQ socket to request data from the server + self.vive_socket = self.context.socket(zmq.REQ) + self.vive_socket.connect(vive_tracker_address) # Connect to the server address + self.latest_data = None + + def request_vive_data(self): + """Request combined data for both left and right wrists.""" + try: + # Send a request to the server asking for both left and right Vive tracker data + self.vive_socket.send_string("get_vive_data") + + # Receive the response from the server + message = self.vive_socket.recv_string() + + # Parse the received JSON string into a Python dictionary + data = json.loads(message) + + # print(f"Received combined tracker data:", data) + return data + + except zmq.ZMQError as e: + print(f"ZMQ Error while requesting Vive data: {e}") + + def stop(self): + """Stop the client and clean up resources.""" + try: + # Close the socket and terminate the context + self.vive_socket.close() # Close the socket + self.context.term() # Terminate the context + print("Client stopped successfully.") + except zmq.ZMQError as e: + print(f"Error while stopping client: {e}") + + +class ViveStreamer(BaseStreamer): + def __init__(self, ip, port=5555, fps=20, keyword=None, **kwargs): + self.ip = f"tcp://{ip}:{port}" + self.fps = fps + self.latest_data = None + self.stop_thread = False + self.update_thread = None + self.keyword = keyword + + def reset_status(self): + """Reset the cache of the streamer.""" + self.latest_data = None + + def start_streaming(self): + self.data_collect = DataCollectorClient(self.ip) + + def __del__(self): + self.stop_streaming() + + def get(self): + """Request combined data and return transformations as StreamerOutput.""" + # Initialize IK data (ik_keys) - Vive only provides pose data + ik_data = {} + + try: + # Request combined data from the server + combined_data = self.data_collect.request_vive_data() + if combined_data: + for dir in ["left", "right"]: + actual_name = f"{dir}_{self.keyword}" + if actual_name in combined_data and combined_data[actual_name] is not None: + ik_data[f"{dir}_wrist"] = get_transformation(combined_data[actual_name]) + + except zmq.ZMQError as e: + print(f"ZMQ Error while requesting Vive data: {e}") + + # Return structured output - Vive only provides IK data + return StreamerOutput( + ik_data=ik_data, + control_data={}, # No control commands from Vive + teleop_data={}, # No teleop commands from Vive + source="vive", + ) + + def stop_streaming(self): + self.data_collect.stop() diff --git a/gr00t_wbc/control/teleop/teleop_retargeting_ik.py b/gr00t_wbc/control/teleop/teleop_retargeting_ik.py new file mode 100644 index 0000000..a67f1ea --- /dev/null +++ b/gr00t_wbc/control/teleop/teleop_retargeting_ik.py @@ -0,0 +1,148 @@ +import time +from typing import List, Optional + +import numpy as np + +from gr00t_wbc.control.base.policy import Policy +from gr00t_wbc.control.robot_model.robot_model import ReducedRobotModel, RobotModel +from gr00t_wbc.control.teleop.solver.body.body_ik_solver import BodyIKSolver +from gr00t_wbc.control.teleop.solver.body.body_ik_solver_settings import BodyIKSolverSettings +from gr00t_wbc.control.teleop.solver.solver import Solver +from gr00t_wbc.control.visualization.humanoid_visualizer import RobotVisualizer + + +class TeleopRetargetingIK(Policy): + """ + Robot-agnostic teleop retargeting inverse kinematics code. + Focus only on IK processing, ignore commands. + """ + + def __init__( + self, + robot_model: RobotModel, + left_hand_ik_solver: Solver, + right_hand_ik_solver: Solver, + enable_visualization=False, + body_active_joint_groups: Optional[List[str]] = None, + body_ik_solver_settings_type: str = "default", + ): + # initialize the body + if body_active_joint_groups is not None: + self.body = ReducedRobotModel.from_active_groups(robot_model, body_active_joint_groups) + self.full_robot = self.body.full_robot + self.using_reduced_robot_model = True + else: + self.body = robot_model + self.full_robot = self.body + self.using_reduced_robot_model = False + if body_ik_solver_settings_type == "default": + body_ik_solver_settings = BodyIKSolverSettings() + else: + raise ValueError( + f"Unknown body_ik_solver_settings_type: {body_ik_solver_settings_type}" + ) + self.body_ik_solver = BodyIKSolver(body_ik_solver_settings) + + # We register the specific robot model to the robot-agnostic body IK solver class + self.body_ik_solver.register_robot(self.body) + + # Hand IK solvers are hand specific, so we pass them in the constructor + self.left_hand_ik_solver = left_hand_ik_solver + self.right_hand_ik_solver = right_hand_ik_solver + + # enable visualizer + self.enable_visualization = enable_visualization + if self.enable_visualization: + self.visualizer = RobotVisualizer(self.full_robot) + self.visualizer.visualize(self.full_robot.q_zero) + time.sleep(1) # wait for the visualizer to start + + self.in_warmup = True + self._most_recent_ik_data = None + self._most_recent_q = self.full_robot.default_body_pose.copy() + + def compute_joint_positions( + self, body_data: dict, left_hand_data: dict, right_hand_data: dict + ) -> np.ndarray: + """Process only IK-related data, return joint positions""" + if self.in_warmup: + # TODO: Warmup is not necessary if we start IK from the current robot qpos, rather than the zero qpos + for _ in range(50): + target_robot_joints = self._inverse_kinematics( + body_data, left_hand_data, right_hand_data + ) + self.in_warmup = False + else: + target_robot_joints = self._inverse_kinematics( + body_data, left_hand_data, right_hand_data + ) + + return target_robot_joints + + def _inverse_kinematics( + self, + body_target_pose, + left_hand_target_pose, + right_hand_target_pose, + ): + """ + Solve the inverse kinematics problem for the given target poses. + Args: + body_target_pose: Dictionary of link names and their corresponding target pose. + left_hand_target_pose: Dictionary with key "position" mapping to a (25, 4, 4) np.ndarray from AVP data + right_hand_target_pose: Dictionary with key "position" mapping to a (25, 4, 4) np.ndarray from AVP data + q: Initial configuration vector. + Returns: + Configuration vector that achieves the target poses. + """ + if body_target_pose: + if self.using_reduced_robot_model: + body_q = self.body.reduced_to_full_configuration( + self.body_ik_solver(body_target_pose) + ) + else: + body_q = self.body_ik_solver(body_target_pose) + else: + # If no body target pose is provided, set the body to the default pose + body_q = self.full_robot.default_body_pose.copy() + + if left_hand_target_pose is not None: + left_hand_actuated_q = self.left_hand_ik_solver(left_hand_target_pose) + body_q[self.full_robot.get_hand_actuated_joint_indices(side="left")] = ( + left_hand_actuated_q + ) + + if right_hand_target_pose is not None: + right_hand_actuated_q = self.right_hand_ik_solver(right_hand_target_pose) + body_q[self.full_robot.get_hand_actuated_joint_indices(side="right")] = ( + right_hand_actuated_q + ) + + if self.enable_visualization: + self.visualizer.visualize(np.array(body_q)) + + return body_q + + def reset(self): + """Reset the robot model and IK solvers to the initial state, and re-activate the warmup procedure.""" + self.body.reset_forward_kinematics() # self.body is the same one as self.body_ik_solver.robot + self.full_robot.reset_forward_kinematics() + self.body_ik_solver.initialize() + # If in the future, the hand IK solver has initialize method, call it + self._most_recent_ik_data = None + self._most_recent_q = self.full_robot.default_body_pose.copy() + self.in_warmup = True + + def set_goal(self, ik_data: dict): + self._most_recent_ik_data = ik_data + + def get_action(self) -> dict[str, any]: + # Process IK if active + if self._most_recent_ik_data is not None: + body_data = self._most_recent_ik_data["body_data"] + left_hand_data = self._most_recent_ik_data["left_hand_data"] + right_hand_data = self._most_recent_ik_data["right_hand_data"] + target_joints = self.compute_joint_positions(body_data, left_hand_data, right_hand_data) + self._most_recent_q = target_joints + + return self._most_recent_q[self.full_robot.get_joint_group_indices("upper_body")] diff --git a/gr00t_wbc/control/teleop/teleop_streamer.py b/gr00t_wbc/control/teleop/teleop_streamer.py new file mode 100644 index 0000000..6d0449d --- /dev/null +++ b/gr00t_wbc/control/teleop/teleop_streamer.py @@ -0,0 +1,238 @@ +from math import floor +import pickle +from typing import Optional + +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.teleop.pre_processor.fingers.fingers import FingersPreProcessor +from gr00t_wbc.control.teleop.pre_processor.wrists.wrists import WristsPreProcessor +from gr00t_wbc.control.teleop.streamers.base_streamer import StreamerOutput + + +class TeleopStreamer: + def __init__( + self, + robot_model: RobotModel, + body_control_device: Optional[str] = None, + hand_control_device: Optional[str] = None, + enable_real_device=True, + body_streamer_ip="", + body_streamer_keyword="", + replay_data_path: Optional[str] = None, + replay_speed: float = 1.0, + ): + # initialize the body + self.body = robot_model + + self.body_control_device = body_control_device + self.hand_control_device = hand_control_device + self.body_streamer_ip = body_streamer_ip + self.body_streamer_keyword = body_streamer_keyword + self.replay_speed = replay_speed + + # enable real robot and devices + self.enable_real_device = enable_real_device + if self.enable_real_device: + if body_control_device == "vive": + from gr00t_wbc.control.teleop.streamers.vive_streamer import ViveStreamer + + self.body_streamer = ViveStreamer( + ip=self.body_streamer_ip, keyword=self.body_streamer_keyword + ) + self.body_streamer.start_streaming() + elif body_control_device == "iphone": + from gr00t_wbc.control.teleop.streamers.iphone_streamer import IphoneStreamer + + self.body_streamer = IphoneStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "leapmotion": + from gr00t_wbc.control.teleop.streamers.leapmotion_streamer import LeapMotionStreamer + + self.body_streamer = LeapMotionStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "joycon": + from gr00t_wbc.control.teleop.streamers.joycon_streamer import JoyconStreamer + + self.body_streamer = JoyconStreamer() + self.body_streamer.start_streaming() + + elif body_control_device == "pico": + from gr00t_wbc.control.teleop.streamers.pico_streamer import PicoStreamer + + self.body_streamer = PicoStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "dummy": + from gr00t_wbc.control.teleop.streamers.dummy_streamer import DummyStreamer + + self.body_streamer = DummyStreamer() + self.body_streamer.start_streaming() + else: + self.body_streamer = None + + if hand_control_device and hand_control_device != body_control_device: + if hand_control_device == "manus": + from gr00t_wbc.control.teleop.streamers.manus_streamer import ManusStreamer + + self.hand_streamer = ManusStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "joycon": + from gr00t_wbc.control.teleop.streamers.joycon_streamer import JoyconStreamer + + self.hand_streamer = JoyconStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "iphone": + from gr00t_wbc.control.teleop.streamers.iphone_streamer import IphoneStreamer + + self.hand_streamer = IphoneStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "pico": + from gr00t_wbc.control.teleop.streamers.pico_streamer import PicoStreamer + + self.hand_streamer = PicoStreamer() + self.hand_streamer.start_streaming() + else: + self.hand_streamer = None + else: + self.hand_streamer = None + else: + self.body_streamer = None + self.hand_streamer = None + + self.raw_replay_data = None + self.replay_calibration_data = None + self.replay_mode = False + if replay_data_path and not self.enable_real_device: + with open(replay_data_path, "rb") as f: + data_ = pickle.load(f) + self.raw_replay_data = data_["replay_data"] + self.replay_calibration_data = data_["calibration_data"] + print("Found teleop replay data in file: ", replay_data_path) + self.replay_idx = 0 + self.replay_mode = True + + # initialize pre_processors + self.body_control_device = body_control_device + if body_control_device or self.replay_mode: + self.body_pre_processor = WristsPreProcessor( + motion_scale=robot_model.supplemental_info.teleop_upper_body_motion_scale + ) + self.body_pre_processor.register(self.body) + else: + self.body_pre_processor = None + + # initialize hand pre-processors and post-processors + self.hand_control_device = hand_control_device + if hand_control_device or self.replay_mode: + self.left_hand_pre_processor = FingersPreProcessor(side="left") + self.right_hand_pre_processor = FingersPreProcessor(side="right") + + else: + self.left_hand_pre_processor = None + self.right_hand_pre_processor = None + + self.is_calibrated = False + + def _get_replay_data(self) -> StreamerOutput: + streamer_data = StreamerOutput() + + if self.replay_idx < len(self.raw_replay_data): + streamer_data.ik_data.update( + self.raw_replay_data[floor(self.replay_idx / self.replay_speed)] + ) + self.replay_idx += 1 + + return streamer_data + + def _get_live_data(self) -> StreamerOutput: + """Get structured data instead of raw dict""" + if self.body_streamer: + streamer_data = self.body_streamer.get() + else: + streamer_data = StreamerOutput() + + if self.hand_streamer and self.hand_streamer != self.body_streamer: + hand_data = self.hand_streamer.get() + + # Merge hand data into body data (hand data takes precedence) + streamer_data.ik_data.update(hand_data.ik_data) + streamer_data.control_data.update(hand_data.control_data) + streamer_data.teleop_data.update(hand_data.teleop_data) + streamer_data.data_collection_data.update(hand_data.data_collection_data) + + return streamer_data + + def get_streamer_data(self) -> StreamerOutput: + if self.enable_real_device: + streamer_data = self._get_live_data() + elif self.replay_mode: + streamer_data = self._get_replay_data() + else: + streamer_data = StreamerOutput() + + if self.is_calibrated and streamer_data.ik_data: + body_data, left_hand_data, right_hand_data = self.pre_process(streamer_data.ik_data) + streamer_data.ik_data = { + "body_data": body_data, + "left_hand_data": left_hand_data, + "right_hand_data": right_hand_data, + } + elif not self.is_calibrated: + streamer_data.ik_data = {} + + return streamer_data + + def calibrate(self): + """Calibrate the pre-processors using only IK data.""" + if self.replay_mode: + ik_data = self.replay_calibration_data + else: + streamer_data = self._get_live_data() + ik_data = streamer_data.ik_data + + if self.body_pre_processor: + self.body_pre_processor.calibrate(ik_data, self.body_control_device) + if self.left_hand_pre_processor: + self.left_hand_pre_processor.calibrate(ik_data, self.hand_control_device) + if self.right_hand_pre_processor: + self.right_hand_pre_processor.calibrate(ik_data, self.hand_control_device) + + self.is_calibrated = True + + def pre_process(self, raw_data): + """Pre-process the raw data.""" + assert ( + self.body_pre_processor or self.left_hand_pre_processor or self.right_hand_pre_processor + ), "Pre-processors are not initialized." + + # Check if finger data is present in raw_data + has_finger_data = "left_fingers" in raw_data and "right_fingers" in raw_data + + if self.body_pre_processor: + body_data = self.body_pre_processor(raw_data) + # Only process hand data if finger keys are present and preprocessors are available + if has_finger_data and self.left_hand_pre_processor and self.right_hand_pre_processor: + left_hand_data = self.left_hand_pre_processor(raw_data) + right_hand_data = self.right_hand_pre_processor(raw_data) + return body_data, left_hand_data, right_hand_data + else: + return body_data, None, None + else: # only hands + if has_finger_data and self.left_hand_pre_processor and self.right_hand_pre_processor: + left_hand_data = self.left_hand_pre_processor(raw_data) + right_hand_data = self.right_hand_pre_processor(raw_data) + return None, left_hand_data, right_hand_data + else: + # No finger data available, return None for hand data + return None, None, None + + def reset(self): + if self.body_streamer is not None: + self.body_streamer.reset_status() + if self.hand_streamer is not None: + self.hand_streamer.reset_status() + + def stop_streaming(self): + if self.body_streamer: + self.body_streamer.stop_streaming() + # Only stop hand_streamer if it's a different instance than body_streamer + if self.hand_streamer and self.hand_streamer is not self.body_streamer: + self.hand_streamer.stop_streaming() diff --git a/gr00t_wbc/control/utils/__init__.py b/gr00t_wbc/control/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/control/utils/cv_bridge.py b/gr00t_wbc/control/utils/cv_bridge.py new file mode 100644 index 0000000..c9e7ade --- /dev/null +++ b/gr00t_wbc/control/utils/cv_bridge.py @@ -0,0 +1,396 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# Copyright (c) 2016, Tal Regev. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys + +import cv2 +import sensor_msgs.msg + + +def CV_MAT_CNWrap(flags): + return (((flags) & ((63) << 3)) >> 3) + 1 + + +def CV_MAT_DEPTHWrap(flags): + return (flags) & 7 + + +_CV_CONVERSIONS = { + ("mono8", "rgb8"): cv2.COLOR_GRAY2RGB, + ("mono8", "bgr8"): cv2.COLOR_GRAY2BGR, + ("mono8", "rgba8"): cv2.COLOR_GRAY2RGBA, + ("mono8", "bgra8"): cv2.COLOR_GRAY2BGRA, + ("rgb8", "mono8"): cv2.COLOR_RGB2GRAY, + ("rgb8", "bgr8"): cv2.COLOR_RGB2BGR, + ("rgb8", "rgba8"): cv2.COLOR_RGB2RGBA, + ("rgb8", "bgra8"): cv2.COLOR_RGB2BGRA, + ("bgr8", "mono8"): cv2.COLOR_BGR2GRAY, + ("bgr8", "rgb8"): cv2.COLOR_BGR2RGB, + ("bgr8", "rgba8"): cv2.COLOR_BGR2RGBA, + ("bgr8", "bgra8"): cv2.COLOR_BGR2BGRA, + ("rgba8", "mono8"): cv2.COLOR_RGBA2GRAY, + ("rgba8", "rgb8"): cv2.COLOR_RGBA2RGB, + ("rgba8", "bgr8"): cv2.COLOR_RGBA2BGR, + ("rgba8", "bgra8"): cv2.COLOR_RGBA2BGRA, + ("bgra8", "mono8"): cv2.COLOR_BGRA2GRAY, + ("bgra8", "rgb8"): cv2.COLOR_BGRA2RGB, + ("bgra8", "bgr8"): cv2.COLOR_BGRA2BGR, + ("bgra8", "rgba8"): cv2.COLOR_BGRA2RGBA, + ("yuv422", "mono8"): cv2.COLOR_YUV2GRAY_UYVY, + ("yuv422", "rgb8"): cv2.COLOR_YUV2RGB_UYVY, + ("yuv422", "bgr8"): cv2.COLOR_YUV2BGR_UYVY, + ("yuv422", "rgba8"): cv2.COLOR_YUV2RGBA_UYVY, + ("yuv422", "bgra8"): cv2.COLOR_YUV2BGRA_UYVY, + ("bayer_rggb8", "mono8"): cv2.COLOR_BayerBG2GRAY, + ("bayer_rggb8", "rgb8"): cv2.COLOR_BayerBG2RGB, + ("bayer_rggb8", "bgr8"): cv2.COLOR_BayerBG2BGR, + ("bayer_bggr8", "mono8"): cv2.COLOR_BayerRG2GRAY, + ("bayer_bggr8", "rgb8"): cv2.COLOR_BayerRG2RGB, + ("bayer_bggr8", "bgr8"): cv2.COLOR_BayerRG2BGR, + ("bayer_gbrg8", "mono8"): cv2.COLOR_BayerGR2GRAY, + ("bayer_gbrg8", "rgb8"): cv2.COLOR_BayerGR2RGB, + ("bayer_gbrg8", "bgr8"): cv2.COLOR_BayerGR2BGR, + ("bayer_grbg", "mono8"): cv2.COLOR_BayerGB2GRAY, + ("bayer_grbg", "rgb8"): cv2.COLOR_BayerGB2RGB, + ("bayer_grbg", "bgr8"): cv2.COLOR_BayerGB2BGR, +} + +_CV_TYPES = { + "rgb8": cv2.CV_8UC3, + "rgba8": cv2.CV_8UC4, + "rgb16": cv2.CV_16UC3, + "rgba16": cv2.CV_16UC4, + "bgr8": cv2.CV_8UC3, + "bgra8": cv2.CV_8UC4, + "bgr16": cv2.CV_16UC3, + "bgra16": cv2.CV_16UC4, + "mono8": cv2.CV_8UC1, + "mono16": cv2.CV_16UC1, + "8UC1": cv2.CV_8UC1, + "8UC2": cv2.CV_8UC2, + "8UC3": cv2.CV_8UC3, + "8UC4": cv2.CV_8UC4, + "8SC1": cv2.CV_8SC1, + "8SC2": cv2.CV_8SC2, + "8SC3": cv2.CV_8SC3, + "8SC4": cv2.CV_8SC4, + "16UC1": cv2.CV_8UC1, + "16UC2": cv2.CV_8UC2, + "16UC3": cv2.CV_8UC3, + "16UC4": cv2.CV_8UC4, + "16SC1": cv2.CV_16SC1, + "16SC2": cv2.CV_16SC2, + "16SC3": cv2.CV_16SC3, + "16SC4": cv2.CV_16SC4, + "32SC1": cv2.CV_32SC1, + "32SC2": cv2.CV_32SC2, + "32SC3": cv2.CV_32SC3, + "32SC4": cv2.CV_32SC4, + "32FC1": cv2.CV_32FC1, + "32FC2": cv2.CV_32FC2, + "32FC3": cv2.CV_32FC3, + "32FC4": cv2.CV_32FC4, + "64FC1": cv2.CV_64FC1, + "64FC2": cv2.CV_64FC2, + "64FC3": cv2.CV_64FC3, + "64FC4": cv2.CV_64FC4, + "bayer_rggb8": cv2.CV_8UC1, + "bayer_bggr8": cv2.CV_8UC1, + "bayer_gbrg8": cv2.CV_8UC1, + "bayer_grbg8": cv2.CV_8UC1, + "bayer_rggb16": cv2.CV_16UC1, + "bayer_bggr16": cv2.CV_16UC1, + "bayer_gbrg16": cv2.CV_16UC1, + "bayer_grbg16": cv2.CV_16UC1, +} + + +def cvtColor2(img, encoding_in, encoding_out): + if encoding_in == encoding_out: + return img + + conversion = _CV_CONVERSIONS[(encoding_in, encoding_out)] + # depth conversion is not yet implemented + return cv2.cvtColor(img, conversion) + + +def getCvType(encoding): + return _CV_TYPES[encoding] + + +class CvBridgeError(TypeError): + """ + This is the error raised by :class:`cv_bridge.CvBridge` methods when they fail. + """ + + pass + + +class CvBridge(object): + """ + The CvBridge is an object that converts between OpenCV Images and ROS Image messages. + + .. doctest:: + :options: -ELLIPSIS, +NORMALIZE_WHITESPACE + + >>> import cv2 + >>> import numpy as np + >>> from cv_bridge import CvBridge + >>> br = CvBridge() + >>> dtype, n_channels = br.encoding_as_cvtype2('8UC3') + >>> im = np.ndarray(shape=(480, 640, n_channels), dtype=dtype) + >>> msg = br.cv2_to_imgmsg(im) # Convert the image to a message + >>> im2 = br.imgmsg_to_cv2(msg) # Convert the message to a new image + >>> cmprsmsg = br.cv2_to_compressed_imgmsg(im) # Convert the image to a compress message + >>> im22 = br.compressed_imgmsg_to_cv2(msg) # Convert the compress message to a new image + >>> cv2.imwrite("this_was_a_message_briefly.png", im2) + + """ + + def __init__(self): + import cv2 + + self.cvtype_to_name = {} + self.cvdepth_to_numpy_depth = { + cv2.CV_8U: "uint8", + cv2.CV_8S: "int8", + cv2.CV_16U: "uint16", + cv2.CV_16S: "int16", + cv2.CV_32S: "int32", + cv2.CV_32F: "float32", + cv2.CV_64F: "float64", + } + + for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F"]: + for c in [1, 2, 3, 4]: + nm = "%sC%d" % (t, c) + self.cvtype_to_name[getattr(cv2, "CV_%s" % nm)] = nm + + self.numpy_type_to_cvtype = { + "uint8": "8U", + "int8": "8S", + "uint16": "16U", + "int16": "16S", + "int32": "32S", + "float32": "32F", + "float64": "64F", + } + self.numpy_type_to_cvtype.update( + dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()) + ) + + def dtype_with_channels_to_cvtype2(self, dtype, n_channels): + return "%sC%d" % (self.numpy_type_to_cvtype[dtype.name], n_channels) + + def cvtype2_to_dtype_with_channels(self, cvtype): + return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype) + + def encoding_to_cvtype2(self, encoding): + try: + return getCvType(encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + def encoding_to_dtype_with_channels(self, encoding): + return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding)) + + def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding="passthrough"): + """ + Convert a sensor_msgs::CompressedImage message to an OpenCV :cpp:type:`cv::Mat`. + + :param cmprs_img_msg: A :cpp:type:`sensor_msgs::CompressedImage` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import cv2 + import numpy as np + + str_msg = cmprs_img_msg.data + buf = np.ndarray(shape=(1, len(str_msg)), dtype=np.uint8, buffer=cmprs_img_msg.data) + im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR) + + if desired_encoding == "passthrough": + return im + + try: + res = cvtColor2(im, "bgr8", desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def imgmsg_to_cv2(self, img_msg, desired_encoding="passthrough"): + """ + Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`. + + :param img_msg: A :cpp:type:`sensor_msgs::Image` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import numpy as np + + dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding) + dtype = np.dtype(dtype) + dtype = dtype.newbyteorder(">" if img_msg.is_bigendian else "<") + if n_channels == 1: + im = np.ndarray(shape=(img_msg.height, img_msg.width), dtype=dtype, buffer=img_msg.data) + else: + im = np.ndarray( + shape=(img_msg.height, img_msg.width, n_channels), dtype=dtype, buffer=img_msg.data + ) + # If the byt order is different between the message and the system. + if img_msg.is_bigendian == (sys.byteorder == "little"): + im = im.byteswap().newbyteorder() + + if desired_encoding == "passthrough": + return im + + try: + res = cvtColor2(im, img_msg.encoding, desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def cv2_to_compressed_imgmsg(self, cvim, dst_format="jpg"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::CompressedImage message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param dst_format: The format of the image data, one of the following strings: + + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat + imread(const string& filename, int flags) + * bmp, dib + * jpeg, jpg, jpe + * jp2 + * png + * pbm, pgm, ppm + * sr, ras + * tiff, tif + + :rtype: A sensor_msgs.msg.CompressedImage message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``format`` + + + This function returns a sensor_msgs::Image message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + """ + import cv2 + import numpy as np + + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError("Your input type is not a numpy array") + cmprs_img_msg = sensor_msgs.msg.CompressedImage() + cmprs_img_msg.format = dst_format + ext_format = "." + dst_format + try: + cmprs_img_msg.data = np.array(cv2.imencode(ext_format, cvim)[1]).tostring() + except RuntimeError as e: + raise CvBridgeError(e) + + return cmprs_img_msg + + def cv2_to_imgmsg(self, cvim, encoding="passthrough"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: A sensor_msgs.msg.Image message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` + + If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns a sensor_msgs::Image message on success, or raises + :exc:`cv_bridge.CvBridgeError`on failure. + """ + import numpy as np + + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError("Your input type is not a numpy array") + img_msg = sensor_msgs.msg.Image() + img_msg.height = cvim.shape[0] + img_msg.width = cvim.shape[1] + if len(cvim.shape) < 3: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1) + else: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2]) + if encoding == "passthrough": + img_msg.encoding = cv_type + else: + img_msg.encoding = encoding + # # Verify that the supplied encoding is compatible with the type of the OpenCV image + # if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type: + # raise CvBridgeError( + # "encoding specified as %s, but image has incompatible type %s" + # % (encoding, cv_type) + # ) + if cvim.dtype.byteorder == ">": + img_msg.is_bigendian = True + img_msg.data = cvim.tostring() + img_msg.step = len(img_msg.data) // img_msg.height + + return img_msg diff --git a/gr00t_wbc/control/utils/episode_state.py b/gr00t_wbc/control/utils/episode_state.py new file mode 100644 index 0000000..a808a9c --- /dev/null +++ b/gr00t_wbc/control/utils/episode_state.py @@ -0,0 +1,32 @@ +class EpisodeState: + """Episode state controller for data collection. + + Manages the state transitions for episode recording: + - IDLE: Not recording + - RECORDING: Currently recording data + - NEED_TO_SAVE: Recording stopped, waiting to save + """ + + def __init__(self): + self.RECORDING = "recording" + self.IDLE = "idle" + self.NEED_TO_SAVE = "need_to_save" + + self.state = self.IDLE + + def change_state(self): + """Cycle through states: IDLE -> RECORDING -> NEED_TO_SAVE -> IDLE.""" + if self.state == self.IDLE: + self.state = self.RECORDING + elif self.state == self.RECORDING: + self.state = self.NEED_TO_SAVE + elif self.state == self.NEED_TO_SAVE: + self.state = self.IDLE + + def reset_state(self): + """Reset to IDLE state.""" + self.state = self.IDLE + + def get_state(self): + """Get current state.""" + return self.state diff --git a/gr00t_wbc/control/utils/gear_wbc_utils.py b/gr00t_wbc/control/utils/gear_wbc_utils.py new file mode 100644 index 0000000..bf036ce --- /dev/null +++ b/gr00t_wbc/control/utils/gear_wbc_utils.py @@ -0,0 +1,100 @@ +import os + +import numpy as np +import yaml + + +def load_config(config_path): + """Load and process the YAML configuration file""" + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + # Set the path to the LEGGED_GYM_ROOT_DIR using relative path + current_file_dir = os.path.dirname(os.path.abspath(config_path)) + LEGGED_GYM_ROOT_DIR = os.path.join(current_file_dir, "..", "GearWbcRL", "legged_gym") + LEGGED_GYM_ROOT_DIR = os.path.abspath(LEGGED_GYM_ROOT_DIR) + + # Process paths with LEGGED_GYM_ROOT_DIR + for path_key in ["policy_path", "xml_path", "onnx_policy_path"]: + if path_key in config: + config[path_key] = config[path_key].format(LEGGED_GYM_ROOT_DIR=LEGGED_GYM_ROOT_DIR) + + # Convert lists to numpy arrays where needed + array_keys = ["kps", "kds", "default_angles", "cmd_scale", "cmd_init"] + for key in array_keys: + if key in config: + config[key] = np.array(config[key], dtype=np.float32) + + return config, LEGGED_GYM_ROOT_DIR + + +def pd_control(target_q, q, kp, target_dq, dq, kd): + """Calculates torques from position commands""" + return (target_q - q) * kp + (target_dq - dq) * kd + + +def quat_rotate_inverse(q, v): + """Rotate vector v by the inverse of quaternion q""" + w = q[..., 0] + x = q[..., 1] + y = q[..., 2] + z = q[..., 3] + + q_conj = np.array([w, -x, -y, -z]) + + return np.array( + [ + v[0] * (q_conj[0] ** 2 + q_conj[1] ** 2 - q_conj[2] ** 2 - q_conj[3] ** 2) + + v[1] * 2 * (q_conj[1] * q_conj[2] - q_conj[0] * q_conj[3]) + + v[2] * 2 * (q_conj[1] * q_conj[3] + q_conj[0] * q_conj[2]), + v[0] * 2 * (q_conj[1] * q_conj[2] + q_conj[0] * q_conj[3]) + + v[1] * (q_conj[0] ** 2 - q_conj[1] ** 2 + q_conj[2] ** 2 - q_conj[3] ** 2) + + v[2] * 2 * (q_conj[2] * q_conj[3] - q_conj[0] * q_conj[1]), + v[0] * 2 * (q_conj[1] * q_conj[3] - q_conj[0] * q_conj[2]) + + v[1] * 2 * (q_conj[2] * q_conj[3] + q_conj[0] * q_conj[1]) + + v[2] * (q_conj[0] ** 2 - q_conj[1] ** 2 - q_conj[2] ** 2 + q_conj[3] ** 2), + ] + ) + + +def get_gravity_orientation(quat): + """Get gravity vector in body frame""" + gravity_vec = np.array([0.0, 0.0, -1.0]) + return quat_rotate_inverse(quat, gravity_vec) + + +def compute_observation(d, config, action, cmd, height_cmd, n_joints): + """Compute the observation vector from current state""" + # Get state from MuJoCo + qj = d.qpos[7 : 7 + n_joints].copy() + dqj = d.qvel[6 : 6 + n_joints].copy() + quat = d.qpos[3:7].copy() + omega = d.qvel[3:6].copy() + + # Handle default angles padding + if len(config["default_angles"]) < n_joints: + padded_defaults = np.zeros(n_joints, dtype=np.float32) + padded_defaults[: len(config["default_angles"])] = config["default_angles"] + else: + padded_defaults = config["default_angles"][:n_joints] + + # Scale the values + qj_scaled = (qj - padded_defaults) * config["dof_pos_scale"] + dqj_scaled = dqj * config["dof_vel_scale"] + gravity_orientation = get_gravity_orientation(quat) + omega_scaled = omega * config["ang_vel_scale"] + + # Calculate single observation dimension + single_obs_dim = 3 + 1 + 3 + 3 + n_joints + n_joints + 12 + + # Create single observation + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:3] = cmd[:3] * config["cmd_scale"] + single_obs[3:4] = np.array([height_cmd]) + single_obs[4:7] = omega_scaled + single_obs[7:10] = gravity_orientation + single_obs[10 : 10 + n_joints] = qj_scaled + single_obs[10 + n_joints : 10 + 2 * n_joints] = dqj_scaled + single_obs[10 + 2 * n_joints : 10 + 2 * n_joints + 12] = action + + return single_obs, single_obs_dim diff --git a/gr00t_wbc/control/utils/img_viewer.py b/gr00t_wbc/control/utils/img_viewer.py new file mode 100644 index 0000000..9416eff --- /dev/null +++ b/gr00t_wbc/control/utils/img_viewer.py @@ -0,0 +1,91 @@ +import matplotlib.pyplot as plt +import numpy as np + + +class ImageViewer: + def __init__(self, title="Image Viewer", figsize=(8, 6), num_images=1, image_titles=None): + self.title = title + self.figsize = figsize + self.num_images = num_images + self.image_titles = image_titles or [f"Camera {i+1}" for i in range(num_images)] + + # Enable interactive mode before creating subplots + plt.ion() + + if num_images == 1: + self._fig, self._ax = plt.subplots(figsize=self.figsize) + self._ax.set_title(self.title) + self._im = self._ax.imshow(np.zeros((100, 100))) + self._ax.axis("off") + self._axes = [self._ax] + self._images = [self._im] + else: + # Calculate grid dimensions + cols = min(num_images, 3) # Max 3 columns + rows = (num_images + cols - 1) // cols + + # Adjust figure size based on number of images + fig_width = self.figsize[0] * min(cols, 2) + fig_height = self.figsize[1] * rows / 2 + + self._fig, axes = plt.subplots(rows, cols, figsize=(fig_width, fig_height)) + self._fig.suptitle(self.title) + + # Flatten axes array for easier access + if num_images == 2: + axes = [axes[0], axes[1]] + elif rows == 1: + axes = axes if cols > 1 else [axes] + else: + axes = axes.flatten() + + self._axes = [] + self._images = [] + + for i in range(num_images): + ax = axes[i] + ax.set_title(self.image_titles[i]) + im = ax.imshow(np.zeros((100, 100))) + ax.axis("off") + self._axes.append(ax) + self._images.append(im) + + # Hide unused subplots + for i in range(num_images, len(axes)): + axes[i].set_visible(False) + + # Show the figure initially to make window appear + self._fig.show() + + def show(self, image_array): + """Show a single image (backward compatibility)""" + if self.num_images == 1: + self._images[0].set_data(image_array) + else: + # If multiple viewers but single image provided, show in first viewer + self._images[0].set_data(image_array) + + # non-blocking update + self._fig.canvas.draw_idle() + self._fig.canvas.flush_events() + + def show_multiple(self, images): + """Show multiple images""" + for i, img in enumerate(images): + if i < len(self._images) and img is not None: + self._images[i].set_data(img) + # Auto-adjust aspect ratio + self._axes[i].set_aspect("auto") + + # non-blocking update + self._fig.canvas.draw_idle() + self._fig.canvas.flush_events() + + def close(self): + plt.close(self._fig) + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.close() diff --git a/gr00t_wbc/control/utils/keyboard_dispatcher.py b/gr00t_wbc/control/utils/keyboard_dispatcher.py new file mode 100644 index 0000000..41c28e8 --- /dev/null +++ b/gr00t_wbc/control/utils/keyboard_dispatcher.py @@ -0,0 +1,255 @@ +import os +import subprocess +import sys +import threading + +import rclpy +from sshkeyboard import listen_keyboard, stop_listening +from std_msgs.msg import String as RosStringMsg + +from gr00t_wbc.control.main.constants import KEYBOARD_INPUT_TOPIC + +# Global variable to store original terminal attributes +_original_terminal_attrs = None + + +def save_terminal_state(): + """Save the current terminal state.""" + global _original_terminal_attrs + try: + import termios + + fd = sys.stdin.fileno() + _original_terminal_attrs = termios.tcgetattr(fd) + except (ImportError, OSError, termios.error): + _original_terminal_attrs = None + + +def restore_terminal(): + """Restore terminal to original state.""" + global _original_terminal_attrs + try: + import termios + + if _original_terminal_attrs is not None: + fd = sys.stdin.fileno() + termios.tcsetattr(fd, termios.TCSANOW, _original_terminal_attrs) + return + except (ImportError, OSError, termios.error): + pass + + # Fallback for non-Unix systems or if termios fails + try: + if os.name == "posix": + os.system("stty sane") + except OSError: + pass + + +class ROSKeyboardDispatcher: + """ROS-based keyboard dispatcher that receives keyboard events via ROS topics.""" + + def __init__(self): + self.listeners = [] + self._active = False + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] + print("creating keyboard input subscriber...") + self.subscription = self.node.create_subscription( + RosStringMsg, KEYBOARD_INPUT_TOPIC, self._callback, 10 + ) + + def register(self, listener): + if not hasattr(listener, "handle_keyboard_button"): + raise NotImplementedError("handle_keyboard_button is not implemented") + self.listeners.append(listener) + + def start(self): + """Start the ROS keyboard dispatcher.""" + self._active = True + print("ROS keyboard dispatcher started") + + def stop(self): + """Stop the ROS keyboard dispatcher and cleanup.""" + if self._active: + self._active = False + # Clean up subscription + if hasattr(self, "subscription"): + self.node.destroy_subscription(self.subscription) + print("ROS keyboard dispatcher stopped") + + def _callback(self, msg: RosStringMsg): + if self._active: + for listener in self.listeners: + listener.handle_keyboard_button(msg.data) + + def __del__(self): + """Cleanup when object is destroyed.""" + self.stop() + + +class KeyboardDispatcher: + def __init__(self): + self.listeners = [] + self._listening_thread = None + self._stop_event = threading.Event() + self._key = None + + def register(self, listener): + # raise if handle_keyboard_button is not implemented + # TODO(YL): let listener be a Callable instead of a class + if not hasattr(listener, "handle_keyboard_button"): + raise NotImplementedError("handle_keyboard_button is not implemented") + self.listeners.append(listener) + + def handle_key(self, key): + # Check if we should stop + if self._stop_event.is_set(): + stop_listening() + return + + for listener in self.listeners: + listener.handle_keyboard_button(key) + + def start_listening(self): + try: + save_terminal_state() # Save original terminal state before listening + listen_keyboard( + on_press=self.handle_key, + delay_second_char=0.1, + delay_other_chars=0.05, + sleep=0.01, + ) + except Exception as e: + print(f"Keyboard listener stopped: {e}") + finally: + # Ensure terminal is restored even if an exception occurs + self._restore_terminal() + + def start(self): + self._listening_thread = threading.Thread(target=self.start_listening, daemon=True) + self._listening_thread.start() + + def stop(self): + """Stop the keyboard listener and restore terminal settings.""" + if self._listening_thread and self._listening_thread.is_alive(): + self._stop_event.set() + # Force stop_listening to be called + try: + stop_listening() + except Exception: + pass + # Wait a bit for the thread to finish + self._listening_thread.join(timeout=0.5) + # Restore terminal settings + self._restore_terminal() + + def _restore_terminal(self): + """Restore terminal to a sane state.""" + restore_terminal() + + def __del__(self): + """Cleanup when object is destroyed.""" + self.stop() + + +KEYBOARD_LISTENER_TOPIC_NAME = "/Gr00tKeyboardListener" + + +class KeyboardListener: + def __init__(self): + self.key = None + + def handle_keyboard_button(self, key): + self.key = key + + def pop_key(self): + key = self.key + self.key = None + return key + + +class KeyboardListenerPublisher: + def __init__(self, topic_name: str = KEYBOARD_LISTENER_TOPIC_NAME): + """ + Initialize keyboard listener for remote teleop with simplified interface. + + Args: + remote_system: RemoteSystem instance + control_channel_name: Name of the control channel + """ + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] + self.publisher = self.node.create_publisher(RosStringMsg, topic_name, 1) + + def handle_keyboard_button(self, key): + self.publisher.publish(RosStringMsg(data=key)) + + +class KeyboardListenerSubscriber: + def __init__( + self, + topic_name: str = KEYBOARD_LISTENER_TOPIC_NAME, + node_name: str = "keyboard_listener_subscriber", + ): + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + nodes = executor.get_nodes() + if nodes: + self.node = nodes[0] + self._create_node = False + else: + self.node = rclpy.create_node("KeyboardListenerSubscriber") + executor.add_node(self.node) + self._create_node = True + self.subscriber = self.node.create_subscription(RosStringMsg, topic_name, self._callback, 1) + self._data = None + + def _callback(self, msg: RosStringMsg): + self._data = msg.data + + def read_msg(self): + data = self._data + self._data = None + return data + + +class KeyboardEStop: + def __init__(self): + """Initialize KeyboardEStop with automatic tmux cleanup detection.""" + # Automatically create tmux cleanup if in deployment mode + self.cleanup_callback = self._create_tmux_cleanup_callback() + + def _create_tmux_cleanup_callback(self): + """Create a cleanup callback that kills the tmux session if running in deployment mode.""" + tmux_session = os.environ.get("GR00T_WBC_TMUX_SESSION") + + def cleanup_callback(): + if tmux_session: + print(f"Emergency stop: Killing tmux session '{tmux_session}'...") + try: + subprocess.run(["tmux", "kill-session", "-t", tmux_session], timeout=5) + print("Tmux session terminated successfully.") + except subprocess.TimeoutExpired: + print("Warning: Tmux session termination timed out, forcing kill...") + try: + subprocess.run(["tmux", "kill-session", "-t", tmux_session, "-9"]) + except Exception: + pass + except Exception as e: + print(f"Warning: Error during tmux cleanup: {e}") + # If tmux cleanup fails, fallback to immediate exit + restore_terminal() + os._exit(1) + else: + print("Emergency stop: No tmux session, exiting normally...") + sys.exit(1) + + return cleanup_callback + + def handle_keyboard_button(self, key): + if key == "`": + print("Emergency stop triggered - running cleanup...") + self.cleanup_callback() diff --git a/gr00t_wbc/control/utils/logging_utils.py b/gr00t_wbc/control/utils/logging_utils.py new file mode 100644 index 0000000..0adc48e --- /dev/null +++ b/gr00t_wbc/control/utils/logging_utils.py @@ -0,0 +1,83 @@ +""" +Simple logging utilities for teleop evaluation. +""" + +from datetime import datetime +import os +from pathlib import Path +from typing import Any, Dict + +import numpy as np + +import gr00t_wbc + + +class EvaluationLogger: + """Simple logger that writes evaluation metrics to a timestamped file.""" + + def __init__(self, log_subdir: str = "logs_teleop"): + """ + Initialize the evaluation logger. + + Args: + log_subdir: Subdirectory name under project root for logs + """ + self.log_subdir = log_subdir + self._setup_logging() + + def _setup_logging(self): + """Setup simple file logging with timestamp-based filename""" + # Get project root directory + project_root = Path(os.path.dirname(gr00t_wbc.__file__)).parent + + # Create logs directory if it doesn't exist + self.logs_dir = project_root / self.log_subdir + self.logs_dir.mkdir(exist_ok=True) + + # Create timestamp-based filename + timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + log_filename = f"eval_{timestamp}.log" + self.log_file_path = self.logs_dir / log_filename + + # Open file for writing + self.log_file = open(self.log_file_path, "w") + + def write(self, message: str): + """Write a message to the log file.""" + self.log_file.write(message + "\n") + self.log_file.flush() # Ensure immediate write + + def log_metrics(self, metrics: Dict[str, Any]): + """ + Log evaluation metrics to file - just like the original print statements. + + Args: + metrics: Dictionary of metric names and values + """ + for metric_name, value in metrics.items(): + if isinstance(value, np.ndarray): + # Convert numpy arrays to readable format + if value.size == 1: + self.write(f"{metric_name}: {value.item()}") + else: + self.write(f"{metric_name}: {value}") + else: + self.write(f"{metric_name}: {value}") + + def get_log_file_path(self) -> Path: + """Get the path to the current log file.""" + return self.log_file_path + + def print(self): + """Print out the contents of the log file.""" + if self.log_file_path.exists(): + with open(self.log_file_path, "r") as f: + content = f.read() + print(content) + else: + print(f"Log file not found: {self.log_file_path}") + + def close(self): + """Close the log file.""" + if hasattr(self, "log_file") and self.log_file: + self.log_file.close() diff --git a/gr00t_wbc/control/utils/n1_utils.py b/gr00t_wbc/control/utils/n1_utils.py new file mode 100644 index 0000000..4ceb9da --- /dev/null +++ b/gr00t_wbc/control/utils/n1_utils.py @@ -0,0 +1,226 @@ +from typing import Any, Dict, SupportsFloat, Tuple + +import gymnasium as gym +from gymnasium import spaces +import numpy as np + +from gr00t_wbc.control.main.constants import DEFAULT_BASE_HEIGHT, DEFAULT_NAV_CMD +from gr00t_wbc.control.main.teleop.configs.configs import SyncSimDataCollectionConfig +from gr00t_wbc.control.policy.wbc_policy_factory import get_wbc_policy +from gr00t_wbc.control.robot_model import RobotModel +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model + + +class WholeBodyControlWrapper(gym.Wrapper): + """Gymnasium wrapper to integrate whole-body control for locomotion/manipulation sims.""" + + def __init__(self, env, script_config): + super().__init__(env) + self.script_config = script_config + self.script_config["robot"] = env.unwrapped.robot_name + self.wbc_policy = self.setup_wbc_policy() + self._action_space = self._wbc_action_space() + + @property + def robot_model(self) -> RobotModel: + """Return the robot model from the wrapped environment.""" + return self.env.unwrapped.robot_model # type: ignore + + def reset(self, **kwargs): + obs, info = self.env.reset(**kwargs) + self.wbc_policy = self.setup_wbc_policy() + self.wbc_policy.set_observation(obs) + return obs, info + + def step(self, action: Dict[str, Any]) -> Tuple[Any, SupportsFloat, bool, bool, Dict[str, Any]]: + action_dict = concat_action(self.robot_model, action) + + wbc_goal = {} + for key in ["navigate_cmd", "base_height_command", "target_upper_body_pose"]: + if key in action_dict: + wbc_goal[key] = action_dict[key] + + self.wbc_policy.set_goal(wbc_goal) + wbc_action = self.wbc_policy.get_action() + + result = super().step(wbc_action) + self.wbc_policy.set_observation(result[0]) + return result + + def setup_wbc_policy(self): + robot_type, robot_model = get_robot_type_and_model( + self.script_config["robot"], + enable_waist_ik=self.script_config.get("enable_waist", False), + ) + config = SyncSimDataCollectionConfig.from_dict(self.script_config) + config.update( + { + "save_img_obs": False, + "ik_indicator": False, + "enable_real_device": False, + "replay_data_path": None, + } + ) + wbc_config = config.load_wbc_yaml() + wbc_config["upper_body_policy_type"] = "identity" + wbc_policy = get_wbc_policy(robot_type, robot_model, wbc_config, init_time=0.0) + self.total_dofs = len(robot_model.get_joint_group_indices("upper_body")) + wbc_policy.activate_policy() + return wbc_policy + + def _get_joint_group_size(self, group_name: str) -> int: + """Return the number of joints in a group, cached since lookup is static.""" + if not hasattr(self, "_joint_group_size_cache"): + self._joint_group_size_cache = {} + if group_name not in self._joint_group_size_cache: + self._joint_group_size_cache[group_name] = len( + self.robot_model.get_joint_group_indices(group_name) + ) + return self._joint_group_size_cache[group_name] + + def _wbc_action_space(self) -> spaces.Dict: + action_space: Dict[str, spaces.Space] = { + "action.navigate_command": spaces.Box( + low=-np.inf, high=np.inf, shape=(3,), dtype=np.float32 + ), + "action.base_height_command": spaces.Box( + low=-np.inf, high=np.inf, shape=(1,), dtype=np.float32 + ), + "action.left_hand": spaces.Box( + low=-np.inf, + high=np.inf, + shape=(self._get_joint_group_size("left_hand"),), + dtype=np.float32, + ), + "action.right_hand": spaces.Box( + low=-np.inf, + high=np.inf, + shape=(self._get_joint_group_size("right_hand"),), + dtype=np.float32, + ), + "action.left_arm": spaces.Box( + low=-np.inf, + high=np.inf, + shape=(self._get_joint_group_size("left_arm"),), + dtype=np.float32, + ), + "action.right_arm": spaces.Box( + low=-np.inf, + high=np.inf, + shape=(self._get_joint_group_size("right_arm"),), + dtype=np.float32, + ), + } + if ( + "waist" + in self.robot_model.supplemental_info.joint_groups["upper_body_no_hands"]["groups"] # type: ignore[attr-defined] + ): + action_space["action.waist"] = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(self._get_joint_group_size("waist"),), + dtype=np.float32, + ) + return spaces.Dict(action_space) + + +def concat_action(robot_model: RobotModel, goal: Dict[str, Any]) -> Dict[str, Any]: + """Combine individual joint-group targets into the upper-body action vector.""" + processed_goal = {} + for key, value in goal.items(): + processed_goal[key.replace("action.", "")] = value + + first_value = next(iter(processed_goal.values())) + action = np.zeros(first_value.shape[:-1] + (robot_model.num_dofs,)) + + action_dict = {} + action_dict["navigate_cmd"] = processed_goal.pop("navigate_command", DEFAULT_NAV_CMD) + action_dict["base_height_command"] = np.array( + processed_goal.pop("base_height_command", DEFAULT_BASE_HEIGHT) + ) + + for joint_group, value in processed_goal.items(): + indices = robot_model.get_joint_group_indices(joint_group) + action[..., indices] = value + + upper_body_indices = robot_model.get_joint_group_indices("upper_body") + action = action[..., upper_body_indices] + action_dict["target_upper_body_pose"] = action + return action_dict + + +def prepare_observation_for_eval(robot_model: RobotModel, obs: dict) -> dict: + """Add joint-group slices to an observation dict (real + sim evaluation helper).""" + assert "q" in obs, "q is not in the observation" + + whole_q = obs["q"] + assert whole_q.shape[-1] == robot_model.num_joints, "q has wrong shape" + + left_arm_q = whole_q[..., robot_model.get_joint_group_indices("left_arm")] + right_arm_q = whole_q[..., robot_model.get_joint_group_indices("right_arm")] + waist_q = whole_q[..., robot_model.get_joint_group_indices("waist")] + left_leg_q = whole_q[..., robot_model.get_joint_group_indices("left_leg")] + right_leg_q = whole_q[..., robot_model.get_joint_group_indices("right_leg")] + left_hand_q = whole_q[..., robot_model.get_joint_group_indices("left_hand")] + right_hand_q = whole_q[..., robot_model.get_joint_group_indices("right_hand")] + + obs["state.left_arm"] = left_arm_q + obs["state.right_arm"] = right_arm_q + obs["state.waist"] = waist_q + obs["state.left_leg"] = left_leg_q + obs["state.right_leg"] = right_leg_q + obs["state.left_hand"] = left_hand_q + obs["state.right_hand"] = right_hand_q + + return obs + + +def prepare_gym_space_for_eval( + robot_model: RobotModel, gym_space: gym.spaces.Dict +) -> gym.spaces.Dict: + """Extend a gym Dict space with the joint-group keys used during evaluation.""" + left_arm_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("left_arm")),), + ) + right_arm_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("right_arm")),), + ) + waist_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("waist")),), + ) + left_leg_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("left_leg")),), + ) + right_leg_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("right_leg")),), + ) + left_hand_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("left_hand")),), + ) + right_hand_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(len(robot_model.get_joint_group_indices("right_hand")),), + ) + + gym_space["state.left_arm"] = left_arm_space + gym_space["state.right_arm"] = right_arm_space + gym_space["state.waist"] = waist_space + gym_space["state.left_leg"] = left_leg_space + gym_space["state.right_leg"] = right_leg_space + gym_space["state.left_hand"] = left_hand_space + gym_space["state.right_hand"] = right_hand_space + + return gym_space diff --git a/gr00t_wbc/control/utils/network_utils.py b/gr00t_wbc/control/utils/network_utils.py new file mode 100644 index 0000000..105a5cd --- /dev/null +++ b/gr00t_wbc/control/utils/network_utils.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +""" +Simple network interface utilities +""" + +import platform +import re +import subprocess + + +def get_network_interfaces(): + """Get network interfaces with their IP addresses""" + try: + result = subprocess.run( + ["/sbin/ip", "addr", "show"], capture_output=True, text=True, check=True + ) + return _parse_ip_output(result.stdout) + except (subprocess.CalledProcessError, FileNotFoundError): + try: + result = subprocess.run(["ifconfig"], capture_output=True, text=True, check=True) + return _parse_ifconfig_output(result.stdout) + except (subprocess.CalledProcessError, FileNotFoundError): + return {} + + +def _parse_ip_output(output): + """Parse 'ip addr' command output""" + interfaces = {} + current_interface = None + + for line in output.split("\n"): + interface_match = re.match(r"^\d+:\s+(\w+):", line) + if interface_match: + current_interface = interface_match.group(1) + interfaces[current_interface] = [] + + ip_match = re.search(r"inet\s+(\d+\.\d+\.\d+\.\d+)", line) + if ip_match and current_interface: + interfaces[current_interface].append(ip_match.group(1)) + + return interfaces + + +def _parse_ifconfig_output(output): + """Parse 'ifconfig' command output""" + interfaces = {} + current_interface = None + + for line in output.split("\n"): + interface_match = re.match(r"^(\w+):", line) + if interface_match: + current_interface = interface_match.group(1) + interfaces[current_interface] = [] + + ip_match = re.search(r"inet\s+(\d+\.\d+\.\d+\.\d+)", line) + if ip_match and current_interface: + interfaces[current_interface].append(ip_match.group(1)) + + return interfaces + + +def find_interface_by_ip(target_ip): + """Find interface name for given IP address""" + interfaces = get_network_interfaces() + for interface, ip_list in interfaces.items(): + if target_ip in ip_list: + return interface + return None + + +def resolve_interface(interface: str) -> tuple[str, str]: + """ + Resolve interface parameter to actual network interface name and environment type + + Args: + interface: "sim", "real", or direct interface name or IP address + + Returns: + tuple: (interface_name, env_type) where env_type is "sim" or "real" + """ + # Check if interface is an IP address + if re.match(r"^\d+\.\d+\.\d+\.\d+$", interface): + if interface == "127.0.0.1": + return interface, "sim" + else: + return interface, "real" + + if interface == "sim": + lo_interface = find_interface_by_ip("127.0.0.1") + if lo_interface: + # macOS uses lo0 instead of lo + if platform.system() == "Darwin" and lo_interface == "lo": + return "lo0", "sim" + return lo_interface, "sim" + return ("lo0" if platform.system() == "Darwin" else "lo"), "sim" + + elif interface == "real": + interfaces = get_network_interfaces() + for iface, ip_list in interfaces.items(): + for ip in ip_list: + if ip.startswith("192.168.123."): + return iface, "real" + return interface, "real" # fallback + + else: + # Direct interface name - check if it has 127.0.0.1 to determine env_type + interfaces = get_network_interfaces() + if interface in interfaces: + for ip in interfaces[interface]: + if ip == "127.0.0.1": + return interface, "sim" + + # macOS lo interface handling + if platform.system() == "Darwin" and interface == "lo": + return "lo0", "sim" + + # Default to real for unknown interfaces + return interface, "real" + + +if __name__ == "__main__": + interfaces = get_network_interfaces() + + if not interfaces: + print("No network interfaces found") + exit(1) + + # Show all interfaces + print("Network interfaces:") + for interface, ip_list in interfaces.items(): + print(f" {interface}: {', '.join(ip_list)}") + + # Test resolve_interface function + print("\nTesting resolve_interface:") + for test_interface in ["sim", "real", "lo", "127.0.0.1"]: + interface_name, env_type = resolve_interface(test_interface) + print(f" {test_interface} -> {interface_name} ({env_type})") diff --git a/gr00t_wbc/control/utils/ros_utils.py b/gr00t_wbc/control/utils/ros_utils.py new file mode 100644 index 0000000..783cc20 --- /dev/null +++ b/gr00t_wbc/control/utils/ros_utils.py @@ -0,0 +1,201 @@ +import base64 +import signal +import threading +from typing import Optional + +import msgpack +import msgpack_numpy as mnp +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import ByteMultiArray +from std_srvs.srv import Trigger + +_signal_registered = False + + +def register_keyboard_interrupt_handler(): + """ + Register a signal handler for SIGINT (Ctrl+C) and SIGTERM that raises KeyboardInterrupt. + This ensures consistent exception handling across different termination signals. + """ + global _signal_registered + if not _signal_registered: + + def signal_handler(signum, frame): + raise KeyboardInterrupt + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + _signal_registered = True + + +class ROSManager: + """ + Manages the ROS2 node and executor. + + Usage example: + ```python + def main(): + ros_manager = ROSManager() + node = ros_manager.node + + try: + while ros_manager.ok(): + time.sleep(0.1) + except ros_manager.exceptions() as e: + print(f"ROSManager interrupted by user: {e}") + finally: + ros_manager.shutdown() + ``` + """ + + def __init__(self, node_name: str = "ros_manager"): + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node(node_name) + self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) + self.thread.start() + else: + executor = rclpy.get_global_executor() + if len(executor.get_nodes()) > 0: + self.node = executor.get_nodes()[0] + else: + self.node = rclpy.create_node(node_name) + + register_keyboard_interrupt_handler() + + @staticmethod + def ok(): + return rclpy.ok() + + @staticmethod + def shutdown(): + if rclpy.ok(): + rclpy.shutdown() + + @staticmethod + def exceptions(): + return (rclpy.exceptions.ROSInterruptException, KeyboardInterrupt) + + +class ROSMsgPublisher: + """ + Publishes any serializable dict to a topic. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self.publisher = self.node.create_publisher(ByteMultiArray, topic_name, 1) + + def publish(self, msg: dict): + payload = msgpack.packb(msg, default=mnp.encode) + payload = tuple(bytes([a]) for a in payload) + msg = ByteMultiArray() + msg.data = payload + self.publisher.publish(msg) + + +class ROSMsgSubscriber: + """ + Subscribes to any topics published by a ROSMsgPublisher. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self._msg = None + self.subscription = self.node.create_subscription( + ByteMultiArray, topic_name, self._callback, 1 + ) + + def _callback(self, msg: ByteMultiArray): + self._msg = msg + + def get_msg(self) -> Optional[dict]: + msg = self._msg + if msg is None: + return None + self._msg = None + return msgpack.unpackb(bytes([ab for a in msg.data for ab in a]), object_hook=mnp.decode) + + +class ROSImgMsgSubscriber: + """ + Subscribes to an `Image` topic and returns the image as a numpy array and timestamp. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self._msg = None + self.subscription = self.node.create_subscription(Image, topic_name, self._callback, 1) + + from gr00t_wbc.control.utils.cv_bridge import CvBridge + + self.bridge = CvBridge() + + def _callback(self, msg: Image): + self._msg = msg + + def get_image(self) -> Optional[dict]: + """ + Returns the image as a numpy array and the timestamp. + """ + + msg = self._msg + if msg is None: + return None + return { + "image": self.bridge.imgmsg_to_cv2(msg), + "timestamp": msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + } + + +class ROSServiceServer: + """ + Generic ROS2 Service server that stores and serves a config dict. + """ + + def __init__(self, service_name: str, config: dict): + ros_manager = ROSManager() + self.node = ros_manager.node + packed = msgpack.packb(config, default=mnp.encode) + self.message = base64.b64encode(packed).decode("ascii") + self.server = self.node.create_service(Trigger, service_name, self._callback) + + def _callback(self, request, response): + try: + response.success = True + response.message = self.message + print("Sending encoded message of length:", len(response.message)) + except Exception as e: + response.success = False + response.message = str(e) + return response + + +class ROSServiceClient(Node): + + def __init__(self, service_name: str, node_name: str = "service_client"): + super().__init__(node_name) + self.cli = self.create_client(Trigger, service_name) + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available, waiting again...") + self.req = Trigger.Request() + + def get_config(self): + future = self.cli.call_async(self.req) + executor = SingleThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(future, timeout_sec=1.0) + executor.remove_node(self) + executor.shutdown() + result = future.result() + if result.success: + decoded = base64.b64decode(result.message.encode("ascii")) + return msgpack.unpackb(decoded, object_hook=mnp.decode) + else: + raise RuntimeError(f"Service call failed: {result.message}") diff --git a/gr00t_wbc/control/utils/run_real_checklist.py b/gr00t_wbc/control/utils/run_real_checklist.py new file mode 100755 index 0000000..9ececbc --- /dev/null +++ b/gr00t_wbc/control/utils/run_real_checklist.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 + +import sys + + +def check_real_deployment(extra_args): + """Check if this is a real robot deployment.""" + is_real_deployment = False + + # Check if interface argument is provided and not 'lo' or 'lo0' + for i, arg in enumerate(extra_args): + if arg == "--interface": + # Get the next argument (interface value) + if i + 1 < len(extra_args): + interface_value = extra_args[i + 1] + if interface_value not in ["lo", "lo0"]: + is_real_deployment = True + print(f"Real deployment detected: interface = {interface_value}") + break + else: + print(f"Simulation deployment detected: interface = {interface_value}") + + # If no interface specified, assume simulation (default is 'lo' in deploy_g1.py) + if not is_real_deployment: + print("No interface specified - assuming simulation (default interface = lo)") + + return is_real_deployment + + +def show_deployment_checklist(): + """Show deployment checklist and get confirmation.""" + checklist_content = """═══════════════════════════════════════════════════════════════════════════════ + G1 ROBOT DEPLOYMENT CHECKLIST +═══════════════════════════════════════════════════════════════════════════════ + +⚠️ SAFETY VERIFICATION - Complete ALL checks before deployment + +PRE-DEPLOYMENT CHECKLIST: + +□ Sim2Sim Verification + Test in simulation first with interface set to 'sim' before real deployment + +□ Camera System Check + Test real camera with simulation environment before full deployment + +□ State Reading Validation + • Disable action queue + • Verify sensor readings (IMU, joints, fingers) + • Use rerun for visualization + • Contact: Dennis Da (xda@nvidia.com) for assistance + +□ Low Gain Test + • Start with low kp values (2-5x lower than normal) + • Keep kd values unchanged + +□ Clear Workspace + • Remove obstacles and avoid tables + • Ensure adequate clearance in all directions + +□ Emergency Stop Ready + Ensure access to at least one: + • Keyboard e-stop + • Joycon controller + • External power cutoff + +═══════════════════════════════════════════════════════════════════════════════ +🚨 EMERGENCY: Press ` at any time to stop all processes +📹 RECORDING: Connect a webcam to your computer to record the experiment +═══════════════════════════════════════════════════════════════════════════════ + +Usages: + +- hit ` to stop all processes +- hit Ctrl+C to stop single process +- hit Ctrl+\ to quit the tmux +""" + + print("") + print("🚨 REAL ROBOT DEPLOYMENT DETECTED 🚨") + print("") + print(checklist_content) + print("") + + # Get user confirmation + while True: + user_input = input("Continue with deployment? [Y/n]: ").strip() + + # Default to Y if empty input + if not user_input: + user_input = "Y" + + user_input_upper = user_input.upper() + + if user_input_upper in ["Y", "YES"]: + print("") + print("✅ Deployment confirmed. Proceeding with robot deployment...") + print("") + return True + elif user_input_upper in ["N", "NO"]: + print("") + print("❌ Deployment aborted by user.") + print("") + return False + else: + print( + "❌ Invalid input. Please enter 'Y' for yes, 'N' for no, or press Enter for default (Y)." + ) + + +def main(): + """Main function.""" + # Always show the checklist + if not show_deployment_checklist(): + print("Deployment cancelled.") + sys.exit(1) + + return 0 + + +if __name__ == "__main__": + main() diff --git a/gr00t_wbc/control/utils/service.py b/gr00t_wbc/control/utils/service.py new file mode 100644 index 0000000..1d510ab --- /dev/null +++ b/gr00t_wbc/control/utils/service.py @@ -0,0 +1,182 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass +from io import BytesIO +from typing import Any, Callable, Dict + +import torch +import zmq + + +class TorchSerializer: + @staticmethod + def to_bytes(data: dict) -> bytes: + buffer = BytesIO() + torch.save(data, buffer) + return buffer.getvalue() + + @staticmethod + def from_bytes(data: bytes) -> dict: + buffer = BytesIO(data) + obj = torch.load(buffer, weights_only=False) + return obj + + +@dataclass +class EndpointHandler: + handler: Callable + requires_input: bool = True + + +class BaseInferenceServer: + """ + An inference server that spin up a ZeroMQ socket and listen for incoming requests. + Can add custom endpoints by calling `register_endpoint`. + """ + + def __init__(self, host: str = "*", port: int = 5555): + self.running = True + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REP) + self.socket.bind(f"tcp://{host}:{port}") + self._endpoints: dict[str, EndpointHandler] = {} + + # Register the ping endpoint by default + self.register_endpoint("ping", self._handle_ping, requires_input=False) + self.register_endpoint("kill", self._kill_server, requires_input=False) + + def _kill_server(self): + """ + Kill the server. + """ + self.running = False + + def _handle_ping(self) -> dict: + """ + Simple ping handler that returns a success message. + """ + return {"status": "ok", "message": "Server is running"} + + def register_endpoint(self, name: str, handler: Callable, requires_input: bool = True): + """ + Register a new endpoint to the server. + + Args: + name: The name of the endpoint. + handler: The handler function that will be called when the endpoint is hit. + requires_input: Whether the handler requires input data. + """ + self._endpoints[name] = EndpointHandler(handler, requires_input) + + def run(self): + addr = self.socket.getsockopt_string(zmq.LAST_ENDPOINT) + print(f"Server is ready and listening on {addr}") + while self.running: + try: + message = self.socket.recv() + request = TorchSerializer.from_bytes(message) + endpoint = request.get("endpoint", "get_action") + + if endpoint not in self._endpoints: + raise ValueError(f"Unknown endpoint: {endpoint}") + + handler = self._endpoints[endpoint] + result = ( + handler.handler(request.get("data", {})) + if handler.requires_input + else handler.handler() + ) + self.socket.send(TorchSerializer.to_bytes(result)) + except Exception as e: + print(f"Error in server: {e}") + import traceback + + print(traceback.format_exc()) + self.socket.send(b"ERROR") + + +class BaseInferenceClient: + def __init__(self, host: str = "localhost", port: int = 5555, timeout_ms: int = 15000): + self.context = zmq.Context() + self.host = host + self.port = port + self.timeout_ms = timeout_ms + self._init_socket() + + def _init_socket(self): + """Initialize or reinitialize the socket with current settings""" + self.socket = self.context.socket(zmq.REQ) + self.socket.connect(f"tcp://{self.host}:{self.port}") + + def ping(self) -> bool: + try: + self.call_endpoint("ping", requires_input=False) + return True + except zmq.error.ZMQError: + self._init_socket() # Recreate socket for next attempt + return False + + def kill_server(self): + """ + Kill the server. + """ + self.call_endpoint("kill", requires_input=False) + + def call_endpoint( + self, endpoint: str, data: dict | None = None, requires_input: bool = True + ) -> dict: + """ + Call an endpoint on the server. + + Args: + endpoint: The name of the endpoint. + data: The input data for the endpoint. + requires_input: Whether the endpoint requires input data. + """ + request: dict = {"endpoint": endpoint} + if requires_input: + request["data"] = data + + self.socket.send(TorchSerializer.to_bytes(request)) + message = self.socket.recv() + if message == b"ERROR": + raise RuntimeError("Server error") + return TorchSerializer.from_bytes(message) + + def __del__(self): + """Cleanup resources on destruction""" + self.socket.close() + self.context.term() + + +class ExternalRobotInferenceClient(BaseInferenceClient): + """ + Client for communicating with the RealRobotServer + """ + + def set_observation(self, observation: dict[str, Any]): + self.call_endpoint("set_observation", data=observation) + + def get_action(self, time: float | None = None) -> Dict[str, Any]: + """ + Get the action from the server. + The exact definition of the observations is defined + by the policy, which contains the modalities configuration. + """ + return self.call_endpoint("get_action", data={"time": time}) + + def get_modality_config(self) -> dict[str, Any]: + return self.call_endpoint("get_modality_config") diff --git a/gr00t_wbc/control/utils/sync_sim_utils.py b/gr00t_wbc/control/utils/sync_sim_utils.py new file mode 100644 index 0000000..0048f95 --- /dev/null +++ b/gr00t_wbc/control/utils/sync_sim_utils.py @@ -0,0 +1,590 @@ +from datetime import datetime +import pathlib +import shutil +from typing import Any, Dict, Optional + +import numpy as np + +from gr00t_wbc.control.envs.robocasa.sync_env import G1SyncEnv, SyncEnv +from gr00t_wbc.control.envs.robocasa.utils.controller_utils import ( + get_body_ik_solver_settings_type, + update_robosuite_controller_configs, +) +from gr00t_wbc.control.main.constants import DEFAULT_BASE_HEIGHT, DEFAULT_NAV_CMD +from gr00t_wbc.control.main.teleop.configs.configs import SyncSimDataCollectionConfig +from gr00t_wbc.control.policy.teleop_policy import TeleopPolicy +from gr00t_wbc.control.policy.wbc_policy_factory import get_wbc_policy +from gr00t_wbc.control.robot_model.instantiation import get_robot_type_and_model +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.teleop.solver.hand.instantiation.g1_hand_ik_instantiation import ( + instantiate_g1_hand_ik_solver, +) +from gr00t_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK +from gr00t_wbc.control.utils.episode_state import EpisodeState +from gr00t_wbc.control.utils.text_to_speech import TextToSpeech +from gr00t_wbc.data.exporter import Gr00tDataExporter +from gr00t_wbc.data.utils import get_dataset_features, get_modality_config + +MAX_MUJOCO_STATE_LEN = 800 +COLLECTION_KEY = "c" +SKIP_KEY = "x" + + +class EpisodeManager: + """Manages episode state transitions, done flags, and task completion hold counts. + + This class encapsulates the logic for: + - Episode state management (IDLE -> RECORDING -> NEED_TO_SAVE) + - Done flag handling + - Task completion hold count tracking + - Data collection triggering based on manual/auto mode + - Step counting within episodes + """ + + def __init__(self, config: SyncSimDataCollectionConfig): + self.config = config + self.task_completion_hold_count = -1 + self.done = False + self.step_count = 0 + + # Initialize episode state and text-to-speech for both manual and automatic modes + self.episode_state = EpisodeState() + self.text_to_speech = TextToSpeech() + + def should_collect_data(self) -> bool: + """Determine if data should be collected at this timestep.""" + if not self.config.data_collection: + return False + + if self.config.manual_control: + # Manual mode: only collect when RECORDING or NEED_TO_SAVE + return self.episode_state.get_state() in [ + self.episode_state.RECORDING, + self.episode_state.NEED_TO_SAVE, + ] + else: + # Auto mode: collect when RECORDING or NEED_TO_SAVE + return self.episode_state.get_state() in [ + self.episode_state.RECORDING, + self.episode_state.NEED_TO_SAVE, + ] + + def increment_step(self): + """Increment the step counter.""" + self.step_count += 1 + + def reset_step_count(self): + """Reset the step counter to 0.""" + self.step_count = 0 + + def get_step_count(self) -> int: + """Get the current step count.""" + return self.step_count + + def handle_collection_trigger( + self, wbc_goal: dict, keyboard_input: str | None, step_info: dict + ): + """Handle data collection start/stop triggers. + + Args: + wbc_goal: WBC goal dictionary + keyboard_input: Keyboard input from user (can be None) + step_info: Step information from environment containing success flag + """ + self.done = False + state_changed = False + + if self.config.manual_control: + if wbc_goal.get("toggle_data_collection", False) or ( + keyboard_input and keyboard_input == COLLECTION_KEY + ): + self.episode_state.change_state() + # IDLE -> RECORDING or RECORDING -> NEED_TO_SAVE + state_changed = True + else: + # Auto mode: automatically start collecting data for new env + if self.episode_state.get_state() == self.episode_state.IDLE: + self.episode_state.change_state() # IDLE -> RECORDING + state_changed = True + self.done = step_info.get("success", False) + if ( + self.episode_state.get_state() == self.episode_state.RECORDING + and self.done + and self.task_completion_hold_count == 0 + ): + self.episode_state.change_state() # RECORDING -> NEED_TO_SAVE + state_changed = True + + # Check for CI test completion + if self.config.ci_test and self.step_count >= self._get_ci_test_steps(): + self.episode_state.change_state() # RECORDING -> NEED_TO_SAVE + state_changed = True + + if state_changed: + if self.episode_state.get_state() == self.episode_state.RECORDING: + self.text_to_speech.print_and_say("Started recording episode") + elif self.episode_state.get_state() == self.episode_state.NEED_TO_SAVE: + self.done = True + self.task_completion_hold_count = 0 + self.text_to_speech.print_and_say("Stopping recording, preparing to save") + + def check_export_and_completion(self, exporter: Gr00tDataExporter) -> bool: + """Check if episode should be exported and update completion state. + + Args: + exporter: Data exporter instance + + Returns: + bool: True if environment needs to be reset + """ + need_reset = False + + # Check if we should save the episode + if self.task_completion_hold_count == 0: + exporter.save_episode() + need_reset = True + self.task_completion_hold_count = -1 + + if self.episode_state.get_state() == self.episode_state.NEED_TO_SAVE: + self.episode_state.change_state() # NEED_TO_SAVE -> IDLE + self.text_to_speech.print_and_say("Episode saved.") + + # State machine to check for having a success for N consecutive timesteps + elif self.done: + print( + f"Task success detected! Will collect {self.config.success_hold_steps} additional steps..." + ) + print(f"currently {self.task_completion_hold_count}") + if self.task_completion_hold_count > 0: + self.task_completion_hold_count -= 1 # latched state, decrement count + print(f"Task completed! Collecting {self.task_completion_hold_count} more steps...") + else: + self.task_completion_hold_count = ( + self.config.success_hold_steps + ) # reset count on first success timestep + print( + f"Task success detected! Will collect {self.config.success_hold_steps} additional steps..." + ) + else: + self.task_completion_hold_count = -1 # null the counter if there's no success + + return need_reset + + def handle_skip( + self, wbc_goal: dict, keyboard_input: str | None, exporter: Gr00tDataExporter + ) -> bool: + """Handle episode skip/abort. + + Args: + wbc_goal: WBC goal dictionary + keyboard_input: Keyboard input from user (can be None) + exporter: Data exporter instance + + Returns: + bool: True if episode was skipped and needs reset + """ + if wbc_goal.get("toggle_data_abort", False) or ( + keyboard_input and keyboard_input == SKIP_KEY + ): + exporter.skip_and_start_new_episode() + self.episode_state.reset_state() + self.text_to_speech.print_and_say("Episode discarded, starting new episode") + return True + return False + + def _get_ci_test_steps(self) -> int: + """Get CI test steps based on CI test mode.""" + if self.config.get("ci_test_mode", "unit") == "unit": + return 50 + else: # pre_merge + return 500 + + +class CITestManager: + def __init__(self, config: SyncSimDataCollectionConfig): + self.config = config + self.ci_test_steps = 50 if config.ci_test_mode == "unit" else 500 + self.enable_tracking_check = True if config.ci_test_mode == "pre_merge" else False + self.upper_body_speed_hist = [] + self.last_q_upper_body = None + self.end_effector_tracking_errors = [] + + def check_upper_body_motion( + self, robot_model: RobotModel, wbc_action: dict, config: SyncSimDataCollectionConfig + ): + upper_body_joint_indices = robot_model.get_joint_group_indices("upper_body") + q_upper_body = wbc_action["q"][upper_body_joint_indices] + if self.last_q_upper_body is None: + self.last_q_upper_body = q_upper_body.copy() + self.upper_body_speed_hist.append( + np.abs(q_upper_body - self.last_q_upper_body).mean() * config.control_frequency + ) + self.last_q_upper_body = q_upper_body.copy() + assert q_upper_body.mean() != 0, "Upper body joints should not be zero" + + def check_end_effector_tracking( + self, teleop_cmd: dict, obs: dict, config: SyncSimDataCollectionConfig, i: int + ): + """ + Check end effector tracking error and validate thresholds. + + Args: + teleop_cmd: Teleoperation command containing target poses + obs: Environment observation containing current poses + end_effector_tracking_errors: List to store tracking errors + config: Configuration object containing robot type + i: Current step index + ci_test_steps: Number of steps for CI test + upper_body_speed_hist: History of upper body joint speeds + """ + from scipy.spatial.transform import Rotation as R + + # Get target poses from teleop command (replay data format) + target_left_wrist = teleop_cmd.get("left_wrist") + target_right_wrist = teleop_cmd.get("right_wrist") + + wrist_pose = obs.get("wrist_pose") + + # Extract left and right wrist poses from environment observation (7 for left + 7 for right) + left_pos = wrist_pose[:3] + left_quat = wrist_pose[3:7] + right_pos = wrist_pose[7:10] + right_quat = wrist_pose[10:14] + + # Convert quaternions to rotation matrices for error calculation + left_rot_matrix = R.from_quat(left_quat, scalar_first=True).as_matrix() + right_rot_matrix = R.from_quat(right_quat, scalar_first=True).as_matrix() + + # Construct 4x4 transformation matrices + actual_left_wrist = np.eye(4) + actual_left_wrist[:3, 3] = left_pos + actual_left_wrist[:3, :3] = left_rot_matrix + + actual_right_wrist = np.eye(4) + actual_right_wrist[:3, 3] = right_pos + actual_right_wrist[:3, :3] = right_rot_matrix + + # Calculate position error + left_pos_error = np.linalg.norm(target_left_wrist[:3, 3] - actual_left_wrist[:3, 3]) + right_pos_error = np.linalg.norm(target_right_wrist[:3, 3] - actual_right_wrist[:3, 3]) + + # Calculate rotation error (similar to test_teleop_retargeting_ik) + left_rot_diff = actual_left_wrist[:3, :3] @ target_left_wrist[:3, :3].T + left_rot_error = np.arccos(np.clip((np.trace(left_rot_diff) - 1) / 2, -1, 1)) + right_rot_diff = actual_right_wrist[:3, :3] @ target_right_wrist[:3, :3].T + right_rot_error = np.arccos(np.clip((np.trace(right_rot_diff) - 1) / 2, -1, 1)) + + # Store max error for this timestep + max_pos_error = max(left_pos_error, right_pos_error) + max_rot_error = max(left_rot_error, right_rot_error) + self.end_effector_tracking_errors.append((max_pos_error, max_rot_error)) + + if i >= self.ci_test_steps: + max_pos_errors = [error[0] for error in self.end_effector_tracking_errors] + max_rot_errors = [error[1] for error in self.end_effector_tracking_errors] + + max_pos_error = np.max(max_pos_errors) + max_rot_error = np.max(max_rot_errors) + + average_pos_error = np.mean(max_pos_errors) + average_rot_error = np.mean(max_rot_errors) + + # More realistic thresholds based on observed data + max_pos_threshold = 0.07 # 7cm threshold + max_rot_threshold = np.deg2rad(17) # 17 degree threshold + average_pos_threshold = 0.05 # 5cm threshold + average_rot_threshold = np.deg2rad(12) # 12 degree threshold + + print(f" Position errors - Max: {max_pos_error:.4f}") + print(f" Rotation errors - Max: {np.rad2deg(max_rot_error):.2f}°") + print(f" Average position error: {average_pos_error:.4f}") + print(f" Average rotation error: {np.rad2deg(average_rot_error):.2f}°") + + assert ( + max_pos_error < max_pos_threshold + ), "Maximum end effector position tracking error exceeds threshold" + assert ( + max_rot_error < max_rot_threshold + ), "Maximum end effector rotation tracking error exceeds threshold" + assert ( + average_pos_error < average_pos_threshold + ), "Average end effector position tracking error exceeds threshold" + assert ( + average_rot_error < average_rot_threshold + ), "Average end effector rotation tracking error exceeds threshold" + + assert ( + np.array(self.upper_body_speed_hist).mean() > 0.03 + ), "Mean upper body joint velocities should be larger. Robot might not be moving." + + print("End effector tracking validation passed.") + + # Ensure end effector tracking validation has run + if not self.end_effector_tracking_errors: + assert False, "No end effector tracking data collected during CI test" + elif len(self.end_effector_tracking_errors) < self.ci_test_steps: + assert ( + False + ), f"Only {len(self.end_effector_tracking_errors)} end effector tracking samples collected" + + +def get_features( + robot_model: RobotModel, save_img_obs: bool = False, image_obs_configs: dict[str, dict] = {} +) -> dict[str, dict]: + """Fixture providing test features dict.""" + features = get_dataset_features(robot_model) + features.update( + { + "observation.sim.seed": { + "dtype": "int32", + "shape": (1,), + }, + "observation.sim.max_mujoco_state_len": { + "dtype": "int32", + "shape": (1,), + }, + "observation.sim.mujoco_state_len": { + "dtype": "int32", + "shape": (1,), + }, + "observation.sim.mujoco_state": { + "dtype": "float64", + "shape": (MAX_MUJOCO_STATE_LEN,), + }, + "observation.sim.left_wrist": { + "dtype": "float64", + "shape": (16,), + }, + "observation.sim.right_wrist": { + "dtype": "float64", + "shape": (16,), + }, + "observation.sim.left_fingers": { + "dtype": "float64", + "shape": (400,), + }, + "observation.sim.right_fingers": { + "dtype": "float64", + "shape": (400,), + }, + "observation.sim.target_upper_body_pose": { + "dtype": "float64", + "shape": (len(robot_model.get_joint_group_indices("upper_body")),), + }, + # TODO: support different reduced robot models + } + ) + + features.pop("observation.img_state_delta") + features.pop("observation.images.ego_view") + + if save_img_obs: + for key, value in image_obs_configs.items(): + features.update( + { + f"observation.images.{key.replace('_image', '')}": { + "dtype": "video", + "shape": value["shape"], + "names": ["height", "width", "channel"], + } + } + ) + + return features + + +def generate_frame( + obs: Dict[str, Any], + wbc_action: Dict[str, Any], + seed: int, + mujoco_state: np.ndarray, + mujoco_state_len: int, + max_mujoco_state_len: int, + teleop_cmd: Dict[str, Any], + wbc_goal: Dict[str, Any], + save_img_obs: bool = False, +): + frame = { + "observation.state": np.array(obs["q"], dtype=np.float64), + "observation.eef_state": np.array(obs["wrist_pose"], dtype=np.float64), + "action": np.array(wbc_action["q"], dtype=np.float64), + "action.eef": np.array(wbc_goal["wrist_pose"], dtype=np.float64), + "teleop.navigate_command": np.array( + wbc_goal.get("navigate_cmd", DEFAULT_NAV_CMD), dtype=np.float64 + ), + "teleop.base_height_command": np.array( + teleop_cmd.get("base_height_command", [DEFAULT_BASE_HEIGHT]), dtype=np.float64 + ).reshape( + 1, + ), + "observation.sim.seed": np.array([seed], dtype=np.int32), + "observation.sim.mujoco_state_len": np.array([mujoco_state_len], dtype=np.int32), + "observation.sim.max_mujoco_state_len": np.array([max_mujoco_state_len], dtype=np.int32), + "observation.sim.mujoco_state": mujoco_state.astype(np.float64), + "observation.sim.left_wrist": teleop_cmd["left_wrist"].flatten().astype(np.float64), + "observation.sim.right_wrist": teleop_cmd["right_wrist"].flatten().astype(np.float64), + "observation.sim.left_fingers": teleop_cmd["left_fingers"]["position"] + .flatten() + .astype(np.float64), + "observation.sim.right_fingers": teleop_cmd["right_fingers"]["position"] + .flatten() + .astype(np.float64), + "observation.sim.target_upper_body_pose": wbc_goal["target_upper_body_pose"].astype( + np.float64 + ), + # "observation.sim.target_time": np.array([wbc_goal["target_time"]], dtype=np.float64), + # "observation.sim.interpolation_garbage_collection_time": np.array( + # [wbc_goal["interpolation_garbage_collection_time"]], dtype=np.float64 + # ), + } + + if save_img_obs: + for key, value in obs.items(): + if key.endswith("image"): + frame[f"observation.images.{key.replace('_image', '')}"] = value + return frame + + +def get_data_exporter( + config: SyncSimDataCollectionConfig, + obs: Dict[str, Any], + robot_model: RobotModel, + save_path: Optional[pathlib.Path] = None, +) -> Gr00tDataExporter: + if save_path is None: + save_path = pathlib.Path( + f"./outputs/{datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}-{config.robot}-sim-{config.task_name}/" + ) + + if config.remove_existing_dir: + if save_path.exists(): + shutil.rmtree(save_path) + + image_obs_configs = {} + for key, value in obs.items(): + if key.endswith("image"): + image_obs_configs[key] = { + "dtype": "uint8", + "shape": value.shape, + } + + # TODO: use standardized keys for training dataset + modality_config = get_modality_config(robot_model) + exporter = Gr00tDataExporter.create( + save_root=save_path, + fps=config.control_frequency, + features=get_features( + robot_model, save_img_obs=config.save_img_obs, image_obs_configs=image_obs_configs + ), + modality_config=modality_config, + task=config.task_name, + script_config=config, + robot_type=config.robot, + vcodec="libx264", # Use a common codec that should be available + ) + return exporter + + +def get_env(config: SyncSimDataCollectionConfig, **kwargs) -> SyncEnv: + robot_type, _ = get_robot_type_and_model(config.robot, enable_waist_ik=config.enable_waist) + print("Instantiating environment:", config.env_name, config.robot) + controller_configs = update_robosuite_controller_configs( + robot=config.robot, + wbc_version=config.wbc_version, + enable_gravity_compensation=config.enable_gravity_compensation, + ) + + kwargs.update( + { + "ik_indicator": config.ik_indicator, + "control_freq": config.control_frequency, + "renderer": config.renderer, + "controller_configs": controller_configs, + "enable_waist": config.enable_waist, + "enable_gravity_compensation": config.enable_gravity_compensation, + "gravity_compensation_joints": config.gravity_compensation_joints, + } + ) + if robot_type == "g1": + env_type = G1SyncEnv + else: + raise ValueError(f"Unsupported robot type: {robot_type}") + + env = env_type( + env_name=config.task_name, # TODO: should merge with config.env_name + robot_name=config.robot, + **kwargs, + ) + return env + + +def get_env_name(robot: str, task_name: str, enable_waist_ik: bool = False) -> str: + robot_type, _ = get_robot_type_and_model(robot, enable_waist_ik=enable_waist_ik) + env_name = f"gr00tlocomanip_{robot_type}_sim/{task_name}_{robot}_Env" + return env_name + + +def get_body_teleoped_joint_groups(robot: str) -> list[str]: + robot2body_teleoped_joint_groups = { + "G1FixedLowerBody": ["arms"], + "G1FixedBase": ["arms"], + "G1ArmsOnly": ["arms"], + "G1": ["upper_body"], + } + return robot2body_teleoped_joint_groups[robot] + + +def get_teleop_policy( + robot_type: str, + robot_model: RobotModel, + config: SyncSimDataCollectionConfig, + activate_keyboard_listener: bool = True, +) -> TeleopPolicy: + if robot_type == "g1": + left_hand_ik_solver, right_hand_ik_solver = instantiate_g1_hand_ik_solver() + else: + raise ValueError(f"Invalid robot type: {robot_type}") + + # Initializing the teleop policy will block the main process until the Leap Motion is ready. + retargeting_ik = TeleopRetargetingIK( + robot_model=robot_model, + left_hand_ik_solver=left_hand_ik_solver, + right_hand_ik_solver=right_hand_ik_solver, + enable_visualization=config.enable_visualization, + body_active_joint_groups=get_body_teleoped_joint_groups(config.robot), + body_ik_solver_settings_type=get_body_ik_solver_settings_type(config.robot), + ) + teleop_policy = TeleopPolicy( + robot_model=robot_model, + retargeting_ik=retargeting_ik, + body_control_device=config.body_control_device, + hand_control_device=config.hand_control_device, + body_streamer_ip=config.body_streamer_ip, + body_streamer_keyword=config.body_streamer_keyword, + enable_real_device=config.enable_real_device, + replay_data_path=config.replay_data_path, + replay_speed=config.replay_speed, + activate_keyboard_listener=activate_keyboard_listener, + ) + return teleop_policy + + +def get_wbc_config(config: SyncSimDataCollectionConfig): + wbc_config = config.load_wbc_yaml() + wbc_config["upper_body_policy_type"] = "identity" + return wbc_config + + +def get_policies( + config: SyncSimDataCollectionConfig, + robot_type: str, + robot_model: RobotModel, + activate_keyboard_listener: bool = True, +): + wbc_config = get_wbc_config(config) + wbc_policy = get_wbc_policy(robot_type, robot_model, wbc_config, init_time=0.0) + wbc_policy.activate_policy() + teleop_policy = get_teleop_policy(robot_type, robot_model, config, activate_keyboard_listener) + if not config.manual_control: + teleop_policy.activate_policy() + return wbc_policy, teleop_policy diff --git a/gr00t_wbc/control/utils/telemetry.py b/gr00t_wbc/control/utils/telemetry.py new file mode 100644 index 0000000..6043184 --- /dev/null +++ b/gr00t_wbc/control/utils/telemetry.py @@ -0,0 +1,129 @@ +from collections import deque +import time + + +class Telemetry: + """Handles timing and performance monitoring for different code sections.""" + + def __init__(self, window_size: int = 100): + self.window_size = window_size + self._current_timers = {} + self._last_results = {} + self._history = {} # Stores deques for history + + def start_timer(self, name: str): + """Starts a timer for a given operation.""" + self._current_timers[name] = time.perf_counter() + + def stop_timer(self, name: str) -> float: + """Stops a timer and records the duration.""" + if name not in self._current_timers: + print(f"Warning: Telemetry Timer '{name}' stopped without being started.") + return 0.0 + + start_time = self._current_timers.pop(name) + duration = time.perf_counter() - start_time + self.record_value(name, duration) + return duration + + class Timer: + """Context manager for timing operations.""" + + def __init__(self, telemetry, name: str): + self.telemetry = telemetry + self.name = name + + def __enter__(self): + self.telemetry.start_timer(self.name) + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.telemetry.stop_timer(self.name) + return False # Don't suppress exceptions + + def timer(self, name: str): + """Returns a context manager for timing an operation. + + Example usage: + with telemetry.timer("operation_name"): + # Code to time goes here + """ + return self.Timer(self, name) + + def record_value(self, name: str, value: float): + """Records a pre-calculated value (e.g., duration, count).""" + self._last_results[name] = value + if name not in self._history: + self._history[name] = deque(maxlen=self.window_size) + self._history[name].append(value) + + def get_last_timing(self) -> dict[str, float]: + """Returns the timing results from the most recent cycle and clears internal state.""" + results = self._last_results.copy() + # Don't clear _last_results here, let the logger decide when it's done + return results + + def clear_last_timing(self): + """Clears the timing results for the next iteration.""" + self._last_results.clear() + + def get_average(self, name: str) -> float | None: + """Calculates the moving average for a recorded metric.""" + if name not in self._history or not self._history[name]: + return None + return sum(self._history[name]) / len(self._history[name]) + + def get_history(self, name: str) -> deque[float] | None: + """Returns the historical data for a metric.""" + return self._history.get(name) + + def log_timing_info( + self, + context: str = "", + threshold: float = 0.001, + log_averages: bool = True, + ): + """Logs timing information based on thresholds and averages.""" + current_iteration_data = self.get_last_timing() # Get the latest data + significant_timings = {k: v for k, v in current_iteration_data.items() if v > threshold} + + should_log = bool(significant_timings) # Log if any timing exceeds threshold + + # Check averages condition + avg_data = {} + if log_averages: + for name in self._history: + avg = self.get_average(name) + if avg is not None: + avg_data[f"{name}_avg"] = avg + if avg_data: # Log if average data exists + should_log = True + + # If nothing to log, return early + if not should_log: + self.clear_last_timing() # Clear data since it wasn't logged + return + + log_lines = [f"\n{context} Timing breakdown:" if context else "\nTiming breakdown:"] + + # Log current iteration significant timings + if significant_timings: + log_lines.append(f" Current Iteration (> {threshold*1000:.1f}ms):") + for name, duration in sorted(significant_timings.items()): + log_lines.append(f" {name}: {duration * 1000:.2f}ms") + elif current_iteration_data: # Log total time even if below threshold + total_key = next((k for k in current_iteration_data if "total" in k), None) + if total_key: + log_lines.append( + f" Current Iteration Total: {current_iteration_data[total_key] * 1000:.2f}ms" + ) + + # Log averages if requested and available + if log_averages and avg_data: + log_lines.append(f" Moving Averages (last {self.window_size} iters):") + for name, avg in sorted(avg_data.items()): + log_lines.append(f" {name}: {avg * 1000:.2f}ms") + + # Print the collected log lines + print("\n".join(log_lines)) + self.clear_last_timing() # Clear data now that it has been logged/processed diff --git a/gr00t_wbc/control/utils/term_color_constants.py b/gr00t_wbc/control/utils/term_color_constants.py new file mode 100644 index 0000000..4e13ac1 --- /dev/null +++ b/gr00t_wbc/control/utils/term_color_constants.py @@ -0,0 +1,19 @@ +GREEN_BOLD = "\033[1;32m" +RED_BOLD = "\033[1;31m" +YELLOW_BOLD = "\033[1;33m" +BLUE_BOLD = "\033[1;34m" +MAGENTA_BOLD = "\033[1;35m" +CYAN_BOLD = "\033[1;36m" +WHITE_BOLD = "\033[1;37m" +GREY_BOLD = "\033[1;90m" + +GREEN = "\033[32m" +RED = "\033[31m" +YELLOW = "\033[33m" +BLUE = "\033[34m" +MAGENTA = "\033[35m" +CYAN = "\033[36m" +WHITE = "\033[37m" +GREY = "\033[90m" + +RESET = "\033[0m" diff --git a/gr00t_wbc/control/utils/text_to_speech.py b/gr00t_wbc/control/utils/text_to_speech.py new file mode 100644 index 0000000..4e076c8 --- /dev/null +++ b/gr00t_wbc/control/utils/text_to_speech.py @@ -0,0 +1,28 @@ +# text to speech +import pyttsx3 + + +class TextToSpeech: + def __init__(self, rate: int = 150, volume: float = 1.0): + try: + self.engine = pyttsx3.init(driverName="espeak") + self.engine.setProperty("rate", rate) + self.engine.setProperty("volume", volume) + except Exception as e: + print(f"[Text To Speech] Initialization failed: {e}") + self.engine = None + + def say(self, message: str): + """Speak the message if engine is available.""" + if self.engine: + try: + self.engine.say(message) + self.engine.runAndWait() + except Exception as e: + print(f"[Text To Speech] Failed to say message: {e}") + + def print_and_say(self, message: str, say: bool = True): + """Print message and optionally speak it using Text To Speech.""" + print(message) + if say and self.engine is not None: + self.say(message) diff --git a/gr00t_wbc/control/visualization/humanoid_visualizer.py b/gr00t_wbc/control/visualization/humanoid_visualizer.py new file mode 100644 index 0000000..9d770b3 --- /dev/null +++ b/gr00t_wbc/control/visualization/humanoid_visualizer.py @@ -0,0 +1,52 @@ +import time + +import meshcat_shapes +import numpy as np +from pinocchio.visualize import MeshcatVisualizer + +from gr00t_wbc.control.robot_model import RobotModel +from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model + + +class RobotVisualizer: + def __init__(self, robot: RobotModel): + self.robot = robot + self.viz = MeshcatVisualizer( + self.robot.pinocchio_wrapper.model, + self.robot.pinocchio_wrapper.collision_model, + self.robot.pinocchio_wrapper.visual_model, + ) + try: + self.viz.initViewer(open=True) + + except ImportError as err: + print("Error while initializing the viewer. It seems you should install Python meshcat") + print(err) + exit(0) + + self.viz.loadViewerModel() + self.viz.display(self.robot.q_zero) + + # Visualize frames + self.viz_frames = [self.robot.supplemental_info.root_frame_name] + for side in ["left", "right"]: + self.viz_frames.append(self.robot.supplemental_info.hand_frame_names[side]) + for frame in self.viz_frames: + meshcat_shapes.frame(self.viz.viewer[frame], opacity=1.0) + + def visualize(self, robot_state: np.ndarray): + # visualize robot state + if robot_state is not None: + self.robot.cache_forward_kinematics(robot_state, auto_clip=False) + self.viz.display(robot_state) + for frame_name in self.viz_frames: + self.viz.viewer[frame_name].set_transform(self.robot.frame_placement(frame_name).np) + + +if __name__ == "__main__": + # robot_model = instantiate_gr1_robot_model() + robot_model = instantiate_g1_robot_model() + visualizer = RobotVisualizer(robot_model) + while True: + visualizer.visualize(robot_model.q_zero) + time.sleep(0.01) diff --git a/gr00t_wbc/control/visualization/meshcat_visualizer_env.py b/gr00t_wbc/control/visualization/meshcat_visualizer_env.py new file mode 100644 index 0000000..9cedc95 --- /dev/null +++ b/gr00t_wbc/control/visualization/meshcat_visualizer_env.py @@ -0,0 +1,77 @@ +from contextlib import contextmanager +import time + +import gymnasium as gym +import numpy as np +from pinocchio.visualize import MeshcatVisualizer + +from gr00t_wbc.control.base.env import Env +from gr00t_wbc.control.robot_model import RobotModel + + +class MeshcatVisualizerEnv(Env): + def __init__(self, robot_model: RobotModel): + self.robot_model = robot_model + self.viz = MeshcatVisualizer( + self.robot_model.pinocchio_wrapper.model, + self.robot_model.pinocchio_wrapper.collision_model, + self.robot_model.pinocchio_wrapper.visual_model, + ) + try: + self.viz.initViewer(open=True) + + except ImportError as err: + print("Error while initializing the viewer. It seems you should install Python meshcat") + print(err) + exit(0) + + self.viz.loadViewerModel() + self.visualize(self.robot_model.pinocchio_wrapper.q0) + time.sleep(1.0) + + self._observation_space = gym.spaces.Dict( + { + "q": gym.spaces.Box( + low=-2 * np.pi, high=2 * np.pi, shape=(self.robot_model.num_dofs,) + ) + } + ) + self._action_space = gym.spaces.Dict( + { + "q": gym.spaces.Box( + low=-2 * np.pi, high=2 * np.pi, shape=(self.robot_model.num_dofs,) + ) + } + ) + + def visualize(self, robot_state: np.ndarray): + # visualize robot state + if robot_state is not None: + self.viz.display(robot_state) + + def observe(self): + # Dummy observation + return {"q": self.robot_model.pinocchio_wrapper.q0} + + def queue_action(self, action: dict[str, np.ndarray]): + self.visualize(action["q"]) + + def reset(self, **kwargs): + self.visualize(self.robot_model.pinocchio_wrapper.q0) + return {"q": self.robot_model.pinocchio_wrapper.q0} + + def sensors(self) -> dict[str, any]: + return {} + + def observation_space(self) -> gym.Space: + return self._observation_space + + def action_space(self) -> gym.Space: + return self._action_space + + def close(self): + return + + @contextmanager + def activate(self): + yield diff --git a/gr00t_wbc/data/constants.py b/gr00t_wbc/data/constants.py new file mode 100644 index 0000000..024865f --- /dev/null +++ b/gr00t_wbc/data/constants.py @@ -0,0 +1,5 @@ +# This will be used for both sim and real data collection +RS_VIEW_CAMERA_HEIGHT = 480 +RS_VIEW_CAMERA_WIDTH = 640 + +BUCKET_BASE_PATH = "GearRawDataLeRobotV0" diff --git a/gr00t_wbc/data/exporter.py b/gr00t_wbc/data/exporter.py new file mode 100644 index 0000000..443d627 --- /dev/null +++ b/gr00t_wbc/data/exporter.py @@ -0,0 +1,514 @@ +import copy +from dataclasses import dataclass +from functools import partial +import json +import os +from pathlib import Path +import shutil +from typing import Optional + +import datasets +from datasets import load_dataset +from datasets.utils import disable_progress_bars +from huggingface_hub.errors import RepositoryNotFoundError +from lerobot.common.datasets.lerobot_dataset import ( + LeRobotDataset, + LeRobotDatasetMetadata, + compute_episode_stats, +) +from lerobot.common.datasets.utils import ( + check_timestamps_sync, + get_episode_data_index, + validate_episode_buffer, + validate_frame, +) +import numpy as np +from PIL import Image as PILImage +import torch +from torchvision import transforms + +from gr00t_wbc.control.main.config_template import ArgsConfig +from gr00t_wbc.data.video_writer import VideoWriter + +disable_progress_bars() # Disable HuggingFace progress bars + + +@dataclass +class DataCollectionInfo: + """ + This dataclass stores additional information that is relevant to the data collection process. + """ + + lower_body_policy: Optional[str] = None + wbc_model_path: Optional[str] = None + teleoperator_username: Optional[str] = None + support_operator_username: Optional[str] = None + robot_type: Optional[str] = None + robot_id: Optional[str] = None + + def to_dict(self) -> dict: + """Convert the dataclass to a dictionary for JSON serialization.""" + return { + "lower_body_policy": self.lower_body_policy, + "wbc_model_path": self.wbc_model_path, + "teleoperator_username": self.teleoperator_username, + "support_operator_username": self.support_operator_username, + "robot_type": self.robot_type, + "robot_id": self.robot_id, + } + + @classmethod + def from_dict(cls, data: dict) -> "DataCollectionInfo": + """Create a DataCollectionInfo instance from a dictionary.""" + return cls(**data) + + +class Gr00tDatasetMetadata(LeRobotDatasetMetadata): + """ + Additional metadata on top of LeRobotDatasetMetadata: + - modality_config: Written to `meta/modality.json` + - discarded_episode_indices: List of episode indices that were discarded. Written to `meta/info.json` + """ + + MODALITY_CONFIG_REL_PATH = Path("meta/modality.json") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + with open(self.root / self.MODALITY_CONFIG_REL_PATH, "rb") as f: + self.modality_config = json.load(f) + + @classmethod + def create( + cls, + modality_config: dict, + script_config: dict, + data_collection_info: DataCollectionInfo, + *args, + **kwargs, + ): + cls.validate_modality_config(modality_config) + + # Create base metadata object using parent class + obj = super().create(*args, **kwargs) + + # we also need to initialize the discarded_episode_indices + obj.info["script_config"] = script_config + obj.info["discarded_episode_indices"] = [] + obj.info["data_collection_info"] = data_collection_info.to_dict() + with open(obj.root / "meta" / "info.json", "w") as f: + json.dump(obj.info, f, indent=4) + + obj.__class__ = cls + with open(obj.root / cls.MODALITY_CONFIG_REL_PATH, "w") as f: + json.dump(modality_config, f, indent=4) + obj.modality_config = modality_config + return obj + + @staticmethod + def validate_modality_config(modality_config: dict) -> None: + # verify if it contains all state, action, video, annotation keys + valid_keys = ["state", "action", "video", "annotation"] + if not all(key in modality_config for key in valid_keys): + raise ValueError( + f"Modality config must contain all of the following keys: {valid_keys}" + ) + + # verify that each key has a modality_config dict + for key in valid_keys: + if key not in modality_config: + raise ValueError(f"Modality config must contain a '{key}' key") + + +class Gr00tDataExporter(LeRobotDataset): + """ + A class for exporting data collected for a single session to LeRobot Dataset. + + Intended life cycle: + 1. Create a Gr00tDataExporter object + 2. Add frames using add_frame() + 3. Save the episode using save_episode() + - This will flush the episode buffer to disk + - This will also close the video writers + - Create a new video writer and ep buffer to start new episode + + If interrupted, here's the indented behavior: + - Interruption before save_episode() is called: loses the current episode + - Interruption after save_episode() is called: keeps completed episodes + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.video_writers = self.create_video_writer() + + @property + def repo_id(self): + return self.meta.repo_id + + @property + def root(self): + return self.meta.root + + @property + def local_files_only(self): + return self.meta.local_files_only + + @property + def video_keys(self): + return self.meta.video_keys + + @classmethod + def create( + cls, + save_root: str | Path, + fps: int, + features: dict, + modality_config: dict, + task: str, + script_config: ArgsConfig = ArgsConfig(), + data_collection_info: DataCollectionInfo = DataCollectionInfo(), + robot_type: str | None = None, + tolerance_s: float = 1e-4, + vcodec: str = "h264", + overwrite_existing: bool = False, + upload_bucket_path: str | None = None, + ) -> "Gr00tDataExporter": + """ + Create a Gr00tDataExporter object. + + Args: + save_root: The root directory to save the dataset. + fps: The frame rate of the dataset. + features: The features of the dataset. + modality_config: The modality config of the dataset. + task: The task performed during the data collection session. + data_collection_info: The data collection info. + If the dataset already exists, this argument will be ignored. + If data_collection_info is not provided, it will be set to an empty DataCollectionInfo object. + robot_type: The type of robot. + tolerance_s: The tolerance for the dataset. + image_writer_processes: The number of processes to use for the image writer. + image_writer_threads: The number of threads to use for the image writer. + vcodec: The codec to use for the video writer. + """ + + obj = cls.__new__(cls) + repo_id = ( + "tmp/tmp_dataset" # NOTE(fengyuanh): Not relevant since we are not pushing to the hub + ) + if overwrite_existing and (Path(save_root)).exists(): + print( + f"Found existing dataset at {save_root}", + "Cleaning up this directory since overwrite_existing is True.", + ) + shutil.rmtree(save_root) + + if (Path(save_root)).exists(): + # Try to resume from existing dataset + try: + # Load the metadata + obj.meta = Gr00tDatasetMetadata( + repo_id=repo_id, + root=save_root, + ) + + except RepositoryNotFoundError as e: + raise ValueError( + f"Failed to resume from corrupted dataset. Please manually check the dataset at {save_root}" + ) from e + else: + if not isinstance(script_config, dict): + script_config = script_config.to_dict() + obj.meta = Gr00tDatasetMetadata.create( + repo_id=repo_id, + fps=fps, + root=save_root, + # NOTE(fengyuanh): We use "robot_type" instead of this field which requires a Robot object + robot=None, + robot_type=robot_type, + features=features, + modality_config=modality_config, + script_config=script_config, + # NOTE(fengyuanh): Always use videos for exporting + use_videos=True, + data_collection_info=data_collection_info, + ) + obj.tolerance_s = tolerance_s + obj.video_backend = ( + "pyav" # NOTE(fengyuanh): Only used in training, not relevant for exporting + ) + obj.vcodec = vcodec + obj.task = task + obj.image_writer = None + + obj.episode_buffer = obj.create_episode_buffer() + + obj.episodes = None + obj.hf_dataset = obj.create_hf_dataset() + obj.image_transforms = None + obj.delta_timestamps = None + obj.delta_indices = None + obj.episode_data_index = None + obj.upload_bucket_path = upload_bucket_path + obj.video_writers = obj.create_video_writer() + return obj + + def create_video_writer(self) -> dict[str, VideoWriter]: + video_writers = {} + for key in self.meta.video_keys: + video_writers[key] = VideoWriter( + self.root + / self.meta.get_video_file_path(self.episode_buffer["episode_index"], key), + self.meta.shapes[key][1], + self.meta.shapes[key][0], + self.fps, + self.vcodec, + ) + return video_writers + + # @note (k2): This function is copied from LeRobotDataset.add_frame. + # This is done because we want to bypass lerobot's + # image_writer and use our own VideoWriter class. + def add_frame(self, frame: dict) -> None: + """ + This function only adds the frame to the episode_buffer. Videos are handled by the video_writer, + which uses a stream writer to write to disk. + """ + frame = copy.deepcopy(frame) + frame["task"] = frame.get("task", self.task) + + # Convert torch to numpy if needed + for name in frame: + if isinstance(frame[name], torch.Tensor): + frame[name] = frame[name].numpy() + + validate_frame(frame, self.features) + + if self.episode_buffer is None: + self.episode_buffer = self.create_episode_buffer() + + # Automatically add frame_index and timestamp to episode buffer + frame_index = self.episode_buffer["size"] + timestamp = frame.pop("timestamp") if "timestamp" in frame else frame_index / self.fps + self.episode_buffer["frame_index"].append(frame_index) + self.episode_buffer["timestamp"].append(timestamp) + + # Add frame features to episode_buffer + for key in frame: + if key == "task": + # Note: we associate the task in natural language to its task index during `save_episode` + self.episode_buffer["task"].append(frame["task"]) + continue + + if key not in self.features: + raise ValueError( + f"An element of the frame is not in the features. '{key}' not in '{self.features.keys()}'." + ) + + if self.features[key]["dtype"] in ["image", "video"]: + img_path = self._get_image_file_path( + episode_index=self.episode_buffer["episode_index"], + image_key=key, + frame_index=frame_index, + ) + if frame_index == 0: + img_path.parent.mkdir(parents=True, exist_ok=True) + + # @note (k2): using our own VideoWriter class, bypassing the image_writer + self.video_writers[key].add_frame(frame[key]) + self.episode_buffer[key].append(str(img_path)) + else: + self.episode_buffer[key].append(frame[key]) + + self.episode_buffer["size"] += 1 + + def stop_video_writers(self): + if not hasattr(self, "video_writers"): + raise RuntimeError( + "Can't stop video writers because they haven't been initialized. Call create() first." + ) + for key in self.video_writers: + self.video_writers[key].stop() + + def skip_and_start_new_episode( + self, + ) -> None: + """ + Skip the current episode and start a new one. + """ + self.stop_video_writers() + self.episode_buffer = self.create_episode_buffer() + self.video_writers = self.create_video_writer() + + # @note (k2): Code copied from LeRobotDataset.save_episode + # We override this function because we want to bypass lerobot's `compute_episode_stats` on video features + # since `compute_episode_stats` only works when images are written to disk. + def save_episode(self, episode_data: dict | None = None) -> None: + if not episode_data: + episode_buffer = self.episode_buffer + + validate_episode_buffer(episode_buffer, self.meta.total_episodes, self.features) + + # size and task are special cases that won't be added to hf_dataset + episode_length = episode_buffer.pop("size") + tasks = episode_buffer.pop("task") + episode_tasks = list(set(tasks)) + episode_index = episode_buffer["episode_index"] + + episode_buffer["index"] = np.arange( + self.meta.total_frames, self.meta.total_frames + episode_length + ) + episode_buffer["episode_index"] = np.full((episode_length,), episode_index) + + # Add new tasks to the tasks dictionary + for task in episode_tasks: + task_index = self.meta.get_task_index(task) + if task_index is None: + self.meta.add_task(task) + + # Given tasks in natural language, find their corresponding task indices + episode_buffer["task_index"] = np.array([self.meta.get_task_index(task) for task in tasks]) + + for key, ft in self.features.items(): + # index, episode_index, task_index are already processed above, and image and video + # are processed separately by storing image path and frame info as meta data + if key in ["index", "episode_index", "task_index"] or ft["dtype"] in ["image", "video"]: + continue + episode_buffer[key] = np.stack(episode_buffer[key]) + + self._wait_image_writer() + self._save_episode_table(episode_buffer, episode_index) + + # @note (k2): computing only non-video features stats + non_video_features = {k: v for k, v in self.features.items() if v["dtype"] not in ["video"]} + non_vid_ep_buffer = { + k: v for k, v in episode_buffer.items() if k in non_video_features.keys() + } + ep_stats = compute_episode_stats(non_vid_ep_buffer, non_video_features) + + if len(self.meta.video_keys) > 0: + video_paths = self.encode_episode_videos(episode_index) + for key in self.meta.video_keys: + episode_buffer[key] = video_paths[key] + + # `meta.save_episode` be executed after encoding the videos + self.meta.save_episode(episode_index, episode_length, episode_tasks, ep_stats) + + ep_data_index = get_episode_data_index(self.meta.episodes, [episode_index]) + ep_data_index_np = {k: t.numpy() for k, t in ep_data_index.items()} + check_timestamps_sync( + episode_buffer["timestamp"], + episode_buffer["episode_index"], + ep_data_index_np, + self.fps, + self.tolerance_s, + ) + + video_files = list(self.root.rglob("*.mp4")) + assert len(video_files) == self.num_episodes * len(self.meta.video_keys) + + parquet_files = list(self.root.rglob("*.parquet")) + assert len(parquet_files) == self.num_episodes + + # delete images + img_dir = self.root / "images" + if img_dir.is_dir(): + shutil.rmtree(self.root / "images") + + if not episode_data: # Reset the buffer and create new video writers + self.episode_buffer = self.create_episode_buffer() + self.video_writers = self.create_video_writer() + + # check if all video and parquet files exist + for key in self.meta.video_keys: + video_path = os.path.join(self.root, self.meta.get_video_file_path(episode_index, key)) + if not os.path.exists(video_path): + raise FileNotFoundError( + f"Video path: {video_path} does not exist for episode {episode_index}" + ) + + parquet_path = os.path.join(self.root, self.meta.get_data_file_path(episode_index)) + if not os.path.exists(parquet_path): + raise FileNotFoundError( + f"Parquet path: {parquet_path} does not exist for episode {episode_index}" + ) + + # @note (k2): Overriding LeRobotDataset.encode_episode_videos to use our own VideoWriter class + def encode_episode_videos(self, episode_index: int) -> dict: + video_paths = {} + for key in self.meta.video_keys: + video_paths[key] = self.video_writers[key].stop() + return video_paths + + def save_episode_as_discarded(self) -> None: + """ + Flag ongoing episode as discarded and save it to disk. Failed manipulations (grasp, manipulation) are + flagged as discarded. It will add the episode index to the discarded episode indices list in info.json. + """ + self.meta.info["discarded_episode_indices"] = self.meta.info.get( + "discarded_episode_indices", [] + ) + [self.episode_buffer["episode_index"]] + self.save_episode() + + +def hf_transform_to_torch_by_features( + features: datasets.Sequence, items_dict: dict[torch.Tensor | None] +): + """Get a transform function that convert items from Hugging Face dataset (pyarrow) + to torch tensors. Importantly, images are converted from PIL, which corresponds to + a channel last representation (h w c) of uint8 type, to a torch image representation + with channel first (c h w) of float32 type in range [0,1]. + """ + for key in items_dict: + first_item = items_dict[key][0] + if isinstance(first_item, PILImage.Image): + to_tensor = transforms.ToTensor() + items_dict[key] = [to_tensor(img) for img in items_dict[key]] + elif first_item is None: + pass + else: + if isinstance(features[key], datasets.Value): + dtype_str = features[key].dtype + elif isinstance(features[key], datasets.Sequence): + assert isinstance(features[key].feature, datasets.Value) + dtype_str = features[key].feature.dtype + else: + raise ValueError(f"Unsupported feature type for key '{key}': {features[key]}") + dtype_mapping = { + "float32": torch.float32, + "float64": torch.float64, + "int32": torch.int32, + "int64": torch.int64, + } + items_dict[key] = [ + torch.tensor(x, dtype=dtype_mapping[dtype_str]) for x in items_dict[key] + ] + return items_dict + + +# This is a subclass of LeRobotDataset that only fixes the data type when loading +# By default, LeRobotDataset will automatically convert float64 to float32 +class TypedLeRobotDataset(LeRobotDataset): + def __init__(self, load_video=True, *args, **kwargs): + super().__init__(*args, **kwargs) + if not load_video: + video_keys = [] + for key in self.meta.features.keys(): + if self.meta.features[key]["dtype"] == "video": + video_keys.append(key) + for key in video_keys: + self.meta.features.pop(key) + + def load_hf_dataset(self) -> datasets.Dataset: + """hf_dataset contains all the observations, states, actions, rewards, etc.""" + if self.episodes is None: + path = str(self.root / "data") + hf_dataset = load_dataset("parquet", data_dir=path, split="train") + else: + files = [ + str(self.root / self.meta.get_data_file_path(ep_idx)) for ep_idx in self.episodes + ] + hf_dataset = load_dataset("parquet", data_files=files, split="train") + + # TODO(aliberts): hf_dataset.set_format("torch") + hf_dataset.set_transform(partial(hf_transform_to_torch_by_features, hf_dataset.features)) + return hf_dataset diff --git a/gr00t_wbc/data/utils.py b/gr00t_wbc/data/utils.py new file mode 100644 index 0000000..f18fbe3 --- /dev/null +++ b/gr00t_wbc/data/utils.py @@ -0,0 +1,156 @@ +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + + +def get_modality_config(robot_model: RobotModel, add_stereo_camera: bool = False) -> dict: + """ + Get the modality config for the robot model. + """ + left_hand_indices = sorted(robot_model.get_joint_group_indices("left_hand")) + right_hand_indices = sorted(robot_model.get_joint_group_indices("right_hand")) + left_arm_indices = sorted(robot_model.get_joint_group_indices("left_arm")) + right_arm_indices = sorted(robot_model.get_joint_group_indices("right_arm")) + waist_indices = sorted(robot_model.get_joint_group_indices("waist")) + left_leg_indices = sorted(robot_model.get_joint_group_indices("left_leg")) + right_leg_indices = sorted(robot_model.get_joint_group_indices("right_leg")) + + modality_config = { + "state": { + "left_leg": {"start": left_leg_indices[0], "end": left_leg_indices[-1] + 1}, + "right_leg": {"start": right_leg_indices[0], "end": right_leg_indices[-1] + 1}, + "waist": {"start": waist_indices[0], "end": waist_indices[-1] + 1}, + "left_arm": {"start": left_arm_indices[0], "end": left_arm_indices[-1] + 1}, + "left_hand": {"start": left_hand_indices[0], "end": left_hand_indices[-1] + 1}, + "right_arm": {"start": right_arm_indices[0], "end": right_arm_indices[-1] + 1}, + "right_hand": {"start": right_hand_indices[0], "end": right_hand_indices[-1] + 1}, + "left_wrist_pos": {"start": 0, "end": 3, "original_key": "observation.eef_state"}, + "left_wrist_abs_quat": { + "start": 3, + "end": 7, + "original_key": "observation.eef_state", + "rotation_type": "quaternion", + }, + "right_wrist_pos": {"start": 7, "end": 10, "original_key": "observation.eef_state"}, + "right_wrist_abs_quat": { + "start": 10, + "end": 14, + "original_key": "observation.eef_state", + "rotation_type": "quaternion", + }, + }, + "action": { + "left_leg": {"start": left_leg_indices[0], "end": left_leg_indices[-1] + 1}, + "right_leg": {"start": right_leg_indices[0], "end": right_leg_indices[-1] + 1}, + "waist": {"start": waist_indices[0], "end": waist_indices[-1] + 1}, + "left_arm": {"start": left_arm_indices[0], "end": left_arm_indices[-1] + 1}, + "left_hand": {"start": left_hand_indices[0], "end": left_hand_indices[-1] + 1}, + "right_arm": {"start": right_arm_indices[0], "end": right_arm_indices[-1] + 1}, + "right_hand": {"start": right_hand_indices[0], "end": right_hand_indices[-1] + 1}, + "left_wrist_pos": {"start": 0, "end": 3, "original_key": "action.eef"}, + "left_wrist_abs_quat": { + "start": 3, + "end": 7, + "original_key": "action.eef", + "rotation_type": "quaternion", + }, + "right_wrist_pos": {"start": 7, "end": 10, "original_key": "action.eef"}, + "right_wrist_abs_quat": { + "start": 10, + "end": 14, + "original_key": "action.eef", + "rotation_type": "quaternion", + }, + "base_height_command": { + "start": 0, + "end": 1, + "original_key": "teleop.base_height_command", + }, + "navigate_command": {"start": 0, "end": 3, "original_key": "teleop.navigate_command"}, + }, + "video": {"ego_view": {"original_key": "observation.images.ego_view"}}, + "annotation": {"human.task_description": {"original_key": "task_index"}}, + } + if add_stereo_camera: + modality_config["video"].update( + { + "ego_view_left_mono": {"original_key": "observation.images.ego_view_left_mono"}, + "ego_view_right_mono": {"original_key": "observation.images.ego_view_right_mono"}, + } + ) + + return modality_config + + +def get_dataset_features(robot_model: RobotModel, add_stereo_camera: bool = False) -> dict: + """ + Get the dataset features for the robot model. + """ + dataset_features = { + "observation.images.ego_view": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + "observation.state": { + "dtype": "float64", + "shape": (robot_model.num_joints,), + "names": robot_model.joint_names, + }, + "observation.eef_state": { + "dtype": "float64", + "shape": (14,), + "names": [ + "left_wrist_pos", + "left_wrist_abs_quat", + "right_wrist_pos", + "right_wrist_abs_quat", + ], + }, + "action": { + "dtype": "float64", + "shape": (robot_model.num_joints,), + "names": robot_model.joint_names, + }, + "action.eef": { + "dtype": "float64", + "shape": (14,), + "names": [ + "left_wrist_pos", + "left_wrist_abs_quat", + "right_wrist_pos", + "right_wrist_abs_quat", + ], + }, + "observation.img_state_delta": { + "dtype": "float32", + "shape": (1,), + "names": "img_state_delta", + }, + "teleop.navigate_command": { + "dtype": "float64", + "shape": (3,), + "names": ["lin_vel_x", "lin_vel_y", "ang_vel_z"], + }, + "teleop.base_height_command": { + "dtype": "float64", + "shape": (1,), + "names": "base_height_command", + }, + } + if add_stereo_camera: + dataset_features.update( + { + "observation.images.ego_view_left_mono": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + "observation.images.ego_view_right_mono": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + } + ) + + return dataset_features diff --git a/gr00t_wbc/data/video_writer.py b/gr00t_wbc/data/video_writer.py new file mode 100644 index 0000000..a171c4e --- /dev/null +++ b/gr00t_wbc/data/video_writer.py @@ -0,0 +1,102 @@ +import os +import queue +import sys +import threading +import time + +import av +import numpy as np + + +class VideoWriter: + def __init__( + self, + output_path: str, + width: int, + height: int, + fps: float, + codec: str = "h264", + buffer_size: int = 50, + ): + self.output_path = output_path + self._first_frame = True # Track first frame to suppress x264 info output + + # Create output directory if it doesn't exist + output_dir = os.path.dirname(output_path) + if output_dir and not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + self.queue = queue.Queue(maxsize=buffer_size) + self.container = av.open(output_path, mode="w") + self.stream = self.container.add_stream(codec, rate=fps) + self.stream.width = width + self.stream.height = height + thread = threading.Thread(target=self._writer_worker, daemon=True) + thread.start() + + def _assert_dimensions(self, frame: np.ndarray) -> None: + assert ( + frame.shape[1] == self.stream.width and frame.shape[0] == self.stream.height + ), f"""Incorrect frame dimensions. Input dimensions: {frame.shape[1]}x{frame.shape[0]}. + Expected dimensions: {self.stream.width}x{self.stream.height}""" + + def add_frame(self, frame: np.ndarray) -> None: + self._assert_dimensions(frame) + self.queue.put(frame) + + def _writer_worker(self) -> None: + while True: + frame = self.queue.get() + if frame is None: + continue + self._assert_dimensions(frame) + frame = av.VideoFrame.from_ndarray(frame, format="rgb24") + + # Suppress stderr for first frame encoding (x264 prints info then) + if self._first_frame: + stderr_fd = sys.stderr.fileno() + old_stderr = os.dup(stderr_fd) + devnull = os.open(os.devnull, os.O_WRONLY) + os.dup2(devnull, stderr_fd) + try: + packets = self.stream.encode(frame) + for packet in packets: + self.container.mux(packet) + finally: + os.dup2(old_stderr, stderr_fd) + os.close(old_stderr) + os.close(devnull) + self._first_frame = False + else: + packets = self.stream.encode(frame) + for packet in packets: + self.container.mux(packet) + + def _flush_stream(self) -> None: + packets = self.stream.encode() + for packet in packets: + self.container.mux(packet) + + def stop(self) -> str: + """ + Blocking call. Waits until all the frames in the queue have been written to the file + and the video writer has been closed. + """ + if not self.queue.empty(): + print("Waiting for video writer queue to empty...") + while not self.queue.empty(): + time.sleep(0.1) + + print("Video writer queue is empty, flushing stream...") + self._flush_stream() + self.container.close() + return self.output_path + + def cancel(self) -> None: + """Immediately stops writing and deletes the output file""" + if os.path.exists(self.output_path): + os.remove(self.output_path) + self.container.close() + + def __del__(self) -> None: + self.container.close() diff --git a/gr00t_wbc/data/viz/rerun_viz.py b/gr00t_wbc/data/viz/rerun_viz.py new file mode 100644 index 0000000..db07f2c --- /dev/null +++ b/gr00t_wbc/data/viz/rerun_viz.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 +"""Rerun visualization utilities for plotting data and images.""" + +from __future__ import annotations + +import argparse +import time +from typing import Dict, List, Optional + +import cv2 +import numpy as np +import rerun as rr # pip install rerun-sdk +import rerun.blueprint as rrb + + +class RerunViz: + """Class for visualizing data using Rerun.""" + + def __init__( + self, + image_keys: List[str], + tensor_keys: List[str], + app_name: str = "rerun_visualization", + memory_limit: str = "1GB", + window_size: float = 5.0, + port: int = 9876, + in_docker: bool = False, + ): + """Initialize the RerunViz class. + Args: + app_name: Name of the Rerun application + memory_limit: Memory limit for Rerun + window_size: Size of the time window in seconds + image_keys: List of image keys to plot + tensor_keys: List of tensor keys to plot + in_docker: Whether running inside Docker container. If in docker, + forward data to outside of the container to be rendered. + Use `rerun --port 9876` to visualize. Expecting rerun-cli 0.22.1 outside of docker. + Tested with rerun-sdk 0.21.0 inside docker. + """ + self.app_name = app_name + self.memory_limit = memory_limit + self.window_size = window_size + self.tensor_keys = tensor_keys + self.image_keys = image_keys + self.port = port + self.in_docker = in_docker + # Initialize Rerun + self._initialize_rerun() + + def _initialize_rerun(self): + """Initialize Rerun and set up the blueprint.""" + rr.init(self.app_name) + if not self.in_docker: + # support for web visualization + rr.spawn(memory_limit=self.memory_limit, port=self.port, connect=True) + else: + # forward data to outside of the docker container + rr.connect(f"127.0.0.1:{self.port}") + self._create_blueprint() + + def _create_blueprint(self): + # Create a grid of plots + contents = [] + + # Add time series plots + for tensor_key in self.tensor_keys: + contents.append( + rrb.TimeSeriesView( + origin=tensor_key, + time_ranges=[ + rrb.VisibleTimeRange( + "time", + start=rrb.TimeRangeBoundary.cursor_relative(seconds=-self.window_size), + end=rrb.TimeRangeBoundary.cursor_relative(), + ) + ], + ) + ) + + # Add image views + for image_key in self.image_keys: + contents.append(rrb.Spatial2DView(origin=image_key, name=image_key)) + + # Send the blueprint with collapsed panels to hide side/bottom bars + rr.send_blueprint(rrb.Blueprint(rrb.Grid(contents=contents), collapse_panels=True)) + + def set_rerun_keys(self, image_keys: List[str], tensor_keys: List[str]): + """Set the Rerun keys.""" + self.image_keys = image_keys + self.tensor_keys = tensor_keys + self._create_blueprint() + + def plot_images(self, images: Dict[str, np.ndarray], timestamp: Optional[float] = None): + """Plot image data. + + Args: + images: Dictionary mapping image names to image data + timestamp: Timestamp for the data (if None, uses current time) + """ + if timestamp is None: + timestamp = time.time() + + rr.set_time_seconds("time", timestamp) + + for key, image in images.items(): + if image is None: + continue + + if "depth" in key: + # Color jet + depth_colormap = cv2.applyColorMap( + cv2.convertScaleAbs(image, alpha=0.03), cv2.COLORMAP_JET + ) + rr.log(f"{key}", rr.Image(depth_colormap)) + else: + # Convert to RGB + # image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + rr.log(f"{key}", rr.Image(image)) + + def plot_tensors( + self, data: Optional[Dict[str, np.ndarray]] = None, timestamp: Optional[float] = None + ): + """Plot tensor data. + + Args: + data: Dictionary mapping keys to tensor values + timestamp: Timestamp for the data (if None, uses current time) + """ + if timestamp is None: + timestamp = time.time() + + rr.set_time_seconds("time", timestamp) + + # If no data provided, use random walk generators + for tensor_key in self.tensor_keys: + for i in range(data[tensor_key].shape[0]): + rr.log(f"{tensor_key}/{i}", rr.Scalar(data[tensor_key][i])) + + def close(self): + """Close the RerunViz instance.""" + rr.rerun_shutdown() + + +if __name__ == "__main__": + """Main function to demonstrate the RerunViz class.""" + parser = argparse.ArgumentParser(description="Plot dashboard stress test") + parser.add_argument( + "--freq", type=float, default=20, help="Frequency of logging (applies to all series)" + ) + parser.add_argument( + "--window-size", type=float, default=5.0, help="Size of the window in seconds" + ) + parser.add_argument("--duration", type=float, default=60, help="How long to log for in seconds") + parser.add_argument("--use-rs", action="store_true", help="Use RealSense sensor") + parser.add_argument("--use-zed", action="store_true", help="Use ZED sensor") + parser.add_argument("--in-docker", action="store_true", help="Running inside Docker container") + args = parser.parse_args() + + if args.use_rs: + image_keys = ["color_image", "depth_image"] + from gr00t_wbc.control.sensor.realsense import RealSenseClientSensor + + sensor = RealSenseClientSensor() + elif args.use_zed: + image_keys = ["left_image", "right_image"] + from gr00t_wbc.control.sensor.zed import ZEDClientSensor + + sensor = ZEDClientSensor() + else: + from gr00t_wbc.control.sensor.dummy import DummySensor + + sensor = DummySensor() + image_keys = ["color_image"] + + tensor_keys = ["left_arm_qpos", "left_hand_qpos", "right_arm_qpos", "right_hand_qpos"] + + # Initialize the RerunViz class + viz = RerunViz( + image_keys=image_keys, + tensor_keys=tensor_keys, + window_size=args.window_size, + in_docker=args.in_docker, + ) + + # Run the visualization loop + cur_time = time.time() + end_time = cur_time + args.duration + time_per_tick = 1.0 / args.freq + + while cur_time < end_time: + # Advance time and sleep if necessary + cur_time += time_per_tick + sleep_for = cur_time - time.time() + if sleep_for > 0: + time.sleep(sleep_for) + + if sleep_for < -0.1: + print(f"Warning: missed logging window by {-sleep_for:.2f} seconds") + + # Plot dummy tensor + dummy_tensor = np.random.randn(5) + dummy_tensor_dict = {key: dummy_tensor for key in tensor_keys} + + viz.plot_tensors(dummy_tensor_dict, cur_time) + + # Plot images if available + images = sensor.read() + if images is not None: + img_to_show = {key: images[key] for key in image_keys} + viz.plot_images(img_to_show, cur_time) + + rr.script_teardown(args) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/requirements.txt b/gr00t_wbc/dexmg/gr00trobocasa/requirements.txt new file mode 100644 index 0000000..d6e1198 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/requirements.txt @@ -0,0 +1 @@ +-e . diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/__init__.py new file mode 100644 index 0000000..d8288ae --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/__init__.py @@ -0,0 +1,73 @@ +from robocasa.environments.locomanipulation.base import ( + PnPBottle, + PickBottleShelf, + PnPBottleHigh, + NavPickBottle, + PnPBottleRandRobotPose, + VisualReach, + PnPBottleFixtureToFixture, + PnPBottleFixtureToFixtureSourceDemo, + PnPBottleShelfToTable, + PnPBottleTableToTable, + PickBottleGround, + PickBottles, + NavPickBottles, + PnPBottlesTableToTable, +) +from robocasa.environments.locomanipulation.locomanip_basic import ( + LMPickBottle, + LMPickBottleHigh, + LMNavPickBottle, + LMPickBottleGround, + LMPnPBottle, + LMPickMultipleBottles, + LMPnPMultipleBottles, + LMPickBottleShelf, + LMNavPickBottleShelf, + LMPickBottleShelfLow, + LMNavPickBottleShelfLow, +) +from robocasa.environments.locomanipulation.locomanip_pnp import ( + LMBottlePnP, + LMBoxPnP, +) + + +# from robosuite.controllers import ALL_CONTROLLERS, load_controller_config +from robosuite.controllers import ALL_PART_CONTROLLERS, load_composite_controller_config +from robosuite.environments import ALL_ENVIRONMENTS +from robosuite.models.grippers import ALL_GRIPPERS +from robosuite.robots import ALL_ROBOTS + + +import mujoco + +assert ( + mujoco.__version__ == "3.2.6" or mujoco.__version__ == "3.3.2" +), "MuJoCo version must be 3.2.6 or 3.3.2. Please install the correct version." + +import numpy + +assert numpy.__version__ in [ + "1.23.2", + "1.23.3", + "1.23.5", + "1.26.4", + "2.2.5", + "2.2.6", +], "numpy version must be either 1.23.{2,3,5}, 1.26.4 or 2.2.{5,6}. Please install one of these versions." + +import robosuite + +assert robosuite.__version__ in [ + "1.5.0", + "1.5.1", +], "robosuite version must be 1.5.{0,1}. Please install the correct version" + +__version__ = "0.2.0" +__logo__ = """ + ; / ,--. + ["] ["] ,< |__**| + /[_]\ [~]\/ |// | + ] [ OOO /o|__| +""" diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/__init__.py new file mode 100644 index 0000000..bf2e332 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/__init__.py @@ -0,0 +1,13 @@ +""" +Full list of loco-manipulation tasks. + +GroundOnly - ground only environments + +locomanip_pnp - factory environments, pick and place tasks: +LMBottlePnP +LMBoxPnP +""" + +from .base import REGISTERED_LOCOMANIPULATION_ENVS + +ALL_LOCOMANIPULATION_ENVIRONMENTS = REGISTERED_LOCOMANIPULATION_ENVS.keys() diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/base.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/base.py new file mode 100644 index 0000000..fde74e9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/base.py @@ -0,0 +1,1658 @@ +from copy import deepcopy +import os +from typing import Optional, Type +import warnings +import xml.etree.ElementTree as ET + +import mujoco +import numpy as np +import robosuite +from robosuite.environments.base import EnvMeta +from robosuite.environments.manipulation.manipulation_env import ManipulationEnv +from robosuite.models.arenas import Arena +from robosuite.models.tasks import ManipulationTask +from robosuite.utils.mjcf_utils import array_to_string, find_elements, xml_path_completion +from robosuite.utils.observables import Observable, sensor + +import robocasa +from robocasa.models.objects.objects import MJCFObject +from robocasa.models.scenes import GroundArena +import robocasa.utils.camera_utils as CamUtils +from robocasa.utils.dexmg_utils import DexMGConfigHelper +from robocasa.utils.object_utils import check_obj_upright +from robocasa.utils.visuals_utls import Gradient, randomize_materials_rgba + +REGISTERED_LOCOMANIPULATION_ENVS = {} + + +def register_locomanipulation_env(target_class): + REGISTERED_LOCOMANIPULATION_ENVS[target_class.__name__] = target_class + + +class LocoManipulationEnvMeta(EnvMeta): + """Metaclass for registering robocasa environments""" + + def __new__(meta, name, bases, class_dict): + cls = super().__new__(meta, name, bases, class_dict) + register_locomanipulation_env(cls) + return cls + + +class CameraPoseRandomizer: + @staticmethod + def randomize_cameras( + env: "LocoManipulationEnv", + cam_names: list[str], + pos_range: tuple[np.ndarray, np.ndarray], + euler_range: tuple[np.ndarray, np.ndarray], + ): + """ + Randomize camera poses while maintaining their relative transforms. + + Args: + env: The environment instance + cam_names: List of camera names to randomize together + pos_range: Tuple of (min_pos, max_pos) as 3D arrays for position randomization + euler_range: Tuple of (min_euler, max_euler) as 3D arrays for euler angle randomization (in radians) + """ + if len(cam_names) == 0: + return + + # Sample random transform offset + random_pos_offset = env.rng.uniform(pos_range[0], pos_range[1]) + random_euler_offset = env.rng.uniform(euler_range[0], euler_range[1]) + + # Convert euler offset to quaternion + quat_offset = np.zeros(4, dtype=float) + mujoco.mju_euler2Quat(quat_offset, random_euler_offset, "xyz") + + # Apply the same transform to all specified cameras + for cam_name in cam_names: + if cam_name not in env._cam_configs: + warnings.warn(f"Camera {cam_name} not found in camera configs. Skipping.") + continue + + cam_config = env._cam_configs[cam_name] + + # Get original position and quaternion + original_pos = np.array(cam_config["pos"], dtype=float) + original_quat = np.array(cam_config["quat"], dtype=float) + + # Apply rotation offset to position (rotate position offset by the random rotation) + rotated_offset = np.zeros(3, dtype=float) + mujoco.mju_rotVecQuat(rotated_offset, random_pos_offset, original_quat) + new_pos = original_pos + rotated_offset + + # Compose quaternions: new_quat = quat_offset * original_quat + new_quat = np.zeros(4, dtype=float) + mujoco.mju_mulQuat(new_quat, quat_offset, original_quat) + + # Update camera config + cam_config["pos"] = new_pos.tolist() + cam_config["quat"] = new_quat.tolist() + + # Update in simulation if already created + if hasattr(env, "sim") and env.sim is not None: + try: + cam_id = env.sim.model.camera_name2id(cam_name) + env.sim.model.cam_pos[cam_id] = new_pos + env.sim.model.cam_quat[cam_id] = new_quat + except: + # Camera might not be in the model yet + pass + + +class RobotPoseRandomizer: + @staticmethod + def set_pose( + env: "LocoManipulationEnv", + x_range: [tuple[float, float]], + y_range: [tuple[float, float]], + yaw_range: [tuple[float, float]], + ): + new_x = env.rng.uniform(*x_range) + new_y = env.rng.uniform(*y_range) + new_yaw = env.rng.uniform(*yaw_range) + + if env.robots[0].name == "G1": + base_offset = env.ROBOT_POS_OFFSETS[env.robots[0].robot_model.__class__.__name__] + target_pos = np.array([new_x, new_y, base_offset[2]], dtype=float) + quat = np.zeros(4, dtype=float) + mujoco.mju_euler2Quat(quat, np.array([0.0, 0.0, new_yaw]), "xyz") + base_freejoint = f"{env.robots[0].robot_model.naming_prefix}base" + if base_freejoint in env.sim.model.joint_names: + env.sim.data.set_joint_qpos(base_freejoint, np.concatenate([target_pos, quat])) + else: + warnings.warn(f"Base joint {base_freejoint} not found in the model.") + else: + base_joint_pos = np.array([new_x, new_y, new_yaw]) + base_joint_names = [ + "mobilebase0_joint_mobile_forward", + "mobilebase0_joint_mobile_side", + "mobilebase0_joint_mobile_yaw", + ] + for i, base_joint_name in enumerate(base_joint_names): + if base_joint_name not in env.sim.model.joint_names: + warnings.warn( + f"Base joint {base_joint_name} not found in the model. " + f"Skipping randomization of {base_joint_name}." + ) + else: + env.sim.data.set_joint_qpos(base_joint_name, base_joint_pos[i]) + + @staticmethod + def set_arm(env: ManipulationEnv, elbow_qpos: float, shoulder_pitch_qpos: float): + """Helper function to reinitialize G1 robot arm configuration.""" + robot = env.robots[0] + if "G1" not in robot.name: + # avoid reinitializing arm configuration for non-G1 robots + return + + joint_names = robot.robot_joints + joint_pos_indices = robot._ref_joint_pos_indexes + for joint_name, pos_idx in zip(joint_names, joint_pos_indices): + if "elbow" in joint_name: + print(f"reinitializing G1 {joint_name} with idx {pos_idx} to {elbow_qpos}") + env.sim.data.qpos[pos_idx] = elbow_qpos + elif "shoulder_pitch" in joint_name: + print(f"reinitializing G1 {joint_name} with idx {pos_idx} to {shoulder_pitch_qpos}") + env.sim.data.qpos[pos_idx] = shoulder_pitch_qpos + + +class LocoManipulationEnv(ManipulationEnv, metaclass=LocoManipulationEnvMeta): + """ + Initialized a Base Ground Standing environment. + """ + + MUJOCO_ARENA_CLS: Type[Arena] = GroundArena + + ROBOT_POS_OFFSETS: dict[str, list[float]] = { + "PandaOmron": [0, 0, 0], + "GR1FloatingBody": [0, 0, 0.97], + "GR1": [0, 0, 0.97], + "GR1FixedLowerBody": [0, 0, 0.97], + "GR1FixedLowerBodyInspireHands": [0, 0, 0.97], + "GR1FixedLowerBodyFourierHands": [0, 0, 0.97], + "GR1ArmsOnly": [0, 0, 0.97], + "GR1ArmsOnlyInspireHands": [0, 0, 0.97], + "GR1ArmsOnlyFourierHands": [0, 0, 0.97], + "GR1ArmsAndWaistFourierHands": [0, 0, 0.97], + "G1": [0, 0, 0.793], + "G1FixedBase": [0, 0, 0.793], + "G1FixedLowerBody": [0, 0, 0.793], + "G1ArmsOnly": [0, 0, 0.793], + "G1ArmsOnlyFloating": [0, 0, 0.793], + "G1FloatingBody": [0, 0, 0.793], + "G1FloatingBodyWithVertical": [0, 0, 0.793], + } + + def __init__( + self, + translucent_robot: bool = False, + use_object_obs: bool = False, + randomize_cameras: bool = False, + *args, + **kwargs, + ): + self.mujoco_objects = [] + self.randomize_cameras = randomize_cameras + + super().__init__( + *args, + **kwargs, + ) + + self.translucent_robot = translucent_robot + + def _load_model(self): + super()._load_model() + + self.mujoco_arena = self.MUJOCO_ARENA_CLS() + self.mujoco_arena.set_origin([0, 0, 0]) + self.set_cameras() + + self.model = ManipulationTask( + mujoco_arena=self.mujoco_arena, + mujoco_robots=[robot.robot_model for robot in self.robots], + mujoco_objects=self.mujoco_objects, + ) + + robot_base_pos = self.ROBOT_POS_OFFSETS[self.robots[0].robot_model.__class__.__name__] + robot_model = self.robots[0].robot_model + robot_model.set_base_xpos(robot_base_pos) + # robot_model.set_base_ori(robot_base_ori) + + def set_cameras(self): + """ + Adds new tabletop-relevant cameras to the environment. Will randomize cameras if specified. + """ + + self._cam_configs = deepcopy(CamUtils.CAM_CONFIGS) + + for robot in self.robots: + if hasattr(robot.robot_model, "get_camera_configs"): + self._cam_configs.update(robot.robot_model.get_camera_configs()) + + for cam_name, cam_cfg in self._cam_configs.items(): + if cam_cfg.get("parent_body", None) is not None: + continue + + self.mujoco_arena.set_camera( + camera_name=cam_name, + pos=cam_cfg["pos"], + quat=cam_cfg["quat"], + camera_attribs=cam_cfg.get("camera_attribs", None), + ) + + self.mujoco_arena.set_camera( + camera_name="egoview", + pos=[0.078, 0, 1.308], + quat=[0.66491268, 0.24112495, -0.24112507, -0.66453637], + camera_attribs=dict(fovy="90"), + ) + + def visualize(self, vis_settings): + """ + In addition to super call, make the robot semi-transparent + + Args: + vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific + component should be visualized. Should have "grippers" keyword as well as any other relevant + options specified. + """ + # Run superclass method first + super().visualize(vis_settings=vis_settings) + + visual_geom_names = [] + + for robot in self.robots: + robot_model = robot.robot_model + visual_geom_names += robot_model.visual_geoms + + for name in visual_geom_names: + rgba = self.sim.model.geom_rgba[self.sim.model.geom_name2id(name)] + if self.translucent_robot: + rgba[-1] = 0.10 + else: + rgba[-1] = 1.0 + + def reward(self, action=None): + """ + Reward function for the task. The reward function is based on the task + and to be implemented in the subclasses. Returns 0 by default. + + Returns: + float: Reward for the task + """ + reward = 0 + if self._check_success(): + reward = 1.0 + return reward + + def _check_success(self): + """ + Checks if the task has been successfully completed. + Success condition is based on the task and to be implemented in the + subclasses. Returns False by default. + + Returns: + bool: True if the task is successfully completed, False otherwise + """ + return False + + def edit_model_xml(self, xml_str): + """ + This function postprocesses the model.xml collected from a MuJoCo demonstration + for retrospective model changes. + + Args: + xml_str (str): Mujoco sim demonstration XML file as string + + Returns: + str: Post-processed xml file as string + """ + xml_str = super().edit_model_xml(xml_str) + + tree = ET.fromstring(xml_str) + root = tree + worldbody = root.find("worldbody") + actuator = root.find("actuator") + asset = root.find("asset") + meshes = asset.findall("mesh") + textures = asset.findall("texture") + all_elements = meshes + textures + + robosuite_path_split = os.path.split(robosuite.__file__)[0].split("/") + robocasa_path_split = os.path.split(robocasa.__file__)[0].split("/") + + # replace robocasa-specific asset paths + for elem in all_elements: + old_path = elem.get("file") + if old_path is None: + continue + + old_path_split = old_path.split("/") + # maybe replace all paths to robosuite assets + if "models/assets" in old_path: + if "/robosuite/" in old_path: + check_lst = [ + loc for loc, val in enumerate(old_path_split) if val == "robosuite" + ] + ind = max(check_lst) # last occurrence index + new_path_split = robosuite_path_split + old_path_split[ind + 1 :] + elif "/robocasa/" in old_path: + check_lst = [loc for loc, val in enumerate(old_path_split) if val == "robocasa"] + ind = max(check_lst) # last occurrence index + new_path_split = robocasa_path_split + old_path_split[ind + 1 :] + else: + raise ValueError + + new_path = "/".join(new_path_split) + elem.set("file", new_path) + + # set cameras + for cam_name, cam_config in self._cam_configs.items(): + parent_body = cam_config.get("parent_body", None) + + cam_root = worldbody + if parent_body is not None: + cam_root = find_elements(root=worldbody, tags="body", attribs={"name": parent_body}) + if cam_root is None: + # camera config refers to body that doesnt exist on the robot + continue + + cam = find_elements(root=cam_root, tags="camera", attribs={"name": cam_name}) + + if cam is None: + old_cam = find_elements(root=worldbody, tags="camera", attribs={"name": cam_name}) + if old_cam is not None: + # old camera associated with different body + continue + + cam = ET.Element("camera") + cam.set("mode", "fixed") + cam.set("name", cam_name) + cam_root.append(cam) + + cam.set("pos", array_to_string(cam_config["pos"])) + cam.set("quat", array_to_string(cam_config["quat"])) + for k, v in cam_config.get("camera_attribs", {}).items(): + cam.set(k, v) + + # replace base -> mobilebase (this is needed for old PandaOmron demos) + for elem in find_elements( + root=worldbody, tags=["geom", "site", "body", "joint"], return_first=False + ): + if elem.get("name") is None: + continue + if elem.get("name").startswith("base0_"): + old_name = elem.get("name") + new_name = "mobilebase0_" + old_name[6:] + elem.set("name", new_name) + for elem in find_elements( + root=actuator, + tags=["velocity", "position", "motor", "general"], + return_first=False, + ): + if elem.get("name") is None: + continue + if elem.get("name").startswith("base0_"): + old_name = elem.get("name") + new_name = "mobilebase0_" + old_name[6:] + elem.set("name", new_name) + for elem in find_elements( + root=actuator, + tags=["velocity", "position", "motor", "general"], + return_first=False, + ): + if elem.get("joint") is None: + continue + if elem.get("joint").startswith("base0_"): + old_joint = elem.get("joint") + new_joint = "mobilebase0_" + old_joint[6:] + elem.set("joint", new_joint) + + # result = ET.tostring(root, encoding="utf8").decode("utf8") + result = ET.tostring(root).decode("utf8") + + # # replace with generative textures + # if (self.generative_textures is not None) and ( + # self.generative_textures is not False + # ): + # # sample textures + # assert self.generative_textures == "100p" + # self._curr_gen_fixtures = get_random_textures(self.rng) + + # cab_tex = self._curr_gen_fixtures["cab_tex"] + # counter_tex = self._curr_gen_fixtures["counter_tex"] + # wall_tex = self._curr_gen_fixtures["wall_tex"] + # floor_tex = self._curr_gen_fixtures["floor_tex"] + + # result = replace_cab_textures( + # self.rng, result, new_cab_texture_file=cab_tex + # ) + # result = replace_counter_top_texture( + # self.rng, result, new_counter_top_texture_file=counter_tex + # ) + # result = replace_wall_texture( + # self.rng, result, new_wall_texture_file=wall_tex + # ) + # result = replace_floor_texture( + # self.rng, result, new_floor_texture_file=floor_tex + # ) + + return result + + def _setup_references(self): + super()._setup_references() + + self.obj_body_id = {} + + def _randomize_robot_cameras(self): + """Randomize the poses of robot-mounted cameras while preserving their relative transforms.""" + cam_names = ["robot0_oak_egoview", "robot0_oak_left_monoview", "robot0_oak_right_monoview"] + + # Define randomization ranges + pos_range = ( + np.array([-0.02, -0.02, -0.02]), # min position offset [x, y, z] in meters + np.array([0.02, 0.02, 0.02]), # max position offset [x, y, z] in meters + ) + euler_range = ( + np.array([-0.1, -0.1, -0.1]), # min euler angles [roll, pitch, yaw] in radians + np.array([0.1, 0.1, 0.1]), # max euler angles [roll, pitch, yaw] in radians + ) + + CameraPoseRandomizer.randomize_cameras( + env=self, cam_names=cam_names, pos_range=pos_range, euler_range=euler_range + ) + + def _reset_internal(self): + super()._reset_internal() + + if self.randomize_cameras: + self._randomize_robot_cameras() + + def _reset_observables(self): + if self.hard_reset: + self._observables = self._setup_observables() + + # these sensors need a lot of computation, so we disable them by default for speed up simulation + disabled_sensors = [ + "base_to_left_eef_pos", + "base_to_left_eef_quat", + "base_to_left_eef_quat_site", + "base_to_right_eef_pos", + "base_to_right_eef_quat", + "base_to_right_eef_quat_site", + ] + for name in disabled_sensors: + for robot in self.robots: + robot_name_prefix = robot.robot_model.naming_prefix + if f"{robot_name_prefix}{name}" in self._observables: + self._observables[f"{robot_name_prefix}{name}"].set_enabled(False) + self._observables[f"{robot_name_prefix}{name}"].set_active(False) + + def get_state(self): + return {"states": self.sim.get_state().flatten()} + + +class GroundOnly(LocoManipulationEnv): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + +class PrimitiveBottle: + DEFAULT_RGB = [0.3, 0.7, 0.8] + + def __init__( + self, + name="bottle", + radius: float = 0.03, + half_height: float = 0.075, + rgb: Optional[list[float]] = None, + ): + self.name = name + self.assets = [ + ET.Element( + "texture", + type="2d", + name=f"{name}_tex", + builtin="flat", + rgb1=" ".join(map(str, self.DEFAULT_RGB if rgb is None else rgb)), + width="512", + height="512", + ), + ET.Element( + "material", + name=f"{name}_mat", + texture=f"{name}_tex", + texuniform="true", + reflectance="0.1", + ), + ] + + self.body = ET.Element("body", name=f"{self.name}_body", pos="0.35 0 0.8") + bottle_vis_geom = ET.Element( + "geom", + name=f"{name}_vis", + pos="0 0 0", + size=f"{radius} {half_height}", + type="cylinder", + material=f"{name}_mat", + group="1", + conaffinity="0", + contype="0", + ) + self.body.append(bottle_vis_geom) + + # Cylinder collider approximation for stable contacts + self.contact_geoms = [] + n_sides = 3 + half_width = radius * np.tan(np.pi / n_sides / 2) + for i in range(n_sides): + coll_name = f"{self.name}_collider_{i}" + angle = np.pi / n_sides * i + quat = np.zeros(4) + euler = np.array([0, 0, angle]) + mujoco.mju_euler2Quat(quat, euler, "xyz") + box_geom = ET.Element( + "geom", + name=coll_name, + type="box", + pos="0 0 0", + size=f"{radius} {half_width} {half_height}", + quat=" ".join(map(str, quat)), + solimp="0.998 0.998 0.001", + solref="0.001 2", + density="100", + friction="0.95 0.3 0.1", + ) + self.body.append(box_geom) + self.contact_geoms.append(coll_name) + + bottle_joint = ET.Element( + "joint", + name=f"{self.name}_joint", + type="free", + damping="0.0005", + ) + self.body.append(bottle_joint) + + +class PrimitiveFixture: + DEFAULT_RGB = [0.8, 0.8, 0.8] + + def __init__( + self, + name: str, + pos: np.ndarray = np.array([0.0, 0.0, 0.8]), + half_size: np.ndarray = np.array([0.1, 0.1, 0.001]), + rgb: Optional[str] = None, + ): + """ + A simple primitive fixture as a flat box. + + Args: + half_size: Half-sizes in [x, y, z] directions. Default creates a 20cm x 20cm x 2mm box. + """ + self.half_size = half_size + + self.assets = [ + ET.Element( + "texture", + type="2d", + name=f"{name}", + builtin="flat", + rgb1=" ".join(map(str, self.DEFAULT_RGB if rgb is None else rgb)), + width="512", + height="512", + ), + ET.Element( + "material", + name=f"{name}", + texture=f"{name}", + texuniform="true", + reflectance="0.05", # Less reflective than bottle + ), + ] + + self.body = ET.Element("body", name=f"{name}_body", pos=array_to_string(pos)) + + # Visual geometry + fixture_vis_geom = ET.Element( + "geom", + name=f"{name}_vis", + pos="0 0 0", + size=f"{half_size[0]} {half_size[1]} {half_size[2]}", + type="box", + material=f"{name}", + group="1", + conaffinity="0", + contype="0", + ) + self.body.append(fixture_vis_geom) + + # Collision geometry - just a single box since it's already a simple shape + self.contact_geoms = [] + fixture_collider = ET.Element( + "geom", + name=f"{name}_collider", + type="box", + pos="0 0 0", + size=f"{half_size[0]} {half_size[1]} {half_size[2]}", + solimp="0.998 0.998 0.001", + solref="0.001 2", + density="100", + friction="0.6 0.01 0.001", # Similar to add_fixture_body friction + ) + self.body.append(fixture_collider) + self.contact_geoms.append("fixture_collider") + + +class PnPBottle(LocoManipulationEnv, DexMGConfigHelper): + TABLE_GRADIENT: Gradient = Gradient( + np.array([0.68, 0.34, 0.07, 1.0]), np.array([1.0, 1.0, 1.0, 1.0]) + ) + DEFAULT_BOTTLE_POS: np.ndarray = np.array([0.4, 0, 0.77]) + BOTTLE_POS_RANGE_X = (-0.08, 0.04) + BOTTLE_POS_RANGE_Y = (-0.08, 0.08) + + def __init__(self, *args, **kwargs): + self.objects = {} + super().__init__(*args, **kwargs) + + def _load_model(self): + self.mujoco_objects = [self._create_table("table_body", [0.5, 0, 0], [0, 0, np.pi / 2])] + + super()._load_model() + + self.bottle = self._create_bottle() + + @staticmethod + def _create_table(name: str, position: list[float], euler: list[float]) -> MJCFObject: + table = MJCFObject( + name=name, + mjcf_path=xml_path_completion( + "objects/omniverse/locomanip/lab_table/model.xml", root=robocasa.models.assets_root + ), + scale=1.0, + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=10, + friction=(1, 1, 1), + static=True, + ) + table.set_pos(position) + table.set_euler(euler) + return table + + def _create_bottle( + self, name: str = "bottle", rgb: Optional[list[float]] = None + ) -> PrimitiveBottle: + bottle = PrimitiveBottle(name=name, radius=0.03, half_height=0.075, rgb=rgb) + self.model.asset.extend(bottle.assets) + self.model.worldbody.append(bottle.body) + self.objects[name] = {"name": f"{name}_body"} + return bottle + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + super()._reset_internal() + + if not self.deterministic_reset: + self._randomize_bottle_placement() + self._randomize_table_texture() + + def _randomize_bottle_placement( + self, name: str = "bottle", base_pos: Optional[np.ndarray] = None + ): + if not self.deterministic_reset: + bottle_joint = f"{name}_joint" + base_pos = self.DEFAULT_BOTTLE_POS if base_pos is None else base_pos + + random_x = self.rng.uniform(*self.BOTTLE_POS_RANGE_X) + random_y = self.rng.uniform(*self.BOTTLE_POS_RANGE_Y) + new_pos = base_pos + np.array([random_x, random_y, 0]) + + current_qpos = self.sim.data.get_joint_qpos(bottle_joint) + new_qpos = current_qpos.copy() + new_qpos[:3] = new_pos + + self.sim.data.set_joint_qpos(bottle_joint, new_qpos) + + def _randomize_table_texture(self): + table = self.mujoco_objects[0] + randomize_materials_rgba( + rng=self.rng, mjcf_obj=table, gradient=self.TABLE_GRADIENT, linear=True + ) + + def _setup_references(self): + super()._setup_references() + + self.obj_body_id = {} + for name, model in self.objects.items(): + self.obj_body_id[name] = self.sim.model.body_name2id(model["name"]) + + def _check_success(self): + check_grasp = self._check_grasp(self.robots[0].gripper["right"], self.bottle.contact_geoms) + + bottle_z = self.sim.data.body_xpos[self.obj_body_id["bottle"]][2] + table_z = self.mujoco_objects[0].top_offset[2] + check_bottle_in_air = bottle_z > table_z + 0.2 + # check bottle and table collision + # check_bottle_in_air = not self.check_contact("bottle", "table") + return check_grasp and check_bottle_in_air + + def get_object(self): + return dict( + bottle=dict(obj_name=self.objects["bottle"]["name"], obj_type="body"), + ) + + def get_subtask_term_signals(self): + signals = dict() + signals["grasp_bottle"] = int( + self._check_grasp(self.robots[0].gripper["right"], self.bottle.contact_geoms) + ) + return signals + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + task.task_spec_0.subtask_1 = dict( + object_ref="bottle", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + +def create_shelf(pos: list[float], euler: list[float]) -> MJCFObject: + shelf = MJCFObject( + name="shelf_body", + mjcf_path=xml_path_completion( + "objects/aigc/shelf/model.xml", root=robocasa.models.assets_root + ), + scale=[1.0, 1.0, 1.0], + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=10, + friction=(1, 1, 1), + static=True, + ) + shelf.set_pos(pos) + shelf.set_euler(euler) + return shelf + + +class PickBottleShelf(PnPBottle): + def _load_model(self): + # Create both the original table and the target table + self.mujoco_objects = [create_shelf(pos=[0.8, 0.4, 0], euler=[0, 0, np.pi / 2])] + + LocoManipulationEnv._load_model(self) + + self.bottle = self._create_bottle() + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + LocoManipulationEnv._reset_internal(self) + + if not self.deterministic_reset: + # Base position on ground (z=0.075 is bottle radius) + # Level 2 of shelf + self._randomize_bottle_placement(base_pos=np.array([0.7, 0.4, 0.376660 + 0.075 + 0.02])) + self._randomize_table_texture() + RobotPoseRandomizer.set_arm(self, elbow_qpos=-0.5, shoulder_pitch_qpos=0.5) + + +class PnPBottleHigh(PnPBottle): + def _load_model(self): + self.mujoco_objects = [self._create_table("table_body", [0.5, 0, 0.1], [0, 0, np.pi / 2])] + + LocoManipulationEnv._load_model(self) + + self.bottle = self._create_bottle() + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + LocoManipulationEnv._reset_internal(self) + + # Randomize bottle position within +/- 0.1 range on x and y axes + if not self.deterministic_reset: + # Base position of the bottle + base_pos = np.array([0.4, 0, 0.875]) + + # Add random offset within +/- 0.1 range for x and y + random_x = np.random.uniform(-0.1, 0.1) + random_y = np.random.uniform(-0.1, 0.1) + # New randomized position (keep z constant) + new_pos = base_pos + np.array([random_x, random_y, 0]) + + # Set the bottle position using the free joint + # For free joints, qpos includes [x, y, z, qw, qx, qy, qz] + current_qpos = self.sim.data.get_joint_qpos("bottle_joint") + new_qpos = current_qpos.copy() + new_qpos[:3] = new_pos # Update position (x, y, z) + + self.sim.data.set_joint_qpos("bottle_joint", new_qpos) + + def _setup_observables(self): + observables = super()._setup_observables() + + @sensor(modality="object") + def obj_pos(obs_cache): + return self.sim.data.body_xpos[self.obj_body_id["bottle"]] + + @sensor(modality="object") + def obj_quat(obs_cache): + return self.sim.data.body_xquat[self.obj_body_id["bottle"]] + + @sensor(modality="object") + def obj_linear_vel(obs_cache): + return self.sim.data.get_body_xvelp("bottle_body") + + @sensor(modality="object") + def obj_angular_vel(obs_cache): + return self.sim.data.get_body_xvelr("bottle_body") + + sensors = [obj_pos, obj_quat, obj_linear_vel, obj_angular_vel] + names = [s.__name__ for s in sensors] + + for name, s in zip(names, sensors): + observables[name] = Observable( + name=name, + sensor=s, + sampling_rate=self.control_freq, + ) + + return observables + + def get_privileged_obs_keys(self): + return { + "obj_pos": (3,), + "obj_quat": (4,), + "obj_linear_vel": (3,), + "obj_angular_vel": (3,), + } + + +class NavPickBottle(PnPBottle): + """ + Pick-and-Place Bottle environment with robot position randomized at reset. + """ + + def _reset_internal(self): + super()._reset_internal() + + if not self.deterministic_reset: + RobotPoseRandomizer.set_pose(self, (-0.3, -0.16), (-0.2, 0.2), (-np.pi / 6, np.pi / 6)) + + +class PnPBottleRandRobotPose(NavPickBottle): + pass + + +class VisualReach(LocoManipulationEnv): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + def _load_model(self): + super()._load_model() + + self.create_visual_only_goal_cube() + + def create_visual_only_goal_cube(self): + cube_tex = ET.Element( + "texture", + type="2d", + name="cube", + builtin="flat", + rgb1="1.0 0.0 0.0", + width="512", + height="512", + ) + cube_mat = ET.Element( + "material", + name="cube", + texture="cube", + texuniform="true", + reflectance="0.1", + ) + self.model.asset.append(cube_tex) + self.model.asset.append(cube_mat) + + self.objects = {} + cube_body = ET.Element("body", name="cube_body", pos="0.4 0 0.875") + + cube_vis_geom = ET.Element( + "geom", + name="cube_vis", + pos="0 0 0", + size="0.0375 0.0375 0.0375", + type="box", + material="cube", + group="1", + conaffinity="0", + contype="0", + ) + + cube_body.append(cube_vis_geom) + self.model.worldbody.append(cube_body) + self.objects["cube"] = {"name": "cube_body"} + + def _setup_references(self): + super()._setup_references() + + self.obj_body_id = {} + for name, model in self.objects.items(): + self.obj_body_id[name] = self.sim.model.body_name2id(model["name"]) + + def _check_success(self): + # check_grasp = self._check_grasp(self.robots[0].gripper["right"], self.objects["bottle"]) + # check_reach = self._check_reach(self.objects["bottle"]) + return True + + def _check_reach(self, obj_name): + raise NotImplementedError + # To be implemented by the subclass + + def get_object(self): + return dict( + cube=dict(obj_name=self.objects["cube"].root_body, obj_type="body"), + ) + + def reset_obj_pos(self): + # reset object pos randomly around bottle_body pos="0.4 0 0.875" + init_pos = np.array([0.4, 0, 0.875]) + random_x = np.random.uniform(-0.3, 0.15) + random_y = np.random.uniform(-0.15, 0.15) + random_z = np.random.uniform(-0.15, 0.30) + self.sim.model.body_pos[self.obj_body_id["cube"]] = init_pos + np.array( + [random_x, random_y, random_z] + ) + + def set_cameras(self): + super().set_cameras() + self.mujoco_arena.set_camera( + camera_name="egoview", + pos=[0.078, 0, 1.308], + quat=[0.66491268, 0.24112495, -0.24112507, -0.66453637], + camera_attribs=dict(fovy="90"), + ) + + def _setup_observables(self): + observables = super()._setup_observables() + + @sensor(modality="object") + def obj_pos(obs_cache): + return self.sim.data.body_xpos[self.obj_body_id["cube"]] + + @sensor(modality="object") + def obj_quat(obs_cache): + return self.sim.data.body_xquat[self.obj_body_id["cube"]] + + @sensor(modality="object") + def obj_linear_vel(obs_cache): + return self.sim.data.get_body_xvelp("cube_body") + + @sensor(modality="object") + def obj_angular_vel(obs_cache): + return self.sim.data.get_body_xvelr("cube_body") + + sensors = [obj_pos, obj_quat, obj_linear_vel, obj_angular_vel] + names = [s.__name__ for s in sensors] + + for name, s in zip(names, sensors): + observables[name] = Observable( + name=name, + sensor=s, + sampling_rate=self.control_freq, + ) + + return observables + + def get_privileged_obs_keys(self): + return { + "obj_pos": (3,), + "obj_quat": (4,), + "obj_linear_vel": (3,), + "obj_angular_vel": (3,), + } + + +class PnPBottleFixtureToFixture(PnPBottle): + """ + Task: Robot picks up bottle and places it on a fixture. + + Initialization: bottle rests on a source fixture. + + Idea: by changing the location of the target fixture, we can change the data generation task layout for + these placement related tasks. + """ + + SEPARATION_THRESH_M: float = 0.0005 # 0.05 cm + DISTMAX_SCAN_M: float = 0.05 # 5 cm window for distance queries + _SRC_NAME = "start_fixture" + _TGT_NAME = "target_fixture" + _FIXTURE_HALF_SIZE = np.array([0.05, 0.05, 0.001]) + _BOTTLE_HALF_HEIGHT = 0.075 + _X_SRC_RANGE = (0.30, 0.55) + _X_TGT_RANGE = (0.30, 0.55) + _Y_SRC_RANGE = (-0.20, -0.05) + _Y_TGT_RANGE = (0.05, 0.20) + _SRC_FIXTURE_VISIBLE = False + _TGT_FIXTURE_VISIBLE = True + + def _load_model(self): + self.mujoco_objects = [self._create_table("table_body", [0.5, 0, 0], [0, 0, np.pi / 2])] + LocoManipulationEnv._load_model(self) + self.bottle = self._create_bottle() + self._create_fixture(self._SRC_NAME, visible=self._SRC_FIXTURE_VISIBLE, rgb="1 0 0") + self._create_fixture(self._TGT_NAME, visible=self._TGT_FIXTURE_VISIBLE, rgb="0 1 0") + self._src_body = f"{self._SRC_NAME}_body" + self._tgt_body = f"{self._TGT_NAME}_body" + self._src_coll = f"{self._SRC_NAME}_collider" + self._tgt_coll = f"{self._TGT_NAME}_collider" + + def _create_fixture(self, name: str, visible: bool, rgb: Optional[str] = None) -> None: + """Create a flat box fixture; add assets + body to the compiled model.""" + fx = PrimitiveFixture( + name=name, pos=np.array([0.0, 0.0, 0.0]), half_size=self._FIXTURE_HALF_SIZE, rgb=rgb + ) + + # Make source fixture invisible (keep collision only) + if not visible: + # Find the visual geom and hide it + for child in list(fx.body): + if child.tag == "geom" and child.get("name") == f"{name}_vis": + child.set("rgba", "0 0 0 0") # invisible visual + break + + # Register assets + body into the scene graph + self.model.asset.extend(fx.assets) + self.model.worldbody.append(fx.body) + + def _setup_references(self): + super()._setup_references() + # Table root body is "_main" (same convention as target_table above) + self.table_body_id = self.sim.model.body_name2id("table_body_main") + self.src_fixture_id = self.sim.model.body_name2id(self._src_body) + self.tgt_fixture_id = self.sim.model.body_name2id(self._tgt_body) + + def _check_success(self) -> bool: + """Bottle touches target fixture collider and is upright.""" + bottle_on_target = self.check_contact(self.bottle.contact_geoms, [self._tgt_coll]) + bottle_upright = check_obj_upright(self, "bottle", threshold=0.8, symmetric=True) + return bottle_on_target and bottle_upright + + # --- runtime table height --- + def _table_top_z(self) -> float: + base_z = float(self.sim.data.body_xpos[self.table_body_id][2]) + top_offset_z = float(self.mujoco_objects[0].top_offset[2]) + return base_z + top_offset_z + + def _reset_internal(self): + LocoManipulationEnv._reset_internal(self) + + if not self.deterministic_reset: + # Sample fixture XY, compute Z from current table pose + x_src = self.rng.uniform(*self._X_SRC_RANGE) + y_src = self.rng.uniform(*self._Y_SRC_RANGE) + + x_tgt = self.rng.uniform(*self._X_TGT_RANGE) + y_tgt = self.rng.uniform(*self._Y_TGT_RANGE) + # y_tgt = self.rng.uniform(*self._Y_TGT_RANGE) + + z_top = self._table_top_z() # dynamic table top + src_pos = np.array([x_src, y_src, z_top]) + tgt_pos = np.array([x_tgt, y_tgt, z_top]) + + # Reset fixture body poses (static bodies): write to model; MuJoCo will use it after forward() + self.sim.model.body_pos[self.src_fixture_id] = src_pos + self.sim.model.body_pos[self.tgt_fixture_id] = tgt_pos + + # Place bottle on source fixture: top of fixture + bottle half-height + tiny clearance + bottle_z = z_top + self._FIXTURE_HALF_SIZE[2] + self._BOTTLE_HALF_HEIGHT + 0.002 + qpos = self.sim.data.get_joint_qpos("bottle_joint").copy() + qpos[:3] = np.array([x_src, y_src, bottle_z]) + qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # upright + self.sim.data.set_joint_qpos("bottle_joint", qpos) + + self._randomize_table_texture() + RobotPoseRandomizer.set_pose(self, (-0.3, -0.16), (-0.2, 0.2), (-np.pi / 6, np.pi / 6)) + + # --- distance via MuJoCo --- + def _min_signed_distance_mj(self, geoms_a: list[str], geoms_b: list[str]) -> float: + model, data = self.sim.model, self.sim.data + dmin = np.inf + fromto = np.empty(6, dtype=np.float64) + a_ids = [model.geom_name2id(n) for n in geoms_a] + b_ids = [model.geom_name2id(n) for n in geoms_b] + for ga in a_ids: + for gb in b_ids: + dist = mujoco.mj_geomDistance( + model._model, data._data, ga, gb, self.DISTMAX_SCAN_M + 0.01, fromto + ) + dmin = min(dmin, float(dist)) + return dmin + + def get_subtask_term_signals(self) -> dict[str, int]: + """ + 1 iff (no contact between bottle and source fixture) AND + (min signed distance > DISTMAX_SCAN_M). + """ + in_contact = self.check_contact(self.bottle.contact_geoms, [self._src_coll]) + min_dist = self._min_signed_distance_mj(self.bottle.contact_geoms, [self._src_coll]) + return { + "obj_off_source_fixture": int((not in_contact) and (min_dist > self.DISTMAX_SCAN_M)) + } + + def get_object(self) -> dict: + return dict( + bottle=dict(obj_name=self.objects["bottle"]["name"], obj_type="body"), + source_fixture=dict(obj_name=self._src_body, obj_type="body"), + target_fixture=dict(obj_name=self._tgt_body, obj_type="body"), + ) + + @staticmethod + def task_config() -> dict: + task = DexMGConfigHelper.AttrDict() + # Subtask 1: pick (leave source fixture) + task.task_spec_0.subtask_1 = dict( + object_ref="bottle", + subtask_term_signal="obj_off_source_fixture", + subtask_term_offset_range=(5, 10), + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + # Subtask 2: place on target fixture + task.task_spec_0.subtask_2 = dict( + object_ref="target_fixture", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + # Default filler for task_spec_1, mirroring other tasks + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + +class PnPBottleFixtureToFixtureSourceDemo(PnPBottleFixtureToFixture): + """ + Environment for collecting source demo for PnPBottleFixtureToFixture tasks. + """ + + _X_SRC_RANGE = (0.375, 0.375) + _X_TGT_RANGE = (0.375, 0.375) + _Y_SRC_RANGE = (-0.15, -0.15) + _Y_TGT_RANGE = (0.1, 0.1) + _SRC_FIXTURE_VISIBLE = False + _TGT_FIXTURE_VISIBLE = True + + +class PnPBottleShelfToTable(PnPBottleFixtureToFixture): + """ + Task: Robot picks up bottle from a fixture on a shelf and places it on a fixture on a table. + + Initialization: bottle rests on a source fixture on the shelf. + Target: place bottle on target fixture on the table. + """ + + # Adjust ranges for shelf-to-table layout + _X_SRC_RANGE = (-0.05, 0.05) # Shelf position range + _X_TGT_RANGE = (-0.05 - 0.2, 0.05 - 0.2) # Table position range + # TODO: could be better to have some 'center' specified here + _Y_SRC_RANGE = (-0.05, 0.05) # Shelf position range + _Y_TGT_RANGE = (-0.05, 0.05) # Table position range + _SRC_FIXTURE_VISIBLE = True + _TGT_FIXTURE_VISIBLE = True + _FIXTURE_HALF_SIZE = np.array([0.05, 0.05, 0.001]) + + # Shelf height constants (from PnPBottleShelf) + # _SHELF_HEIGHT = 0.386660 # Level 2 of shelf from the original shelf environment + _SHELF_HEIGHT = 0.753321 + 0.015 # Level 3 of shelf from the original shelf environment + + def _load_model(self): + # Create both shelf and table + self.mujoco_objects = [ + self._create_table("table_body", [0.5, 0.6, 0], [0, 0, np.pi / 2]), + create_shelf(pos=[0.8, -0.4, 0], euler=[0, 0, np.pi / 2]), + ] + + LocoManipulationEnv._load_model(self) + + self.bottle = self._create_bottle() + self._create_fixture(self._SRC_NAME, visible=self._SRC_FIXTURE_VISIBLE, rgb="1 0 0") + self._create_fixture(self._TGT_NAME, visible=self._TGT_FIXTURE_VISIBLE, rgb="0 1 0") + self._src_body = f"{self._SRC_NAME}_body" + self._tgt_body = f"{self._TGT_NAME}_body" + self._src_coll = f"{self._SRC_NAME}_collider" + self._tgt_coll = f"{self._TGT_NAME}_collider" + + def _setup_references(self): + super()._setup_references() + # Add reference to shelf + self.shelf_body_id = self.sim.model.body_name2id("shelf_body_main") + + def _shelf_top_z(self) -> float: + """Get the Z coordinate of the shelf top surface""" + # Use the same shelf height as in PnPBottleShelf + return self._SHELF_HEIGHT + + def _shelf_xy(self) -> tuple[float, float]: + """Get the XY coordinates of the shelf""" + return self.sim.data.body_xpos[self.shelf_body_id][:2] + + def _table_xy(self) -> tuple[float, float]: + """Get the XY coordinates of the table""" + return self.sim.data.body_xpos[self.table_body_id][:2] + + def _reset_internal(self): + LocoManipulationEnv._reset_internal(self) + + if not self.deterministic_reset: + # Sample fixture XY positions + x_src = self.rng.uniform(*self._X_SRC_RANGE) + y_src = self.rng.uniform(*self._Y_SRC_RANGE) + + x_tgt = self.rng.uniform(*self._X_TGT_RANGE) + y_tgt = self.rng.uniform(*self._Y_TGT_RANGE) + + # Source fixture on shelf + z_shelf = self._shelf_top_z() + x_shelf, y_shelf = self._shelf_xy() + src_pos = np.array([x_src, y_src, z_shelf]) + src_pos += np.array([x_shelf, y_shelf, 0]) + + # table pos + # Target fixture on table + z_table = self._table_top_z() + x_table, y_table = self._table_xy() + tgt_pos = np.array([x_tgt, y_tgt, z_table]) + tgt_pos += np.array([x_table, y_table, 0]) + + # Reset fixture body poses + self.sim.model.body_pos[self.src_fixture_id] = src_pos + self.sim.model.body_pos[self.tgt_fixture_id] = tgt_pos + + # Place bottle on source fixture (shelf): top of fixture + bottle half-height + clearance + bottle_z = z_shelf + self._FIXTURE_HALF_SIZE[2] + self._BOTTLE_HALF_HEIGHT + 0.002 + qpos = self.sim.data.get_joint_qpos("bottle_joint").copy() + qpos[:3] = np.array([src_pos[0], src_pos[1], bottle_z]) + qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # upright + self.sim.data.set_joint_qpos("bottle_joint", qpos) + + self._randomize_table_texture() + RobotPoseRandomizer.set_pose(self, (-0.3, -0.16), (-0.2, 0.2), (-np.pi / 6, np.pi / 6)) + + def _randomize_table_texture(self): + """Randomize textures for the table (shelf texture is static)""" + # Only randomize the table texture (index 1), not the shelf + table = self.mujoco_objects[1] + randomize_materials_rgba( + rng=self.rng, mjcf_obj=table, gradient=self.TABLE_GRADIENT, linear=True + ) + + +class PnPBottleTableToTable(PnPBottle): + def _load_model(self): + # Create both the original table and the target table + self.mujoco_objects = [ + self._create_table("table_body", [0.5, 0, 0], [0, 0, np.pi / 2]), + self._create_table("target_table_body", [0.5, 1.2, 0], [0, 0, np.pi / 2]), + ] + + LocoManipulationEnv._load_model(self) + + self.bottle = self._create_bottle() + + def _setup_references(self): + super()._setup_references() + + # Add reference to target table - note the _main suffix + self.target_table_body_id = self.sim.model.body_name2id("target_table_body_main") + + def _check_success(self): + """Check if bottle is successfully placed on the target table""" + bottle_on_table = self.check_contact(self.bottle.contact_geoms, self.mujoco_objects[1]) + bottle_is_upright = check_obj_upright(self, "bottle", threshold=0.8, symmetric=True) + return bottle_on_table and bottle_is_upright + + def _randomize_table_texture(self): + """Randomize textures for both tables""" + # Randomize original table + original_table = self.mujoco_objects[0] + randomize_materials_rgba( + rng=self.rng, mjcf_obj=original_table, gradient=self.TABLE_GRADIENT, linear=True + ) + + # Randomize target table + target_table = self.mujoco_objects[1] + randomize_materials_rgba( + rng=self.rng, mjcf_obj=target_table, gradient=self.TABLE_GRADIENT, linear=True + ) + + def get_object(self): + return dict( + bottle=dict(obj_name=self.objects["bottle"]["name"], obj_type="body"), + target_table=dict(obj_name="target_table_body_main", obj_type="body"), + ) + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + task.task_spec_0.subtask_1 = dict( + object_ref="bottle", + subtask_term_signal="obj_off_table", + subtask_term_offset_range=(5, 10), + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + # Second subtask for placing on target table + task.task_spec_0.subtask_2 = dict( + object_ref="target_table", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + def get_subtask_term_signals(self): + """ + Retrieve signals used to define subtask termination conditions. + + Returns: + dict: Dictionary mapping signal names to their current values + """ + signals = dict() + + obj_z = self.sim.data.body_xpos[self.obj_body_id["bottle"]][2] + target_table_pos = self.sim.data.body_xpos[self.target_table_body_id] + target_table_z = target_table_pos[2] + self.mujoco_objects[1].top_offset[2] + + th = 0.15 + signals["obj_off_table"] = int(obj_z - target_table_z > th) + + return signals + + +class PickBottleGround(PnPBottle): + """ + Pick-and-Place Bottle environment with bottle initialized on the ground. + """ + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + LocoManipulationEnv._reset_internal(self) + + if not self.deterministic_reset: + # Base position on ground (z=0.075 is bottle radius) + self._randomize_bottle_placement(base_pos=np.ndarray([0.4, 0, 0.075])) + self._randomize_table_texture() + + def _randomize_table_texture(self): + pass + + def _check_success(self): + check_grasp = self._check_grasp(self.robots[0].gripper["right"], "bottle") + + bottle_z = self.sim.data.body_xpos[self.obj_body_id["bottle"]][2] + ground_z = 0 + check_bottle_in_air = bottle_z > ground_z + 0.2 + # check bottle and table collision + # check_bottle_in_air = not self.check_contact("bottle", "table") + return check_grasp and check_bottle_in_air + + def _load_model(self): + self.mujoco_objects = [] + + super(PnPBottle, self)._load_model() + self._create_bottle() + + +class PickBottles(PnPBottle): + BOTTLE_POS_RANGE_X = (-0.08, 0.04) + BOTTLE_POS_RANGE_Y = (-0.04, 0.04) + + COLOURS: list[list[float]] = [[0.3, 0.7, 0.8], [0.8, 0.4, 0.3]] + BOTTLES_COUNT = 2 + Y_OFFSET_STEP = 0.1 + + @staticmethod + def _get_bottle_names() -> list[str]: + return [f"bottle_{i}" for i in range(PickBottles.BOTTLES_COUNT)] + + def _load_model(self): + self.mujoco_objects = [self._create_table("table_body", [0.5, 0, 0], [0, 0, np.pi / 2])] + + LocoManipulationEnv._load_model(self) + + self.bottles = self._create_bottles() + + def _create_bottles(self) -> list[PrimitiveBottle]: + bottles = [] + for i, name in enumerate(self._get_bottle_names()): + rgb = self.COLOURS[i % len(self.COLOURS)] + bottles.append(self._create_bottle(name=name, rgb=rgb)) + return bottles + + def _reset_internal(self): + LocoManipulationEnv._reset_internal(self) + + n = len(self.bottles) + offsets = np.arange(n) - (n - 1) / 2.0 + for i, bottle in enumerate(self.bottles): + self._randomize_bottle_placement( + name=bottle.name, + base_pos=self.DEFAULT_BOTTLE_POS + + np.array([0, self.Y_OFFSET_STEP * offsets[i], 0]), + ) + self._randomize_table_texture() + + def _check_success(self): + for bottle in self.bottles: + check_grasp = self._check_grasp( + self.robots[0].gripper["right"], bottle.contact_geoms + ) or self._check_grasp(self.robots[0].gripper["left"], bottle.contact_geoms) + bottle_z = self.sim.data.body_xpos[self.obj_body_id[bottle.name]][2] + table_z = self.mujoco_objects[0].top_offset[2] + check_bottle_in_air = bottle_z > table_z + 0.2 + if check_grasp and check_bottle_in_air: + continue + return False + return True + + def get_object(self): + result = {} + for bottle in self.bottles: + result[bottle.name] = dict(obj_name=self.objects[bottle.name]["name"], obj_type="body") + return result + + def get_subtask_term_signals(self): + signals = dict() + for bottle in self.bottles: + signals[f"grasp_{bottle.name}"] = int( + self._check_grasp(self.robots[0].gripper["right"], bottle.contact_geoms) + or self._check_grasp(self.robots[0].gripper["left"], bottle.contact_geoms) + ) + return signals + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + bottle_names = PickBottles._get_bottle_names() + assert len(bottle_names) == 2 + for i, name in enumerate(bottle_names): + subtask = dict( + object_ref=name, + subtask_term_signal=f"grasp_{name}", + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + spec_attr = f"task_spec_{i}" + setattr(getattr(task, spec_attr), "subtask_1", subtask) + return task.to_dict() + + +class NavPickBottles(PickBottles): + """ + PickBottles environment with robot position randomized further from table at reset. + """ + + def _reset_internal(self): + super()._reset_internal() + + if not self.deterministic_reset: + RobotPoseRandomizer.set_pose(self, (-0.3, -0.16), (-0.2, 0.2), (-np.pi / 6, np.pi / 6)) + + +class PnPBottlesTableToTable(PickBottles): + def _load_model(self): + self.mujoco_objects = [ + self._create_table("table_body", [0.5, 0, 0], [0, 0, np.pi / 2]), + self._create_table("target_table_body", [0.5, 1.2, 0], [0, 0, np.pi / 2]), + ] + + LocoManipulationEnv._load_model(self) + + self.bottles = self._create_bottles() + + def _check_success(self): + """Check if bottles are successfully placed on the target table""" + for bottle in self.bottles: + bottle_on_table = self.check_contact(bottle.contact_geoms, self.mujoco_objects[1]) + bottle_is_upright = check_obj_upright(self, bottle.name, threshold=0.8, symmetric=True) + if bottle_on_table and bottle_is_upright: + continue + return False + return True + + def _setup_references(self): + super()._setup_references() + + # Add reference to target table - note the _main suffix + self.target_table_body_id = self.sim.model.body_name2id("target_table_body_main") + + def get_object(self): + result = super().get_object() + result["target_table"] = dict(obj_name="target_table_body_main", obj_type="body") + return result + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + + bottle_names = PickBottles._get_bottle_names() + assert len(bottle_names) == 2 + for i, name in enumerate(bottle_names): + + # pick subtask per arm + subtask = dict( + object_ref=name, + subtask_term_signal=f"{name}_off_table", + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + spec_attr = f"task_spec_{i}" + setattr(getattr(task, spec_attr), "subtask_1", subtask) + + # place subtask per arm + subtask = dict( + object_ref="target_table", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + spec_attr = f"task_spec_{i}" + setattr(getattr(task, spec_attr), "subtask_2", subtask) + + return task.to_dict() + + def get_subtask_term_signals(self): + signals = dict() + for bottle in self.bottles: + obj_z = self.sim.data.body_xpos[self.obj_body_id[bottle.name]][2] + target_table_pos = self.sim.data.body_xpos[self.target_table_body_id] + target_table_z = target_table_pos[2] + self.mujoco_objects[1].top_offset[2] + th = 0.15 + signals[f"{bottle.name}_off_table"] = int(obj_z - target_table_z > th) + return signals diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip.py new file mode 100644 index 0000000..56661a3 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip.py @@ -0,0 +1,83 @@ +from abc import abstractmethod +from typing import Optional + +from robocasa.environments.locomanipulation.base import LocoManipulationEnv +from robocasa.models.scenes import GroundArena +from robocasa.models.scenes.factory_arena import FactoryArena +from robocasa.utils.scene.configs import SceneConfig, SceneScaleConfig +from robocasa.utils.scene.scene import Scene, SceneObject +from robocasa.utils.scene.success_criteria import SuccessCriteria + + +class LMEnvBase(LocoManipulationEnv): + SCENE_SCALE = SceneScaleConfig() + + def __init__( + self, + translucent_robot: bool = False, + use_object_obs: bool = False, + scene_scale: Optional[SceneScaleConfig] = None, + *args, + **kwargs, + ): + self.scene_scale = scene_scale or self.SCENE_SCALE + super().__init__(translucent_robot, use_object_obs, *args, **kwargs) + + def _load_model(self): + self.scene = Scene(self, self._get_env_config(), self.scene_scale) + self.mujoco_objects = self.scene.mujoco_objects + + super()._load_model() + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + super()._reset_internal() + + if not self.deterministic_reset: + self.scene.reset() + + def _setup_references(self): + super()._setup_references() + + self.obj_body_id = {} + for obj in self.mujoco_objects: + self.obj_body_id[obj.name] = self.sim.model.body_name2id(obj.root_body) + + def _get_env_config(self) -> SceneConfig: + return SceneConfig( + objects=self._get_objects(), + success=self._get_success_criteria(), + instruction=self._get_instruction(), + ) + + @abstractmethod + def _get_objects(self) -> list[SceneObject]: + raise NotImplementedError + + @abstractmethod + def _get_success_criteria(self) -> SuccessCriteria: + raise NotImplementedError + + @abstractmethod + def _get_instruction(self) -> str: + raise NotImplementedError + + def _check_success(self): + return self.scene.success() + + def get_ep_meta(self): + ep_meta = super().get_ep_meta() + ep_meta["lang"] = self.scene.instruction + return ep_meta + + +# noinspection PyAbstractClass +class LMSimpleEnv(LMEnvBase): + MUJOCO_ARENA_CLS = GroundArena + + +# noinspection PyAbstractClass +class LMFactoryEnv(LMEnvBase): + MUJOCO_ARENA_CLS = FactoryArena diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_basic.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_basic.py new file mode 100644 index 0000000..c54e67c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_basic.py @@ -0,0 +1,576 @@ +import numpy as np +from robocasa.environments.locomanipulation.base import RobotPoseRandomizer +from robocasa.environments.locomanipulation.locomanip import LMSimpleEnv +from robocasa.utils.dexmg_utils import DexMGConfigHelper +from robocasa.utils.scene.configs import ( + ObjectConfig, + ReferenceConfig, + SamplingConfig, + SceneScaleConfig, +) +from robocasa.utils.scene.scene import SceneObject +from robocasa.utils.scene.success_criteria import ( + AllCriteria, + AnyCriteria, + IsGrasped, + IsInContact, + IsPositionInRange, + IsRobotInRange, + IsUpright, + NotCriteria, + SuccessCriteria, +) +from robocasa.utils.visuals_utls import Gradient, randomize_materials_rgba + + +class LMPickBottle(LMSimpleEnv, DexMGConfigHelper): + SCENE_SCALE = SceneScaleConfig(planar_scale=1.0) + + TABLE_GRADIENT: Gradient = Gradient( + np.array([0.68, 0.34, 0.07, 1.0]), np.array([1.0, 1.0, 1.0, 1.0]) + ) + LIFT_OFFSET = 0.1 + + def _get_objects(self) -> list[SceneObject]: + self.table = SceneObject( + ObjectConfig( + name="table", + mjcf_path="objects/omniverse/locomanip/lab_table/model.xml", + scale=1.0, + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([0.5, 0, 0]), + rotation=np.array([np.pi * 0.5, np.pi * 0.5]), + ), + ) + ) + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.08, 0.04]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference_pos=np.array([0.4, 0, self.table.mj_obj.top_offset[2]]), + ), + ) + ) + return [self.table, self.bottle] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria( + IsGrasped(self.bottle, "right"), + IsPositionInRange(self.bottle, 2, self.table.mj_obj.top_offset[2] + self.LIFT_OFFSET), + ) + + def _get_instruction(self) -> str: + return "Pick up the bottle." + + def get_object(self): + return dict( + bottle=dict(obj_name=self.bottle.mj_obj.root_body, obj_type="body"), + ) + + def get_subtask_term_signals(self): + signals = dict() + signals["grasp_bottle"] = int( + self._check_grasp(self.robots[0].gripper["right"], self.bottle.mj_obj.contact_geoms) + ) + return signals + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + task.task_spec_0.subtask_1 = dict( + object_ref="bottle", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + def _reset_internal(self): + super()._reset_internal() + + if not self.deterministic_reset: + self._randomize_table_rgba() + + def _randomize_table_rgba(self): + randomize_materials_rgba( + rng=self.rng, mjcf_obj=self.table.mj_obj, gradient=self.TABLE_GRADIENT, linear=True + ) + + +class LMPickBottleHigh(LMPickBottle): + TABLE_OFFSET = 0.1 + + def _get_objects(self) -> list[SceneObject]: + self.table = SceneObject( + ObjectConfig( + name="table", + mjcf_path="objects/omniverse/locomanip/lab_table/model.xml", + scale=1.0, + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([0.5, 0, self.TABLE_OFFSET]), + rotation=np.array([np.pi * 0.5, np.pi * 0.5]), + ), + ) + ) + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.08, 0.04]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference_pos=np.array( + [0.4, 0, self.TABLE_OFFSET + self.table.mj_obj.top_offset[2]] + ), + reference=ReferenceConfig(obj=self.table), + ), + ) + ) + return [self.table, self.bottle] + + +class LMNavPickBottle(LMPickBottle): + def _reset_internal(self): + super()._reset_internal() + + if not self.deterministic_reset: + RobotPoseRandomizer.set_pose(self, (-0.3, -0.16), (-0.2, 0.2), (-np.pi / 6, np.pi / 6)) + + def _get_instruction(self) -> str: + return "Walk forward and pick up the bottle from the table." + + +class LMPickBottleGround(LMPickBottle): + def _get_objects(self) -> list[SceneObject]: + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.08, 0.04]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference_pos=np.array( + [0.4, 0, 0.075] + ), # Base position on ground (z=0.075 is bottle radius) + ), + ) + ) + return [self.bottle] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria( + IsGrasped(self.bottle, "right"), + IsPositionInRange(self.bottle, 2, self.LIFT_OFFSET, 10), + ) + + def _randomize_table_rgba(self): + pass + + +class LMPnPBottle(LMPickBottle): + LIFT_OFFSET = 0.15 + + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.table_target = SceneObject( + ObjectConfig( + name="table_target", + mjcf_path="objects/omniverse/locomanip/lab_table/model.xml", + scale=1.0, + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([0.5, 1.2, 0]), + rotation=np.array([np.pi * 0.5, np.pi * 0.5]), + ), + ) + ) + return [self.table, self.table_target, self.bottle] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria( + IsUpright(self.bottle, symmetric=True), IsInContact(self.bottle, self.table_target) + ) + + def _get_instruction(self) -> str: + return "Pick up the bottle and place it on the other table." + + def get_object(self): + return dict( + bottle=dict(obj_name=self.bottle.mj_obj.root_body, obj_type="body"), + target_table=dict(obj_name=self.table_target.mj_obj.root_body, obj_type="body"), + ) + + def get_subtask_term_signals(self): + obj_z = self.sim.data.body_xpos[self.obj_body_id(self.bottle.mj_obj.name)][2] + target_table_pos = self.sim.data.body_xpos[self.obj_body_id(self.table_target.mj_obj.name)] + target_table_z = target_table_pos[2] + self.table_target.mj_obj.top_offset[2] + return dict(obj_off_table=int(obj_z - target_table_z > self.LIFT_OFFSET)) + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + task.task_spec_0.subtask_1 = dict( + object_ref="bottle", + subtask_term_signal="obj_off_table", + subtask_term_offset_range=(5, 10), + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + # Second subtask for placing on target table + task.task_spec_0.subtask_2 = dict( + object_ref="target_table", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + def _randomize_table_rgba(self): + for table in [self.table_target, self.table]: + randomize_materials_rgba( + rng=self.rng, mjcf_obj=table.mj_obj, gradient=self.TABLE_GRADIENT, linear=True + ) + + +class LMPickMultipleBottles(LMPickBottle): + BOTTLE_COLOURS = [(0.3, 0.7, 0.8, 1.0), (0.8, 0.4, 0.3, 1.0)] + BOTTLES_COUNT = 2 + Y_OFFSET_STEP = 0.1 + + def _get_objects(self) -> list[SceneObject]: + self.table = SceneObject( + ObjectConfig( + name="table", + mjcf_path="objects/omniverse/locomanip/lab_table/model.xml", + scale=1.0, + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([0.5, 0, 0]), + rotation=np.array([np.pi * 0.5, np.pi * 0.5]), + ), + ) + ) + + self.bottles = [] + offsets = np.arange(self.BOTTLES_COUNT) - (self.BOTTLES_COUNT - 1) / 2.0 + for i in range(self.BOTTLES_COUNT): + reference_pos = np.array([0.4, 0, self.table.mj_obj.top_offset[2]]) + reference_pos += np.array([0, self.Y_OFFSET_STEP * offsets[i], 0]) + bottle = SceneObject( + ObjectConfig( + name=f"bottle_{i}", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.08, 0.04]), + y_range=np.array([-0.04, 0.04]), + rotation=np.array([-np.pi, np.pi]), + reference_pos=reference_pos, + ), + rgba=self.BOTTLE_COLOURS[i % len(self.BOTTLE_COLOURS)], + ) + ) + self.bottles.append(bottle) + return [self.table, *self.bottles] + + def _get_success_criteria(self) -> SuccessCriteria: + criteria = [] + for bottle in self.bottles: + criteria.append(AnyCriteria(IsGrasped(bottle, "right"), IsGrasped(bottle, "left"))) + criteria.append( + IsPositionInRange(bottle, 2, self.table.mj_obj.top_offset[2] + self.LIFT_OFFSET, 10) + ) + return AllCriteria(*criteria) + + def _get_instruction(self) -> str: + return "Pick up bottles." + + def get_object(self): + return { + bottle.mj_obj.name: dict(obj_name=bottle.mj_obj.root_body, obj_type="body") + for bottle in self.bottles + } + + def get_subtask_term_signals(self): + return { + f"grasp_{bottle.mj_obj.name}": int( + self._check_grasp(self.robots[0].gripper["right"], bottle.mj_obj) + or self._check_grasp(self.robots[0].gripper["left"], bottle.mj_obj) + ) + for bottle in self.bottles + } + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + for i in range(LMPickMultipleBottles.BOTTLES_COUNT): + subtask = dict( + object_ref=f"bottle_{i}", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + setattr(task.task_spec_0, f"subtask_{i+1}", subtask) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + +class LMPnPMultipleBottles(LMPickMultipleBottles): + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.table_target = SceneObject( + ObjectConfig( + name="table_target", + mjcf_path="objects/omniverse/locomanip/lab_table/model.xml", + scale=1.0, + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([0.5, 1.2, 0]), + rotation=np.array([np.pi * 0.5, np.pi * 0.5]), + ), + ) + ) + return [self.table, self.table_target, *self.bottles] + + def _get_success_criteria(self) -> SuccessCriteria: + criteria = [ + AllCriteria(IsInContact(bottle, self.table_target), IsUpright(bottle, symmetric=True)) + for bottle in self.bottles + ] + return AllCriteria(*criteria) + + def _get_instruction(self) -> str: + return "Pick up bottles from one table and place it on the other." + + @staticmethod + def task_config(): + task = DexMGConfigHelper.AttrDict() + for i in range(LMPnPMultipleBottles.BOTTLES_COUNT): + bottle_name = f"bottle_{i}" + subtask = dict( + object_ref=bottle_name, + subtask_term_signal=f"{bottle_name}_off_table", + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + setattr(task.task_spec_0, f"subtask_{i+1}", subtask) + # Next subtask for placing on target table + task.task_spec_0.subtask_3 = dict( + object_ref="target_table", + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + task.task_spec_1.subtask_1 = dict( + object_ref=None, + subtask_term_signal=None, + subtask_term_offset_range=None, + selection_strategy="random", + selection_strategy_kwargs=None, + action_noise=0.05, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + return task.to_dict() + + def get_subtask_term_signals(self): + signals = dict() + for bottle in self.bottles: + obj_z = self.sim.data.body_xpos[self.obj_body_id(self.bottle.mj_obj.name)][2] + target_table_pos = self.sim.data.body_xpos[ + self.obj_body_id(self.table_target.mj_obj.name) + ] + target_table_z = target_table_pos[2] + self.table_target.mj_obj.top_offset[2] + signals[f"{bottle.mj_obj.name}_off_table"] = int( + obj_z - target_table_z > self.LIFT_OFFSET + ) + return signals + + def _randomize_table_rgba(self): + for table in [self.table_target, self.table]: + randomize_materials_rgba( + rng=self.rng, mjcf_obj=table.mj_obj, gradient=self.TABLE_GRADIENT, linear=True + ) + + +class LMPickBottleShelf(LMPickBottle): + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.shelf = SceneObject( + ObjectConfig( + name="shelf", + mjcf_path="objects/omniverse/locomanip/lab_shelf/model.xml", + static=True, + sampler_config=SamplingConfig( + rotation=np.array([np.pi / 2, np.pi / 2]), + reference_pos=np.array([0.9, 0, 0]), + ), + ) + ) + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.14, -0.06]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference=ReferenceConfig(self.shelf, spawn_id=2), + ), + ) + ) + return [self.shelf, self.bottle] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria( + IsGrasped(self.bottle, "right"), + NotCriteria(IsInContact(self.bottle, self.shelf)), + ) + + +class LMNavPickBottleShelf(LMPickBottleShelf): + ROBOT_DISTANCE_THRESHOLD = 1.0 + + def _reset_internal(self): + super()._reset_internal() + if not self.deterministic_reset: + RobotPoseRandomizer.set_pose(self, (-0.1, 0.1), (-0.1, 0.1), (-np.pi / 6, np.pi / 6)) + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria( + NotCriteria(IsRobotInRange(self.shelf, self.ROBOT_DISTANCE_THRESHOLD, True)), + IsGrasped(self.bottle, "right"), + NotCriteria(IsInContact(self.bottle, self.shelf)), + ) + + def _get_instruction(self) -> str: + return "Pick up the bottle from the shelf and move backward away from it." + + +class LMPickBottleShelfLow(LMPickBottleShelf): + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.14, -0.06]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference=ReferenceConfig(self.shelf, spawn_id=1), + ), + ) + ) + return [self.shelf, self.bottle] + + +class LMNavPickBottleShelfLow(LMNavPickBottleShelf): + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.bottle = SceneObject( + ObjectConfig( + name="bottle", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.14, -0.06]), + y_range=np.array([-0.08, 0.08]), + rotation=np.array([-np.pi, np.pi]), + reference=ReferenceConfig(self.shelf, spawn_id=1), + ), + ) + ) + return [self.shelf, self.bottle] diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_pnp.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_pnp.py new file mode 100644 index 0000000..e526417 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/environments/locomanipulation/locomanip_pnp.py @@ -0,0 +1,99 @@ +import numpy as np +from robocasa.environments.locomanipulation.locomanip import LMFactoryEnv +from robocasa.utils.scene.configs import ( + ObjectConfig, + ReferenceConfig, + SamplingConfig, + SceneHandedness, + SceneScaleConfig, +) +from robocasa.utils.scene.scene import SceneObject +from robocasa.utils.scene.success_criteria import ( + AllCriteria, + IsInContact, + IsUpright, + SuccessCriteria, +) + + +class LMBottlePnP(LMFactoryEnv): + SCENE_SCALE = SceneScaleConfig(planar_scale=(1, 1), handedness=SceneHandedness.RIGHT) + + def _get_objects(self) -> list[SceneObject]: + self.table_target = SceneObject( + ObjectConfig( + name="table_target", + mjcf_path="objects/omniverse/locomanip/factory_ergo_table/model.xml", + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([1.2, 0.8, 0]), + rotation=np.array([np.pi, np.pi]), + ), + ) + ) + self.table_origin = SceneObject( + ObjectConfig( + name="table_origin", + mjcf_path="objects/omniverse/locomanip/factory_ergo_table/model.xml", + static=True, + sampler_config=SamplingConfig( + x_range=np.array([-0.02, 0.02]), + y_range=np.array([-0.02, 0.02]), + reference_pos=np.array([1.2, -0.8, 0]), + rotation=np.array([np.pi, np.pi]), + ), + ) + ) + self.bottle = SceneObject( + ObjectConfig( + name="obj", + mjcf_path="objects/omniverse/locomanip/jug_a01/model.xml", + static=False, + scale=0.6, + sampler_config=SamplingConfig( + x_range=np.array([-0.4, -0.35]), + y_range=np.array([-0.1, 0.1]), + rotation=np.array([-np.pi, np.pi]), + reference=ReferenceConfig(self.table_origin), + ), + ) + ) + return [self.table_origin, self.table_target, self.bottle] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria(IsInContact(self.bottle, self.table_target), IsUpright(self.bottle)) + + def _get_instruction(self) -> str: + return "Pick up the bottle from one table and place it on the other." + + +class LMBoxPnP(LMBottlePnP): + SCENE_SCALE = SceneScaleConfig(planar_scale=(1, 1), handedness=SceneHandedness.RIGHT) + + def _get_objects(self) -> list[SceneObject]: + super()._get_objects() + self.box = SceneObject( + ObjectConfig( + name="obj", + mjcf_path="objects/omniverse/locomanip/cardbox_a1/model.xml", + static=False, + scale=0.7, + density=1, + friction=(2, 1, 1), + sampler_config=SamplingConfig( + x_range=np.array([-0.35, -0.3]), + y_range=np.array([-0.1, 0.1]), + rotation=np.array([np.pi * 0.9, np.pi * 1.1]), + reference=ReferenceConfig(self.table_origin), + ), + ) + ) + return [self.table_origin, self.table_target, self.box] + + def _get_success_criteria(self) -> SuccessCriteria: + return AllCriteria(IsInContact(self.box, self.table_target), IsUpright(self.box)) + + def _get_instruction(self) -> str: + return "Pick up the box from one table and place it on the other." diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc.json b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc.json new file mode 100644 index 0000000..bb76a83 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc.json @@ -0,0 +1,113 @@ +{ + "type": "HYBRID_WHOLE_BODY_MINK_IK", + "composite_controller_specific_configs": { + "ref_name": ["gripper0_right_grip_site", "gripper0_left_grip_site"], + "interpolation": null, + "actuation_part_names": ["torso", "left", "right"], + "external_part_names": ["legs"], + "max_dq": 4, + "ik_pseudo_inverse_damping": 5e-2, + "ik_integration_dt": 1e-1, + "ik_input_type": "absolute", + "ik_input_ref_frame": "base", + "ik_input_rotation_repr": "axis_angle", + "verbose": false, + "ik_posture_weights": { + "robot0_waist_yaw_joint": 100.0, + "robot0_waist_roll_joint": 200.0, + "robot0_waist_pitch_joint": 400.0, + "robot0_left_shoulder_pitch_joint": 4.0, + "robot0_left_shoulder_roll_joint": 3.0, + "robot0_left_shoulder_yaw_joint": 2.0, + "robot0_left_elbow_joint": 1.0, + "robot0_right_shoulder_pitch_joint": 4.0, + "robot0_right_shoulder_roll_joint": 3.0, + "robot0_right_shoulder_yaw_joint": 2.0, + "robot0_right_elbow_joint": 1.0 + }, + "ik_hand_pos_cost": 10.0, + "ik_hand_ori_cost": 5, + "use_joint_angle_action_input": false + }, + "body_parts": { + "legs": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2], + "kv": 0, + "kp": [150, 150, 150, 300, 40, 40, 150, 150, 150, 200, 40, 40], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "use_torque_compensation": false, + "desired_torque_as_acceleration": false + }, + "arms": { + "left": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [5.0, 5.0, 2.0, 2.0, 2.0, 2.0, 2.0], + "kp": [100, 100, 40, 40, 20, 20, 20], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + }, + "use_torque_compensation": false, + "desired_torque_as_acceleration": false + }, + "right": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [5.0, 5.0, 2.0, 2.0, 2.0, 2.0, 2.0], + "kp": [100, 100, 40, 40, 20, 20, 20], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + }, + "use_torque_compensation": false, + "desired_torque_as_acceleration": false + } + }, + "torso": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": 5.0, + "kp": 250.0, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "use_torque_compensation": false, + "desired_torque_as_acceleration": false + }, + "base": { + "type": "JOINT_VELOCITY_AND_POSITION", + "interpolation": "null" + } + } +} diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc_gc.json b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc_gc.json new file mode 100644 index 0000000..7362f22 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/examples/third_party_controller/default_mink_ik_g1_gear_wbc_gc.json @@ -0,0 +1,117 @@ +{ + "type": "HYBRID_WHOLE_BODY_MINK_IK", + "composite_controller_specific_configs": { + "ref_name": ["gripper0_right_grip_site", "gripper0_left_grip_site"], + "interpolation": null, + "actuation_part_names": ["torso", "left", "right"], + "external_part_names": ["legs"], + "max_dq": 4, + "ik_pseudo_inverse_damping": 5e-2, + "ik_integration_dt": 1e-1, + "ik_input_type": "absolute", + "ik_input_ref_frame": "base", + "ik_input_rotation_repr": "axis_angle", + "verbose": false, + "ik_posture_weights": { + "robot0_waist_yaw_joint": 100.0, + "robot0_waist_roll_joint": 200.0, + "robot0_waist_pitch_joint": 400.0, + "robot0_left_shoulder_pitch_joint": 4.0, + "robot0_left_shoulder_roll_joint": 3.0, + "robot0_left_shoulder_yaw_joint": 2.0, + "robot0_left_elbow_joint": 1.0, + "robot0_right_shoulder_pitch_joint": 4.0, + "robot0_right_shoulder_roll_joint": 3.0, + "robot0_right_shoulder_yaw_joint": 2.0, + "robot0_right_elbow_joint": 1.0 + }, + "ik_hand_pos_cost": 10.0, + "ik_hand_ori_cost": 5, + "use_joint_angle_action_input": false + }, + "body_parts": { + "legs": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2], + "kv": 0, + "kp": [150, 150, 150, 300, 40, 40, 150, 150, 150, 200, 40, 40], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "use_torque_compensation": false, + "use_external_torque_compensation": true, + "desired_torque_as_acceleration": false + }, + "arms": { + "left": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [5.0, 5.0, 2.0, 2.0, 2.0, 2.0, 2.0], + "kp": [100, 100, 40, 40, 20, 20, 20], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + }, + "use_torque_compensation": false, + "use_external_torque_compensation": true, + "desired_torque_as_acceleration": false + }, + "right": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": [5.0, 5.0, 2.0, 2.0, 2.0, 2.0, 2.0], + "kp": [100, 100, 40, 40, 20, 20, 20], + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + }, + "use_torque_compensation": false, + "use_external_torque_compensation": true, + "desired_torque_as_acceleration": false + } + }, + "torso": { + "type" : "JOINT_POSITION", + "input_max": 100, + "input_min": -100, + "input_type": "absolute", + "output_max": 100, + "output_min": -100, + "kd": 5.0, + "kp": 250.0, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "use_torque_compensation": false, + "use_external_torque_compensation": true, + "desired_torque_as_acceleration": false + }, + "base": { + "type": "JOINT_VELOCITY_AND_POSITION", + "interpolation": "null" + } + } +} diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/macros.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/macros.py new file mode 100644 index 0000000..048a800 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/macros.py @@ -0,0 +1,32 @@ +""" +Macro settings that can be imported and toggled. Internally, specific parts of the codebase rely on these settings +for determining core functionality. + +To make sure global reference is maintained, should import these settings as: + +`import robocasa.macros as macros` +""" + +SHOW_SITES = False + +# whether to print debugging information +VERBOSE = False + +# Spacemouse settings. Used by SpaceMouse class in robosuite/devices/spacemouse.py +SPACEMOUSE_VENDOR_ID = 9583 +SPACEMOUSE_PRODUCT_ID = 50741 + +DATASET_BASE_PATH = None + +try: + from robocasa.macros_private import * +except ImportError: + from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER + + import robocasa + + ROBOSUITE_DEFAULT_LOGGER.warning("No private macro file found!") + ROBOSUITE_DEFAULT_LOGGER.warning("It is recommended to use a private macro file") + ROBOSUITE_DEFAULT_LOGGER.warning( + "To setup, run: python {}/scripts/setup_macros.py".format(robocasa.__path__[0]) + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/__init__.py new file mode 100644 index 0000000..4eb69cb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/__init__.py @@ -0,0 +1,3 @@ +import os + +assets_root = os.path.join(os.path.dirname(__file__), "assets") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/gear_factory.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/gear_factory.xml new file mode 100644 index 0000000..92f2cd4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/gear_factory.xml @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_0.obj new file mode 100644 index 0000000..5aa4dc7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7ef6eed7cdfc4d73d81b110ddc735f25f528eff0c4be075060e33cfc87aad9b8 +size 2760 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_1.obj new file mode 100644 index 0000000..953af97 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4b353b5d292ad60fb29dbf5357ec8bc1f722de79a14d3550e0416a304e200a7 +size 591488 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_10.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_10.obj new file mode 100644 index 0000000..2f7a029 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_10.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d205deef9e28dedec707e95f6c6d1976621ce0d11d68da83a3b99633467d8826 +size 9340 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_11.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_11.obj new file mode 100644 index 0000000..802e432 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_11.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:29dadafae8b16cf1bd9e94eb542921fae95bfcc88cebed7ae5b6fd7a0640a8ce +size 3644 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_2.obj new file mode 100644 index 0000000..3fbf0f6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5021815fcf59000f857bbd6b31205e3423a67a5a4c098307dc4b6fa9fee4ae7b +size 22319 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_3.obj new file mode 100644 index 0000000..86df603 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e18b08f3710eb882af24a010fbd0b4071f9ad2cfcb303ca194865fe7ac507571 +size 61251 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_4.obj new file mode 100644 index 0000000..930948d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d3229b03fa86e9586c289bd08f36bbdca0acd0dcd8bee0bee32d321b859c2c38 +size 799520 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_5.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_5.obj new file mode 100644 index 0000000..a1191cb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_5.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:055dab0ae75f8759c3adfec6060b3ca4cfa817cd7c7e9ced6bd9356b42423882 +size 939 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_6.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_6.obj new file mode 100644 index 0000000..b67abcb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_6.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6640ee9422b419fcfbda7d6591d349cde8c58b48ef229fc237ba51cede0ac6c +size 939 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_7.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_7.obj new file mode 100644 index 0000000..0a83705 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_7.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e0254a4d587ae89abd780c699970a1161900e5dec4473158ca346f90b6af2f22 +size 931 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_8.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_8.obj new file mode 100644 index 0000000..e13e812 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_8.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f23c95caf4a8c5cb01a4c7d98853a51924936eb9b95a2620d6e2274d1941fa47 +size 931 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_9.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_9.obj new file mode 100644 index 0000000..00cb3cb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_9.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8a55d6d89953196a6b0850dc15d7c3a6abb413cdddea189818cd518cd733400c +size 9280 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_poles.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_poles.obj new file mode 100644 index 0000000..9a9729a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_poles.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:215ee95c29a11b4b5b2c8459a04b778c5fb5ac8bdf37460b4a254008317ea2e4 +size 402088 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_walls.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_walls.obj new file mode 100644 index 0000000..9039778 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/gear_factory_walls.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2579080de17cb3f603d4a0187f6573f622c5ec72bc68cec0926641cb6afe1e08 +size 906 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/paint_lines.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/paint_lines.obj new file mode 100644 index 0000000..6653463 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/meshes/paint_lines.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7fb88ca471f539e16898cfd1550dc04f67fa73c0f68b6ab071820e7af09f49f1 +size 2661 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Fence_Metal_AO.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Fence_Metal_AO.png new file mode 100644 index 0000000..11e7aeb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Fence_Metal_AO.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc11f7243c43d08e03d5d7e1cee2f424c7fc7c7ca44da1a9d3baf1b81445fa79 +size 660075 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Laser_Beam.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Laser_Beam.png new file mode 100644 index 0000000..842d0a1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Laser_Beam.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5467d8b0c8c96255c737be4c8ef69e6e868b6337b2397ea36c8d55bb70fb1da +size 2788 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Light_Curtain.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Light_Curtain.png new file mode 100644 index 0000000..2376960 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Light_Curtain.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b579342d81a168fe47e0c16413716acb5eba7991fda6a17c1d130058ef3f7513 +size 401084 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_3@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_3@Medium.png new file mode 100644 index 0000000..e480711 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_3@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:481233c3e168d507747afe25ce0b19f2327a7b3eeea30cd59638912a2f5f2fd7 +size 67495 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_4@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_4@Medium.png new file mode 100644 index 0000000..0295b50 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Rack_4@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4494244813f3fd471956328f870cff3699ed7df70bdf2105795ee08bf4f06b19 +size 72209 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1@Medium.png new file mode 100644 index 0000000..45e738a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d41ca80b5a2b7b8d2f8c3d70544f7911463cd885791a705d6dffc45db237410b +size 22732 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1_Fence@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1_Fence@Medium.png new file mode 100644 index 0000000..3e77ee9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_1_Fence@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fcbe9be7a6a5e7c6edacec266c0995b92f9a95a7f1f5601e5219c1c2abbb65d6 +size 29983 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2@Medium.png new file mode 100644 index 0000000..10e2096 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1abf589188d93de492f8291706f81702771c2c44bdf2ad2019ae9186734de5ab +size 22723 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2_Fence@Medium.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2_Fence@Medium.png new file mode 100644 index 0000000..7b4306e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Robots_2_Fence@Medium.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4eea4dd26c644c90b57e6eca4affc42910c9762c72b2175f25c21a04e9cab0de +size 28141 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Steel_ASTM_AO.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Steel_ASTM_AO.png new file mode 100644 index 0000000..2e06e28 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/Steel_ASTM_AO.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5499310672dd4d034d21f36c61d8392bddd58c2aff6d500cb1dadbbfdd8062e3 +size 773080 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/T_Floor_A1_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/T_Floor_A1_Albedo.png new file mode 100644 index 0000000..59d8636 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/T_Floor_A1_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:93fac117050110a7e26849c4c1a116b49c650dd9668ae9468f69f7b0f10c6efb +size 1003554 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/factory@1024.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/factory@1024.png new file mode 100644 index 0000000..872f978 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/factory@1024.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b91b16d53a4ae44f9bad5e5a2d3f40517ea60a7df9b625d19431303169d2efe3 +size 631890 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/mtl-base_color@Small.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/mtl-base_color@Small.png new file mode 100644 index 0000000..668fc28 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_factory/textures/mtl-base_color@Small.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:593a32d4b49561df13ed3e84c674333e2c1cacf34ddbf2684ff1596974c4db43 +size 889 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/gear_lab.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/gear_lab.xml new file mode 100644 index 0000000..0e52097 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/gear_lab.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/floor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/floor.png new file mode 100644 index 0000000..fed69aa --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/floor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2348cb54852f55a3bc80104335b9b226b3df7eb7f9f3624d8a795e765660a6ec +size 3048801 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/wall.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/wall.png new file mode 100644 index 0000000..f0c979f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/gear_lab/textures/wall.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9b9dcedebe7f553f1a2924bd6ea3708f8fda81ce630403852d91d4e595fa3d1d +size 2427263 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/ground_arena.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/ground_arena.xml new file mode 100644 index 0000000..f5460ee --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/arenas/ground_arena.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_0.obj new file mode 100644 index 0000000..18ba962 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3bf9f2201b919d41c8dd97bf890ddeb65ac358978e1c24285326ed82e67e40ab +size 1860 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_1.obj new file mode 100644 index 0000000..7e6efbb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:785c15947d5629d794ec34e28c5fe3a66d9df228a7be859747820f8db6629d12 +size 1804 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_2.obj new file mode 100644 index 0000000..6621ee9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:057f24ac6a71b0ed8078f2c38786ae5d5018d5ce07fcbadadae3a0febb870bb4 +size 1826 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_3.obj new file mode 100644 index 0000000..cc5a3f1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d9812f6835a6f8b70d34c4dc9726e5f5f1411fe1b706a1260912a7584718f891 +size 1819 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_4.obj new file mode 100644 index 0000000..947cd83 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/collision/tmpm_h04v7s_collision_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4a4322d7d54a321938f6002e8a899ee74b8b6a0e5a3bd8844bc5955ea5d82001 +size 1859 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/model.xml new file mode 100644 index 0000000..78f4bb8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/model.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/image0.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/image0.png new file mode 100644 index 0000000..ddc0e7a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/image0.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6cfada801814afc0bd12b41f6a912d7c3efa474c95155e64b66b4d045a7ac46 +size 527119 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/material.mtl new file mode 100644 index 0000000..3361680 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/material.mtl @@ -0,0 +1,8 @@ +# https://github.com/mikedh/trimesh + +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.80000000 0.80000000 0.80000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.jpeg \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/model_normalized_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/model_normalized_0.obj new file mode 100644 index 0000000..cfbd7fc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/apple_0/visual/model_normalized_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1cb27d851c8903c81af90ca3c8d1f625cfb679207f2fdfe1bf49abce704d12c8 +size 937838 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/meshes/cardbox_a1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/meshes/cardbox_a1.obj new file mode 100644 index 0000000..932866d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/meshes/cardbox_a1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3a88c44e4ac68d8e4c6a5d5c69d2505ef3e3d48ef584a559abaaae5886051ab7 +size 26428 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/model.xml new file mode 100644 index 0000000..afd903b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/textures/T_Cardbox_A1_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/textures/T_Cardbox_A1_Albedo.png new file mode 100755 index 0000000..96d65ba --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_a1/textures/T_Cardbox_A1_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:15ce39d78be5a045cc92e2790a6da7e335ec2599b4dd21d01960a5d02eff3b00 +size 786202 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/meshes/cardbox_b1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/meshes/cardbox_b1.obj new file mode 100644 index 0000000..f434d57 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/meshes/cardbox_b1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5e06387d89002438810bdde5b05dd578380f2d271213cb800d09de92123ff2fb +size 27831 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/model.xml new file mode 100644 index 0000000..894de69 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/textures/T_Cardbox_B3_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/textures/T_Cardbox_B3_Albedo.png new file mode 100755 index 0000000..93a5cf4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_b1/textures/T_Cardbox_B3_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fa5be6d3d68f730631c818314c4d3c64fb3c1bf7639d37201594b3c49fe3c8d +size 827460 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/meshes/cardbox_c1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/meshes/cardbox_c1.obj new file mode 100644 index 0000000..662b4e1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/meshes/cardbox_c1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1931fc7eb93a4320254b7f1f9eb7d0d92a3417dfd826818086dd071158f8ebc6 +size 12847 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/model.xml new file mode 100644 index 0000000..0b1e037 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/textures/T_Cardbox_C2_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/textures/T_Cardbox_C2_Albedo.png new file mode 100755 index 0000000..aa34221 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_c1/textures/T_Cardbox_C2_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d0ace47be0adf9a7a32d87ccb098426879a1c69c67f03f332d64364c72841a47 +size 1171603 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/meshes/cardbox_d1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/meshes/cardbox_d1.obj new file mode 100644 index 0000000..9db0ce8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/meshes/cardbox_d1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12e8f980631b35af1ef2b41d43cd017c7401dc6840ab262ba3026dec7449ee7b +size 18003 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/model.xml new file mode 100644 index 0000000..528fd9e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/textures/T_Cardbox_D2_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/textures/T_Cardbox_D2_Albedo.png new file mode 100755 index 0000000..8305546 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/cardbox_d1/textures/T_Cardbox_D2_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:41d36736ccdc8cebe557274140e6e8a4a9b7caab452490c7a0be6afa233c5271 +size 1106523 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/material.mtl new file mode 100644 index 0000000..679fd4e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/material.mtl @@ -0,0 +1,5 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 1000.00000000 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_0.obj new file mode 100644 index 0000000..191d026 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7c1a3c79bd17621f64f1cc7b8d33d76a7bdf174ca84e91ad154ffae2fbb1e7c1 +size 227353 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_1.obj new file mode 100644 index 0000000..ad12012 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d52345abf3f34f29392d9a75b8e2978c87b33f63f0b66a4b2b359a41608ded2 +size 574169 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_2.obj new file mode 100644 index 0000000..7a3bf47 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:79a0051d06b25ed816e408f1a493304361701ed0897548af9dae8b3f86f9a810 +size 14820 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_3.obj new file mode 100644 index 0000000..3df4be0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:362256f23ca7873d0562fe7e7c90aba2952887f18dca4a56007472c731104478 +size 176468 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_4.obj new file mode 100644 index 0000000..6ac2fda --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:95bc7c62d4cf4ae4afb6fa188d26a45d0d033c1eb2bedbba85f6059949c5a5b3 +size 66158 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_5.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_5.obj new file mode 100644 index 0000000..21aebb6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_5.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fbba68b8ae873330d18cd6d88b3c0822c615b52a809cd1b480e38cb84b173821 +size 3972 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_6.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_6.obj new file mode 100644 index 0000000..a12b3e5 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_6.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:056a8c4a9323809ec06978416970bbc4da6dc724fe5c1fa0530de62711499dd2 +size 33285 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_0.obj new file mode 100644 index 0000000..6ad7465 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3d238540e2232790c82db0cfb52419e09013aefae4e331f93a4d702bde419392 +size 1852 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_1.obj new file mode 100644 index 0000000..90826cb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0a9c29776f51b0af613f4ba45f08deb7b638ef6c2326e1ddcc7b400c561055eb +size 1863 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_10.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_10.obj new file mode 100644 index 0000000..d6e5498 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_10.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d0d58f8a8aafe616fdfe18f8b71505b468f8c7e4233134787e05c6d280f35e1 +size 1789 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_11.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_11.obj new file mode 100644 index 0000000..29040cd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_11.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad2e51b49c16685e8f1a5dcc3fdb9b99b2678136b0556d1437dfa6986b7fa00b +size 1792 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_2.obj new file mode 100644 index 0000000..ffa316c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5f78f411190e81d52d6aac00c037fc76faaffa0a1f935f3b3f064c60c9fc396 +size 1839 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_3.obj new file mode 100644 index 0000000..ddbe44a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:79f3b1e31a51a1ecf914b7cd76b4a39531da06be5b76eab859d9e8719862fac7 +size 1819 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_4.obj new file mode 100644 index 0000000..5857fc7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:978fbd289fd46feb7161874ea2a6c70955a77a44047fa472306a32884f9c5f76 +size 1859 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_5.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_5.obj new file mode 100644 index 0000000..44b39ee --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_5.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:727d944f3f9d30a7b1b2893226ea8439f9ff469533f9c82aaacec1ff2e7f00d2 +size 1841 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_6.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_6.obj new file mode 100644 index 0000000..183da70 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_6.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:adeb625333d9c1c891a187595da3cabf6c835011ea49f4af8ccf78d451f73d0b +size 1810 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_7.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_7.obj new file mode 100644 index 0000000..5f87121 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_7.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b0edb9058056aa856782c17bf4ad3aec934f7aba355dfecfd26c2a7b2a390595 +size 1821 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_8.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_8.obj new file mode 100644 index 0000000..040c0d3 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_8.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:506a2431499173aa9bfd784c415bc5dea65d4a76cee564a6bbbd39af3149cfed +size 1821 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_9.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_9.obj new file mode 100644 index 0000000..a87335b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/meshes/clock_0_0_collision_0_9.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ce3bfcdf5d6f2d79ed81fe9919a4351bdd52f33509b457b2cff5ea94309fa15 +size 1814 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/model.xml new file mode 100644 index 0000000..535a3af --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/model.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/chasovaya_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/chasovaya_baseColor.png new file mode 100644 index 0000000..f95698d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/chasovaya_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a88fe9760f77a3d1bbb458adf17f7a046bb598b750715c19d9c43491c9f2a2c4 +size 113289 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/ciferblat_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/ciferblat_baseColor.png new file mode 100644 index 0000000..f1c2569 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/ciferblat_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:712a9416d7ded0efef98582799c7bc0fd68ce9aefa9962653378fe49d95282d1 +size 515150 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/hrom_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/hrom_baseColor.png new file mode 100644 index 0000000..957bdaa --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/hrom_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:47963b57e114147b61520c75992c0c804fbd2fe87caca3f8470795553be9edb4 +size 955035 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/korpus_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/korpus_baseColor.png new file mode 100644 index 0000000..9719023 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/korpus_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2871a0e70593a927bc073471a81671b0e1bf628cd8ff36057e67e2b3bb2f6526 +size 720486 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/minutnaya_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/minutnaya_baseColor.png new file mode 100644 index 0000000..975308c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/clock_0/textures/minutnaya_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:026a626bbfb627ecead5716d675113f97d8da1d24ea514d5bcf99cd5ab28e5cb +size 58463 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_0.obj new file mode 100644 index 0000000..ebd4db8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e50761859d97ea2589490d9360bb40facc2657fa96f9455836120032ac9f4fcc +size 2727 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_1.obj new file mode 100644 index 0000000..e4cac2a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:343d8d6fcddfad1246648ca4f4f9ad18eef70606cc3d9d6837e3f5d941efe9df +size 9894 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_2.obj new file mode 100644 index 0000000..4c7489e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0e1105580c441125902651507c045d37e7866fcee325a10117617d54197465e +size 57933 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_3.obj new file mode 100644 index 0000000..ab87e7c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/meshes/control_box_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8695beb6927d7ca91c7c3be583ec2201601c5349d4a40bb6db20f7687ae971f4 +size 691460 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/model.xml new file mode 100644 index 0000000..3ea3615 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/model.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/textures/Switch_Box_Rack_AO.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/textures/Switch_Box_Rack_AO.png new file mode 100644 index 0000000..1bcf7cc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_box/textures/Switch_Box_Rack_AO.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5efc6c3f01e422858f21341786b4718284124674496fff5b566cb7383fb78483 +size 761721 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/material.mtl new file mode 100644 index 0000000..db21711 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/material.mtl @@ -0,0 +1,5 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.35686275 0.35686275 0.35686275 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/ControlPanel_AO.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/ControlPanel_AO.png new file mode 100644 index 0000000..18bd0b0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/ControlPanel_AO.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b8359bce9d201add9a3bbbb52a89bc955a98d82244f4cac789d8ffa9c611b98f +size 357071 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_0.obj new file mode 100644 index 0000000..218ef94 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1206c9655fe5b62da90c085e8114bab9e69353dbb1e7bbf8c67c761675e7aa1 +size 19789 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_1.obj new file mode 100644 index 0000000..c01e861 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:224090ddc2fd699419b8244969c346dcfe410e98e8a294a659b89d9a914dc2a2 +size 255796 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_4.obj new file mode 100644 index 0000000..4279abd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa1b9063cb9daa8dce7d9de724b2d4d2426378deed4529c182f3620152feef79 +size 554559 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.mtl new file mode 100644 index 0000000..2318be2 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.mtl @@ -0,0 +1,12 @@ +# Blender 4.4.3 MTL File: 'None' +# www.blender.org + +newmtl Plastic_Yellow_A.002 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd ControlPanel_AO.png diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.obj new file mode 100644 index 0000000..8c31b0d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_body.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f7af8dcc87926f84ba09f578f96e843966d539f0fbe3aa8fbd069e1f74a1eb3f +size 204955 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_button.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_button.obj new file mode 100644 index 0000000..789fd99 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_button.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:202abd6aab485fa652c8ec8c59b29a31f9c9e96ab6c693037993cc42eaa646fb +size 69705 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_0.obj new file mode 100644 index 0000000..67e0bfc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f3faf6c0bddb635a5ac99ceb50769a38a947cc99964ff0d965111269ce881533 +size 17472 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_1.obj new file mode 100644 index 0000000..9ac878c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a2c57a196dca4c048f582ae71601f31467dbff2232044bf87bbaffaa94f479b6 +size 26414 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_2.obj new file mode 100644 index 0000000..b996e9f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_collision_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b075fec279bb5df917fb9ff9ef292ead0013e713fab9065e56411ad0216e4ca5 +size 20848 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_lever.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_lever.obj new file mode 100644 index 0000000..917f5c6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/meshes/control_conveyorbelt_a08_lever.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5e40db732bd937b7472ba3309ef06c608f2504714e320df32128ad2fa5dc8cfc +size 30023 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/model.xml new file mode 100644 index 0000000..28678bd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/model.xml @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/textures/ControlPanel_AO.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/textures/ControlPanel_AO.png new file mode 100644 index 0000000..18bd0b0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/control_conveyorbelt_a08/textures/ControlPanel_AO.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b8359bce9d201add9a3bbbb52a89bc955a98d82244f4cac789d8ffa9c611b98f +size 357071 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/material.mtl new file mode 100644 index 0000000..d9c888e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 1.00000000 1.00000000 1.00000000 +Ns 200.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_0.obj new file mode 100644 index 0000000..d6b4ccb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:993a5dbe1ca76dfa58e23e7f2f9d70e62c3bb4d73bd28b33db64bc083be8870c +size 113635 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_1.obj new file mode 100644 index 0000000..b3fb8fd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:07e4a9f23cac60235fb99e860c48e92cc924dc658875af909cfabff68dc94647 +size 49763 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_2.obj new file mode 100644 index 0000000..0548273 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ccd733391a6cf73609417992bdf6f7d4d3bef3110ba87c5af323babbfe5c83ff +size 1094 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_l.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_l.obj new file mode 100644 index 0000000..e205c01 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_l.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b00929a6bdb75d544e47a09a3c802650937d8a64d0cb7f801f414b49ea8978b +size 1674 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_r.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_r.obj new file mode 100644 index 0000000..46fe63d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_border_r.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4bd207633dea28ebe876332de0d69dbdba6a0184f16294bd19c39e71819feedb +size 1699 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_platform.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_platform.obj new file mode 100644 index 0000000..5c2bb19 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/meshes/dock_board_a12_convex_platform.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c2992253d9e2115e3ca4680db2a7f5cfacc9f3e6910ee0cf9189dcb066eef1d8 +size 3963 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/model.xml new file mode 100644 index 0000000..fac18f8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/model.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Aluminium_Brushed_A1_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Aluminium_Brushed_A1_Albedo.png new file mode 100755 index 0000000..436f1b6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Aluminium_Brushed_A1_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9a09faefcbf11807aea9657bd653522130085f2f6adc435d75d9b4ba0343eec +size 569649 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Plastic_Orange_B_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Plastic_Orange_B_Albedo.png new file mode 100755 index 0000000..6228737 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/T_Plastic_Orange_B_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c912bb893c0da0d984101e58a1c762ca228ebe61f292ef74f3b674815590a277 +size 4548 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/t_metal_circles_grid_grey_a_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/t_metal_circles_grid_grey_a_Albedo.png new file mode 100755 index 0000000..b09506e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/dock_board_a12/textures/t_metal_circles_grid_grey_a_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ae46d20631a698091639902a27d59e60b92b6b0fe0b325287b73949800ca7e7e +size 698388 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_0.obj new file mode 100644 index 0000000..b83e365 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a1f59593b1ba86311d30f107536fb1fb212f891d1bef268ae484dc8f1fde569 +size 39825 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_1.obj new file mode 100644 index 0000000..afcdc83 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f34b23866e3ab7a0d3e7d12cb79cce0f202e3d06cb8ea66186df8f563d5dc46 +size 10920 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_2.obj new file mode 100644 index 0000000..a641ca7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6a641a487987d02ef2beedac4f24a6af016bec4d77cca5551ca1f8d02d951d85 +size 5472 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_3.obj new file mode 100644 index 0000000..83297b8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/meshes/ergo_table_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8be5ac7b1006c29458964ab1c1c777d2b5cec9954367a57f7896f280cab556fd +size 35063 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/model.xml new file mode 100644 index 0000000..c1a038e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/model.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/textures/Rubber_Smooth_BaseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/textures/Rubber_Smooth_BaseColor.png new file mode 100644 index 0000000..1c1a608 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/factory_ergo_table/textures/Rubber_Smooth_BaseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3c322a9923807ce826e379a1aecad8051fb49538a2e9506d246e272a48e3a800 +size 4546 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/meshes/jug_a01.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/meshes/jug_a01.obj new file mode 100644 index 0000000..e523c6e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/meshes/jug_a01.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cab493a9ef239fcbc2b3b411ec9aed0eef169f687f2363111bdf57f61f4f4c35 +size 425956 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/model.xml new file mode 100644 index 0000000..6b5d4c0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/model.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/textures/Jug_A.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/textures/Jug_A.png new file mode 100644 index 0000000..468af03 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/jug_a01/textures/Jug_A.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:760aada863076fe8d2640c53280d9c91e54933d99fa608ead9a0fd784dd89609 +size 306131 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf/model.xml new file mode 100644 index 0000000..f0e2521 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf/model.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_board/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_board/model.xml new file mode 100644 index 0000000..e08ea10 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_board/model.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_cabinet/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_cabinet/model.xml new file mode 100644 index 0000000..9e48631 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_shelf_cabinet/model.xml @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/material.mtl new file mode 100644 index 0000000..c56604f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.jpeg \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/meshes/lab_table.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/meshes/lab_table.obj new file mode 100644 index 0000000..4c72be9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/meshes/lab_table.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:885970e46bf8f2c31f1234ab3ed78998f74728abfab4ad7721e0e8a08b82f975 +size 1550942 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/model.xml new file mode 100644 index 0000000..864c3be --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/model.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/textures/Image_0.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/textures/Image_0.png new file mode 100644 index 0000000..2370738 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/lab_table/textures/Image_0.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4ffd78e92179759939991d6fff3a4570d814dbda34b2aacf56bafc3e3a72011e +size 767585 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_0.obj new file mode 100644 index 0000000..6f5b526 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e282d18b41b39da6993474405ac9a034608e388076ae989efd8d4a997782fda9 +size 90017 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_1.obj new file mode 100644 index 0000000..7fedf57 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/meshes/longbox_a08_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6bb66c694f99fd72b0dd16b322237109ba151da35845669d6b90af9f9ea9840c +size 31086 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/model.xml new file mode 100644 index 0000000..981b2af --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/model.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Tile_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Tile_Albedo.png new file mode 100755 index 0000000..f1a344e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Tile_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0df7cfcef1140ecc7a661ba27e4ebee496f9c014dbe765d2d22c3bc4c034038 +size 1556243 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Trim_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Trim_Albedo.png new file mode 100755 index 0000000..99edc8e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a08/textures/T_LongBox_A01_Trim_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:37b69190208818111194259585c30657ae2c9d734fe25b0a33b6d360f86664fe +size 627467 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_0.obj new file mode 100644 index 0000000..8844087 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a937e7f0d60b8d669b1de597f0de5e00ca207c419d1b057b43c44873d49d3a45 +size 29676 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_1.obj new file mode 100644 index 0000000..3723d6a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/meshes/longbox_a09_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eb3cf6351a67473bd85b9b0f214f7ee15bc23f5d15458029acd940202ffcef52 +size 54417 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/model.xml new file mode 100644 index 0000000..b3e4d5d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/model.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Tile_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Tile_Albedo.png new file mode 100755 index 0000000..f1a344e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Tile_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0df7cfcef1140ecc7a661ba27e4ebee496f9c014dbe765d2d22c3bc4c034038 +size 1556243 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Trim_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Trim_Albedo.png new file mode 100755 index 0000000..99edc8e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/longbox_a09/textures/T_LongBox_A01_Trim_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:37b69190208818111194259585c30657ae2c9d734fe25b0a33b6d360f86664fe +size 627467 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/material.mtl new file mode 100644 index 0000000..43b5249 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/material.mtl @@ -0,0 +1,5 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 1.00000000 1.00000000 1.00000000 +Ks 1.00000000 1.00000000 1.00000000 +Ns 200.00000000 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_0.obj new file mode 100644 index 0000000..dcf3be8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:acd5f804b7786ede2307a9cea82f592013610586f930e32ffff2181742c22fdf +size 214596 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_1.obj new file mode 100644 index 0000000..e3f3451 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4616618b090beef9d2a83bd0db97f8893faff0085fa20082c2efe0350d279b3a +size 368876 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_2.obj new file mode 100644 index 0000000..320edbc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5129b5fa5047cd0b6cfc148a6f26b9616d1ea0e9c5566c08813c05918b1b2384 +size 52933 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_3.obj new file mode 100644 index 0000000..9e73f4b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:63139d70ee1a8f517c317d19d3996404708cf666feb706cd6b51f460ad9685f9 +size 170989 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_4.obj new file mode 100644 index 0000000..7a4139f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ff114945b276421816f541897aceebb4f38d0243006bce3c281cb3ff549d782 +size 291825 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_disk.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_disk.obj new file mode 100644 index 0000000..6604e77 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_disk.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3b8146a7eaf0a2127bf02fb805897b8e806a26da31e153c5c14a76ae41c0f6c8 +size 51488 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_hinge.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_hinge.obj new file mode 100644 index 0000000..a9a8683 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_hinge.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b964168fa34e11c9f0589635818432b27aa35b068448866ad889e9e1c3689774 +size 35272 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_rubber.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_rubber.obj new file mode 100644 index 0000000..a551c32 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/meshes/mobile_shelving_cart_wheel_rubber.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:64914fc5620acd0f30dce3ca3720680287144aaa8c29160945daa4b9ac019a59 +size 30396 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/model.xml new file mode 100644 index 0000000..67382df --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/model.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Aluminium_Brushed_A1_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Aluminium_Brushed_A1_Albedo.png new file mode 100755 index 0000000..436f1b6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Aluminium_Brushed_A1_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9a09faefcbf11807aea9657bd653522130085f2f6adc435d75d9b4ba0343eec +size 569649 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Plastic_Blue_A_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Plastic_Blue_A_Albedo.png new file mode 100755 index 0000000..d67d01f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/mobile_shelving_cart/textures/T_Plastic_Blue_A_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6efcce9c65ecd03ba69afb53cbfc7eadf1bb7871d054d2d31fe9f109e1ffb02 +size 815833 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/meshes/pallet_b1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/meshes/pallet_b1.obj new file mode 100644 index 0000000..3e59113 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/meshes/pallet_b1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:76f169c1ad0e53a3a7a38398671b5a167bca3a603cbc15e33f05b541c546f108 +size 113154 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/model.xml new file mode 100644 index 0000000..baa6b1d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/textures/T_Pallet_A2_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/textures/T_Pallet_A2_Albedo.png new file mode 100755 index 0000000..fc83e54 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_b1/textures/T_Pallet_A2_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4e798c482ae87cb1e53cbc20b75c1cca5162987f9ba40501bbf4a2e02dd75a6d +size 1416514 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/meshes/pallet_c1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/meshes/pallet_c1.obj new file mode 100644 index 0000000..553fc95 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/meshes/pallet_c1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d0181122cd5fd3b8d6894788b22dd2ecb38e452ca764e017674fe3d1f7feaca +size 156702 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/model.xml new file mode 100644 index 0000000..a69ffea --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/model.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/textures/T_Pallet_A3_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/textures/T_Pallet_A3_Albedo.png new file mode 100755 index 0000000..4f897c5 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/pallet_c1/textures/T_Pallet_A3_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9964aef74efeb3d1f9684a1a488c29e97cc1616acb8b7125cb9f95f79a95fed7 +size 1385313 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_0.obj new file mode 100644 index 0000000..17d2a9d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9552bb0fe29867cc739b4af6b61bf28a42f4ff3926097f7a47dfccc9c372f846 +size 1814 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_1.obj new file mode 100644 index 0000000..7854d5c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f3a05c7e80685b172b3974b72fb09abc08cfd57eabedcbd527ad40a997fe28dc +size 1829 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_2.obj new file mode 100644 index 0000000..ed854cf --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9967f6ff0ed0cfe1cf343fee8c662df4fa36b80c6d2fc4b39e1eb2a471af81be +size 1856 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_3.obj new file mode 100644 index 0000000..170e9c6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/collision/tmpzdovka8d_collision_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:063871c558b70eebb842d8c663fe08db13815631c642a11ac984f88cca827bbc +size 1881 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/model.xml new file mode 100644 index 0000000..45048c4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/model.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/material.mtl new file mode 100644 index 0000000..1a47eb7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/material.mtl @@ -0,0 +1,7 @@ +# https://github.com/mikedh/trimesh + +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.80000000 0.80000000 0.80000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 359.99999300 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/model_normalized_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/model_normalized_0.obj new file mode 100644 index 0000000..3c4edc1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/plate_1/visual/model_normalized_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:95f98c91f68835de8afeb88622ab2ce9d69686d336075203433230a671064a51 +size 778710 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0.obj new file mode 100644 index 0000000..9ae0363 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa343cf973a977b0dc1d098f1fa5ddfa3a1eed4cb70984996752f9b4186c5e63 +size 327444 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_0.obj new file mode 100644 index 0000000..b894407 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:19563cc88da9378f1b0fe0d6bff80b082374e8aa3ee1d842fef973b81bb6857e +size 1807 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_1.obj new file mode 100644 index 0000000..68f7d73 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0e8c12ede53f3b222655db3101a70995ee65ba93477e8efdf0aa33766f9cda9 +size 1771 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_2.obj new file mode 100644 index 0000000..695cb39 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c1953993e80de209f275363349d6c574bbbcb971f83888d840a13947a6dd699b +size 1761 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_3.obj new file mode 100644 index 0000000..a185047 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/meshes/rubix_cube_1_0_collision_0_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ba0980d31523b2bc2b4dc29b4ce91c1a8879b2b8af8e3dbc165a14e2b5cfea02 +size 1752 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/model.xml new file mode 100644 index 0000000..47722c9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/model.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/textures/Material.002_baseColor.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/textures/Material.002_baseColor.png new file mode 100644 index 0000000..f952ff5 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/rubix_cube_1/textures/Material.002_baseColor.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6d8321a937585aa451d7a142a05d66d0e1b4d5098822a3e9c00bfd455f83225d +size 264262 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/material.mtl new file mode 100644 index 0000000..269919a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_0.obj new file mode 100644 index 0000000..56d5987 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a10674a71b148cf0cea835c3e69b715dd9c27a359e26d1f873e1d6cb8495604c +size 2504230 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_1.obj new file mode 100644 index 0000000..83871e4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/meshes/shelf_a12_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:af8f87a18ecd52e8009cc456e99006ac9af4508887ab1f6e5be4e87ad3d288f6 +size 3708983 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/model.xml new file mode 100644 index 0000000..21f81cc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/model.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Poles.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Poles.png new file mode 100644 index 0000000..6f4474f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Poles.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:324c3e90eb24f83a6f3797d73e3ba77a7bf03d52cfb53db3f29e310c4f49fb3d +size 670918 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Shelves.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Shelves.png new file mode 100644 index 0000000..1db0503 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/shelf_a12/textures/Shelf_A12_Shelves.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:10e5cdfabb0c0f0c292bebe1fb90b5b3c412d816c16cb7d0b43d56d69453d681 +size 595861 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/model.xml new file mode 100644 index 0000000..acee67c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/model.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/textures/Target_Zone.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/textures/Target_Zone.png new file mode 100644 index 0000000..9e651cd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/target_zone/textures/Target_Zone.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:402302497d2f4f7587efa19ac2f42c66f20ed6a7e0556e4a69b94b9f345e5c6b +size 4367541 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/material.mtl new file mode 100644 index 0000000..fb1f30a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/material.mtl @@ -0,0 +1,5 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.80000000 0.80000000 0.80000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_0.obj new file mode 100644 index 0000000..cb966dc --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7b90a9160e30b8caff3fa6cba421abbb7bb86902ee13f2d639259e6e119096dc +size 184808 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_1.obj new file mode 100644 index 0000000..7659aa3 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/meshes/tote_f01_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ddc9ad0070d1ab294de54e201c1d3ce0d6d392259cffa76519ca41ee45465f0 +size 531121 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/model.xml new file mode 100644 index 0000000..91cde71 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f01/model.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/material.mtl new file mode 100644 index 0000000..fb1f30a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/material.mtl @@ -0,0 +1,5 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.80000000 0.80000000 0.80000000 +Ks 0.50196078 0.50196078 0.50196078 +Ns 250.00000000 \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_0.obj new file mode 100644 index 0000000..1efa307 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b12c818f1ecd2e5f81663b95cf781a92787d7da40644d027afb40b44f08f4693 +size 184507 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_1.obj new file mode 100644 index 0000000..000eb75 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/meshes/tote_f02_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d7a1e98376e4cc3c3ca9f714c829550d565d7ebdc870d28c4017ce86d697ed1c +size 562174 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/model.xml new file mode 100644 index 0000000..f291a48 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/tote_f02/model.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/material.mtl b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/material.mtl new file mode 100644 index 0000000..d9c888e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/material.mtl @@ -0,0 +1,6 @@ +newmtl material_0 +Ka 1.00000000 1.00000000 1.00000000 +Kd 0.40000000 0.40000000 0.40000000 +Ks 1.00000000 1.00000000 1.00000000 +Ns 200.00000000 +map_Kd material_0.png \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_disk.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_disk.obj new file mode 100644 index 0000000..6604e77 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_disk.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3b8146a7eaf0a2127bf02fb805897b8e806a26da31e153c5c14a76ae41c0f6c8 +size 51488 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_hinge.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_hinge.obj new file mode 100644 index 0000000..a9a8683 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_hinge.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b964168fa34e11c9f0589635818432b27aa35b068448866ad889e9e1c3689774 +size 35272 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_rubber.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_rubber.obj new file mode 100644 index 0000000..a551c32 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/wheel_rubber.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:64914fc5620acd0f30dce3ca3720680287144aaa8c29160945daa4b9ac019a59 +size 30396 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_0.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_0.obj new file mode 100644 index 0000000..6a3c9d7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6c8a6ccd4ebe357d5d9784c657fd6bcb138e41b2dcee74b1bab9792af59a172c +size 340183 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_1.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_1.obj new file mode 100644 index 0000000..6d01f71 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e8b17016b65555cd82b461d726eb820ac4299aa8331d88c06e2190fe480032d5 +size 346311 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_2.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_2.obj new file mode 100644 index 0000000..c2d20b9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:28a87331c04723905d7ca402920fabfbc8c4ff57638416c93ca070e83e99c346 +size 155710 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_3.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_3.obj new file mode 100644 index 0000000..757a37a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5359a08cc9f5524d8cbb16c40b419a4d6cfeb22d7690be673229d9109d8f6afe +size 328039 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_4.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_4.obj new file mode 100644 index 0000000..ea6825b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ca5573682a55c977447847edf90ef2cc8081ae0c35bf364ef8e4dccc2bd60b5 +size 167824 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_5.obj b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_5.obj new file mode 100644 index 0000000..daac351 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/meshes/workshop_trolley_a01_5.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa8262db597020c50be7abc9f473c504c0efa04cb3df07c7f676b0fb8fd0f3f5 +size 525395 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/model.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/model.xml new file mode 100644 index 0000000..5b8a39f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/model.xml @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/textures/T_Plastic_Blue_A_Albedo.png b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/textures/T_Plastic_Blue_A_Albedo.png new file mode 100755 index 0000000..d67d01f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/objects/omniverse/locomanip/workshop_trolley_a01/textures/T_Plastic_Blue_A_Albedo.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6efcce9c65ecd03ba69afb53cbfc7eadf1bb7871d054d2d31fe9f109e1ffb02 +size 815833 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_29dof_rev_1_0.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_29dof_rev_1_0.xml new file mode 100644 index 0000000..da74e29 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_29dof_rev_1_0.xml @@ -0,0 +1,431 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_left_hand.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_left_hand.xml new file mode 100644 index 0000000..4048fc2 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_left_hand.xml @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_right_hand.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_right_hand.xml new file mode 100644 index 0000000..88ba638 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/g1_threefinger_right_hand.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/head_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/head_link.STL new file mode 100644 index 0000000..401f822 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/head_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:005fb67fbd3eff94aa8bf4a6e83238174e9f91b6721f7111594322f223724411 +size 932784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 0000000..7cd7052 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d49e3abc6f5b12e532062cd575b87b5ef40cd2a3fc18f54a1ca5bba4f773d51d +size 71184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 0000000..cb69f65 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4092af943141d4d9f74232f3cfa345afc6565f46a077793b8ae0e68b39dc33f +size 653384 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link.STL new file mode 100644 index 0000000..7fa0527 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fa752198accd104d5c4c3a01382e45165b944fbbc5acce085059223324e5bed3 +size 88784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link_merge.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link_merge.STL new file mode 100644 index 0000000..f287918 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_elbow_link_merge.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:73c8adc6346b900eb6a2ae7159e25dfda1be60cd98affe13536e01ab33a6c1de +size 3136784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_palm_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_palm_link.STL new file mode 100644 index 0000000..c8fcc3e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:23a486b75bd78a9bf03cec25d84d87f97f3dae038cf21a743b6d469b337e4004 +size 2140184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_0_link.STL new file mode 100644 index 0000000..7fb38c1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a90c721661c0622685488a3c74d1e122c7da89242d3a1daef75edb83422d05e0 +size 8884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_1_link.STL new file mode 100644 index 0000000..54f7754 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:445c54a45bc13ce36001556f66bc0f49c83cb40321205ae4d676bb2874325684 +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_2_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_2_link.STL new file mode 100644 index 0000000..3e4f124 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3d8dbe5085acfc213d21aa8b0782e89cd79084e9678f3a85fc7b04a86b029db5 +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000..4cf7475 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4725168105ee768ee31638ef22b53f6be2d7641bfd7cfefe803488d884776fa4 +size 181684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_roll_link.STL new file mode 100644 index 0000000..585f604 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:91f25922ee8a7c3152790051bebad17b4d9cd243569c38fe340285ff93a97acf +size 192184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 0000000..b46a741 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a16d88aa6ddac8083aa7ad55ed317bea44b1fa003d314fba88965b7ed0f3b55b +size 296284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_knee_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_knee_link.STL new file mode 100644 index 0000000..2dcf84e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d92b9e3d3a636761150bb8025e32514c4602b91c7028d523ee42b3e632de477 +size 854884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_rubber_hand.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_rubber_hand.STL new file mode 100644 index 0000000..04a2fa2 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cff2221a690fa69303f61fce68f2d155c1517b52efb6ca9262dd56e0bc6e70fe +size 2287484 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 0000000..926d980 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0d1cfd02fcf0d42f95e678eeca33da3afbcc366ffba5c052847773ec4f31d52 +size 176784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 0000000..4c6840b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fb9df21687773522598dc384f1a2945c7519f11cbc8bd372a49170316d6eee88 +size 400284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 0000000..89b0e06 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1aa97e9748e924336567992181f78c7cd0652fd52a4afcca3df6b2ef6f9e712e +size 249184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 0000000..f9c9e4f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b251d8e05047f695d0f536cd78c11973cfa4e78d08cfe82759336cc3471de3a9 +size 85984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 0000000..2097ca3 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:edc387c9a0ba8c2237e9b296d32531426fabeb6f53e58df45c76106bca74148c +size 356184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_rubber_hand.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..7c58819 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e81030abd023bd9e4a308ef376d814a2c12d684d8a7670c335bbd5cd7809c909 +size 3484884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 0000000..692f4b0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/left_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:83f8fb3a726bf9613d65dd14f0f447cb918c3c95b3938042a0c9c09749267d3b +size 318684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/logo_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/logo_link.STL new file mode 100644 index 0000000..6c25961 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/logo_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8571a0a19bc4916fa55f91449f51d5fdefd751000054865a842449429d5f155b +size 243384 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis.STL new file mode 100644 index 0000000..f98a88d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ba6bbc888e630550140d3c26763f10206da8c8bd30ed886b8ede41c61f57a31 +size 1060884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis_contour_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis_contour_link.STL new file mode 100644 index 0000000..8025bc0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/pelvis_contour_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5cc5c2c7a312329e3feeb2b03d3fc09fc29705bd01864f6767e51be959662420 +size 1805184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 0000000..94a586a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:15be426539ec1be70246d4d82a168806db64a41301af8b35c197a33348c787a9 +size 71184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 0000000..3c38507 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b66222ea56653e627711b56d0a8949b4920da5df091da0ceb343f54e884e3a5 +size 653784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link.STL new file mode 100644 index 0000000..52aa0eb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1be925d7aa268bb8fddf5362b9173066890c7d32092c05638608126e59d1e2ab +size 88784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link_merge.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link_merge.STL new file mode 100644 index 0000000..b042703 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_elbow_link_merge.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e6def7100e5bbb276c7498c5b7a8c8812f560ca35d6bd09c376ebab93595be7f +size 3058284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_palm_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_palm_link.STL new file mode 100644 index 0000000..fd2f7f0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:86c0b231cc44477d64a6493e5a427ba16617a00738112dd187c652675b086fb9 +size 2140184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_0_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_0_link.STL new file mode 100644 index 0000000..a385d96 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:544298d0ea1088f5b276a10cc6a6a9e533efdd91594955fdc956c46211d07f83 +size 8884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_1_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_1_link.STL new file mode 100644 index 0000000..c118de7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0a9a820da8dd10f298778b714f1364216e8a5976f4fd3a05689ea26327d44bf6 +size 475984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_2_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_2_link.STL new file mode 100644 index 0000000..0979fb6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3f1bfb37668e8f61801c8d25f171fa1949e08666be86c67acad7e0079937cc45 +size 1521784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 0000000..064085f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e4f3c99d7f4a7d34eadbef9461fc66e3486cb5442db1ec50c86317d459f1a9c6 +size 181284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_roll_link.STL new file mode 100644 index 0000000..6544025 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4c254ef66a356f492947f360dd931965477b631e3fcc841f91ccc46d945d54f6 +size 192684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 0000000..0ad7bee --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e479c2936ca2057e9eb2f7dff6c189b7419d7b8484dea0b298cbb36a2a6aa668 +size 296284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_knee_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_knee_link.STL new file mode 100644 index 0000000..65e8a70 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:63c4008449c9bbe701a6e2b557b7a252e90cf3a5abcf54cee46862b9a69f8ec8 +size 852284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_rubber_hand.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_rubber_hand.STL new file mode 100644 index 0000000..58148fb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:99533b778bca6246144fa511bb9d4e555e075c641f2a0251e04372869cd99d67 +size 2192684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 0000000..48a1c46 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24cdb387e0128dfe602770a81c56cdce3a0181d34d039a11d1aaf8819b7b8c02 +size 176784 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 0000000..2a5d22f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:962b97c48f9ce9e8399f45dd9522e0865d19aa9fd299406b2d475a8fc4a53e81 +size 401884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 0000000..0882a56 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0b76489271da0c72461a344c9ffb0f0c6e64f019ea5014c1624886c442a2fe5 +size 249984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_pitch_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 0000000..2efd25c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d22f8f3b3127f15a63e5be1ee273cd5075786c3142f1c3d9f76cbf43d2a26477 +size 79584 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 0000000..77d23a7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a7ee9212ff5b94d6cb7f52bb1bbf3f352194d5b598acff74f4c77d340c5b344f +size 356084 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_rubber_hand.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..6f122af --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0729aff1ac4356f9314de13a46906267642e58bc47f0d8a7f17f6590a6242ccf +size 3481584 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 0000000..77edbb4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/right_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc9dece2d12509707e0057ba2e48df8f3d56db0c79410212963a25e8a50f61a6 +size 341484 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_link.STL new file mode 100644 index 0000000..cc2cbbf --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:82be7f93e85b3d303a1d1e1847e2c916939bd61c424ed1ebd28691ec33909dd1 +size 203584 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_rod_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_rod_link.STL new file mode 100644 index 0000000..dd439bf --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_L_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c10de1effa7ea797ac268006aa2a739036c7e1f326b2012d711ee2c20c5a6e96 +size 74884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_link.STL new file mode 100644 index 0000000..422ffe4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:54ded433a3a0c76027365856fdbd55215643de88846f7d436598a4071e682725 +size 203584 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_rod_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_rod_link.STL new file mode 100644 index 0000000..6df46c8 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_constraint_R_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cb83fd38a9f06c99e3f301c70f12d94ae770a8f0cf9501f83580a27f908990b4 +size 74884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link.STL new file mode 100644 index 0000000..e4fb87c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e96d023f0368a4e3450b86ca5d4f10227d8141156a373e7da8cb3c93266523e0 +size 2232984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link_rev_1_0.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link_rev_1_0.STL new file mode 100755 index 0000000..836b992 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/torso_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11ddb46f2098efbbd8816b1d65893632d8e78be936376c7cdcd6771899ccc723 +size 2570584 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_L.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_L.STL new file mode 100644 index 0000000..6ec689b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ebafdcb4de6871113f0ca2c356618d6e46b1d50f6c0bf9e37f47b9d8e100d99 +size 114684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_R.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_R.STL new file mode 100644 index 0000000..69fd76a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_constraint_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:791902a291ffbd35ab97383b7b44ea5d975de7c80eef838797c970790b382ca9 +size 114684 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link.STL new file mode 100644 index 0000000..007f56d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34f0aa73f41131230be4d25876c944fdf6c24d62553f199ff8b980c15e8913df +size 24184 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link_rev_1_0.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link_rev_1_0.STL new file mode 100755 index 0000000..36a5a70 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_roll_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b67c347a05abc3e8ddae600d98a082bee273bb39f8e651647708b0a7140a8a97 +size 85884 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_support_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_support_link.STL new file mode 100644 index 0000000..4a50f94 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_support_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fae9e1bb609848a1667d32eed8d6083ae443538a306843056a2a660f1b2926a +size 150484 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link.STL new file mode 100644 index 0000000..c049deb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2883f20e03f09b669b5b4ce10677ee6b5191c0934b584d7cbaef2d0662856ffb +size 336284 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link_rev_1_0.STL b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link_rev_1_0.STL new file mode 100755 index 0000000..dc628fb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/assets/robots/unitree_g1/meshes/waist_yaw_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec6db442b11f25eed898b5add07940c85d804f300de24dcbd264ccd8be7d554c +size 619984 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/__init__.py new file mode 100644 index 0000000..7e7f360 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/__init__.py @@ -0,0 +1 @@ +from .g1_threefinger_hands import G1ThreeFingerLeftHand, G1ThreeFingerRightHand diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/g1_threefinger_hands.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/g1_threefinger_hands.py new file mode 100644 index 0000000..2c9aaa3 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/grippers/g1_threefinger_hands.py @@ -0,0 +1,170 @@ +""" +Dexterous hands for GR1 robot. +""" + +import numpy as np + +from robosuite.models.grippers.gripper_model import GripperModel +from robosuite.utils.mjcf_utils import xml_path_completion +from robosuite.utils.mjcf_utils import find_parent, xml_path_completion +from robosuite.models.grippers import register_gripper + +import robocasa.models + + +@register_gripper +class G1ThreeFingerLeftHand(GripperModel): + """ + Dexterous left hand of G1 robot + Args: + idn (int or str): Number or some other unique identification string for this gripper instance + """ + + def __init__(self, idn=0): + super().__init__( + xml_path_completion( + "robots/unitree_g1/g1_threefinger_left_hand.xml", + root=robocasa.models.assets_root, + ), + idn=idn, + ) + + def format_action(self, action): + # grasp = action[0] + # action = [0] * 7 + # if grasp > 0: + # # note that the sign of the action is opposite of the right hand + # action[1] = action[2] = grasp + # action[3] = action[4] = action[5] = action[6] = -grasp + + return action + + @property + def init_qpos(self): + return np.array([0.0] * 7) + + @property + def grasp_qpos(self): + return { + -1: np.array([0.0] * 7), + 1: np.array([0, 1, 1.25, -1.2, -1.2, -1.2, -1.2]), + } + + @property + def speed(self): + return 0.15 + + @property + def dof(self): + return 7 # 12 + + @property + def _important_geoms(self): + return { + "left_finger": [ + "left_hand_thumb_0_link_col", + "left_hand_thumb_1_link_col", + "left_hand_thumb_2_link_col", + "left_hand_middle_0_link_col", + "left_hand_middle_1_link_col", + "left_hand_index_0_link_col", + "left_hand_index_1_link_col", + ], + "right_finger": [ + "left_hand_thumb_0_link_col", + "left_hand_thumb_1_link_col", + "left_hand_thumb_2_link_col", + "left_hand_middle_0_link_col", + "left_hand_middle_1_link_col", + "left_hand_index_0_link_col", + "left_hand_index_1_link_col", + ], + "left_fingerpad": [ + "left_hand_thumb_2_link_col", + "left_hand_middle_1_link_col", + "left_hand_index_1_link_col", + ], + "right_fingerpad": [ + "left_hand_thumb_2_link_col", + "left_hand_middle_1_link_col", + "left_hand_index_1_link_col", + ], + } + + +@register_gripper +class G1ThreeFingerRightHand(GripperModel): + """ + Dexterous right hand of G1 robot + Args: + idn (int or str): Number or some other unique identification string for this gripper instance + """ + + def __init__(self, idn=0): + super().__init__( + xml_path_completion( + "robots/unitree_g1/g1_threefinger_right_hand.xml", + root=robocasa.models.assets_root, + ), + idn=idn, + ) + + def format_action(self, action): + # grasp = action[0] + # action = [0] * 7 + # if grasp > 0: + # action[1] = action[2] = -grasp + # action[3] = action[4] = action[5] = action[6] = grasp + return action + + @property + def init_qpos(self): + return np.array([0.0] * 7) + + @property + def grasp_qpos(self): + return { + -1: np.array([0.0] * 7), + 1: np.array([0, -1, -1.25, 1.2, 1.2, 1.2, 1.2]), + } + + @property + def speed(self): + return 0.15 + + @property + def dof(self): + return 7 + + @property + def _important_geoms(self): + return { + "left_finger": [ + "right_hand_thumb_0_link_col", + "right_hand_thumb_1_link_col", + "right_hand_thumb_2_link_col", + "right_hand_middle_0_link_col", + "right_hand_middle_1_link_col", + "right_hand_index_0_link_col", + "right_hand_index_1_link_col", + ], + "right_finger": [ + "right_hand_thumb_0_link_col", + "right_hand_thumb_1_link_col", + "right_hand_thumb_2_link_col", + "right_hand_middle_0_link_col", + "right_hand_middle_1_link_col", + "right_hand_index_0_link_col", + "right_hand_index_1_link_col", + ], + "left_fingerpad": [ + "right_hand_thumb_2_link_col", + "right_hand_middle_1_link_col", + "right_hand_index_1_link_col", + ], + "right_fingerpad": [ + "right_hand_thumb_2_link_col", + "right_hand_middle_1_link_col", + "right_hand_index_1_link_col", + ], + } diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/__init__.py new file mode 100644 index 0000000..6e58d64 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/__init__.py @@ -0,0 +1,13 @@ +from robosuite.models.objects import * +from robosuite.utils import * + +from .xml_objects import ( + CoffeeMachinePodObject, + CoffeeMachineBodyObject, + CoffeeMachineLidObject, + CoffeeMachineBaseObject, + DrawerObject, + LongDrawerObject, +) +from .composite import * +from .composite_body import * diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/__init__.py new file mode 100644 index 0000000..1ac011d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/__init__.py @@ -0,0 +1,6 @@ +from .box_pattern_object import BoxPatternObject +from .needle import NeedleObject +from .ring_tripod import RingTripodObject +from .bin import Bin +from .lid import Lid +from .pot_with_handles import PotWithHandlesObject diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/bin.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/bin.py new file mode 100644 index 0000000..c7e95a6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/bin.py @@ -0,0 +1,205 @@ +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import CustomMaterial, add_to_dict + + +class Bin(CompositeObject): + """ + Generates a four-walled bin container with an open top. + Args: + name (str): Name of this Bin object + bin_size (3-array): (x,y,z) full size of bin + wall_thickness (float): How thick to make walls of bin + transparent_walls (bool): If True, walls will be semi-translucent + friction (3-array or None): If specified, sets friction values for this bin. None results in default values + density (float): Density value to use for all geoms. Defaults to 1000 + use_texture (bool): If true, geoms will be defined by realistic textures and rgba values will be ignored + rgba (4-array or None): If specified, sets rgba values for all geoms. None results in default values + material: If specified, use this material + upside_down (bool): if True, construct and initialize the Bin so the bottom geom is at the top + """ + + def __init__( + self, + name, + bin_size=(0.3, 0.3, 0.15), + wall_thickness=0.01, + transparent_walls=True, + friction=None, + density=1000.0, + use_texture=True, + rgba=(0.2, 0.1, 0.0, 1.0), + material=None, + upside_down=False, + add_second_base=False, + transparent_base=False, + ): + # Set name + self._name = name + + # Set object attributes + self.bin_size = np.array(bin_size) + self.wall_thickness = wall_thickness + self.transparent_walls = transparent_walls + self.friction = friction if friction is None else np.array(friction) + self.density = density + self.use_texture = use_texture + self.rgba = rgba + self.bin_mat_name = "dark_wood_mat" + + # if box should be constructed and initialized upside down + self.upside_down = upside_down + + # if box should have a second base (so it will be a closed box) + self.add_second_base = add_second_base + if self.add_second_base: + assert not self.upside_down + + # if base(s) should be transparent + self.transparent_base = transparent_base + + self.has_material = material is not None + if self.has_material: + assert isinstance(material, CustomMaterial) + self.material = material + self.bin_mat_name = self.material.mat_attrib["name"] + else: + # default material + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "3 3", + "specular": "0.4", + "shininess": "0.1", + } + bin_mat = CustomMaterial( + texture="WoodDark", + tex_name="dark_wood", + mat_name=self.bin_mat_name, + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.material = bin_mat + + # Element references + self._base_geom = "base" + if self.add_second_base: + self._second_base_geom = "base2" + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + self.append_material(self.material) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + base_args = { + "total_size": self.bin_size / 2.0, + "name": self.name, + "locations_relative_to_center": True, + "obj_types": "all", + "density": self.density, + } + obj_args = {} + + # Base(s) + base_geom_loc = (0, 0, -(self.bin_size[2] - self.wall_thickness) / 2) + if self.upside_down: + base_geom_loc = ( + base_geom_loc[0], + base_geom_loc[1], + -1.0 * base_geom_loc[2], + ) + if self.transparent_base: + base_rgba = (1.0, 1.0, 1.0, 0.3) + base_mat = None + else: + base_rgba = None if self.use_texture else self.rgba + base_mat = self.bin_mat_name if self.use_texture else None + + base_geom_names = [self._base_geom] + base_geom_locs = [base_geom_loc] + if self.add_second_base: + base_geom_names.append(self._second_base_geom) + base_geom_locs.append((base_geom_loc[0], base_geom_loc[1], -1.0 * base_geom_loc[2])) + + for base_g_name, base_g_loc in zip(base_geom_names, base_geom_locs): + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=base_g_loc, + geom_quats=(1, 0, 0, 0), + geom_sizes=( + np.array((self.bin_size[0], self.bin_size[1], self.wall_thickness)) + - np.array((self.wall_thickness, self.wall_thickness, 0)) + ) + / 2, + geom_names=base_g_name, + geom_rgbas=base_rgba, + geom_materials=base_mat, + geom_frictions=self.friction, + ) + + # Walls + x_vals = np.array( + [ + 0, + -(self.bin_size[0] - self.wall_thickness) / 2, + 0, + (self.bin_size[0] - self.wall_thickness) / 2, + ] + ) + y_vals = np.array( + [ + -(self.bin_size[1] - self.wall_thickness) / 2, + 0, + (self.bin_size[1] - self.wall_thickness) / 2, + 0, + ] + ) + w_vals = np.array([self.bin_size[0], self.bin_size[1], self.bin_size[0], self.bin_size[1]]) + r_vals = np.array([np.pi / 2, 0, -np.pi / 2, np.pi]) + if self.transparent_walls: + wall_rgba = (1.0, 1.0, 1.0, 0.3) + wall_mat = None + else: + wall_rgba = None if self.use_texture else self.rgba + wall_mat = self.bin_mat_name if self.use_texture else None + for i, (x, y, w, r) in enumerate(zip(x_vals, y_vals, w_vals, r_vals)): + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=(x, y, 0), + geom_quats=T.convert_quat(T.axisangle2quat(np.array([0, 0, r])), to="wxyz"), + geom_sizes=(self.wall_thickness / 2, w / 2, self.bin_size[2] / 2), + geom_names=f"wall{i}", + geom_rgbas=wall_rgba, + geom_materials=wall_mat, + geom_frictions=self.friction, + ) + + # Add back in base args and site args + obj_args.update(base_args) + + # Return this dict + return obj_args + + @property + def base_geoms(self): + """ + Returns: + list of str: geom names corresponding to bin base + """ + return [self.correct_naming(self._base_geom)] diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/box_pattern_object.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/box_pattern_object.py new file mode 100644 index 0000000..d9a3a21 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/box_pattern_object.py @@ -0,0 +1,124 @@ +import numpy as np + +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import add_to_dict +import robosuite.utils.transform_utils as T + + +class BoxPatternObject(CompositeObject): + """ + Generates shapes by using a pattern of unit-size boxes. + + Args: + name (str): Name of this Needle object + """ + + def __init__( + self, + name, + unit_size, + pattern, + rgba=None, + material=None, + density=100.0, + # solref=[0.02, 1.], + # solimp=[0.9, 0.95, 0.001], + friction=None, + ): + """ + Args: + unit_size (3d array / list): size of each unit block in each dimension + + pattern (3d array / list): array of normalized sizes specifying the + geometry of the shape. A "0" indicates the absence of a cube and + a "1" indicates the presence of a full unit block. The dimensions + correspond to z, x, and y respectively. + """ + self._name = name + self.rgba = rgba + self.material = material + self.density = density + self.friction = friction + + # number of blocks in z, x, and y + self.pattern = np.array(pattern) + self.nz, self.nx, self.ny = self.pattern.shape + self.unit_size = unit_size + self.total_size = [ + self.nx * unit_size[0], + self.ny * unit_size[1], + self.nz * unit_size[2], + ] + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + if self.material is not None: + self.append_material(self.material) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + base_args = { + "total_size": self.total_size, + "name": self.name, + "locations_relative_to_center": False, + "obj_types": "all", + "density": self.density, + } + obj_args = {} + + geom_locations = [] + geom_sizes = [] + geom_names = [] + nz, nx, ny = self.pattern.shape + for k in range(nz): + for i in range(nx): + for j in range(ny): + if self.pattern[k, i, j] > 0: + geom_sizes.append( + [ + self.unit_size[0], + self.unit_size[1], + self.unit_size[2], + ] + ) + geom_locations.append( + [ + i * 2.0 * self.unit_size[0], + j * 2.0 * self.unit_size[1], + k * 2.0 * self.unit_size[2], + ] + ) + geom_names.append("{}_{}_{}".format(k, i, j)) + + # geom_rgbas = [rgba for _ in geom_locations] + # geom_frictions = [friction for _ in geom_locations] + for i in range(len(geom_locations)): + add_to_dict( + dic=obj_args, + geom_types="box", + # needle geom needs to be offset from boundary in (x, z) + geom_locations=tuple(geom_locations[i]), + geom_quats=(1, 0, 0, 0), + geom_sizes=tuple(geom_sizes[i]), + geom_names=geom_names[i], + geom_rgbas=self.rgba, + geom_materials=self.material.name if self.material is not None else None, + geom_frictions=None, + ) + + # Add back in base args and site args + obj_args.update(base_args) + + # Return this dict + return obj_args diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/lid.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/lid.py new file mode 100644 index 0000000..3eb602b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/lid.py @@ -0,0 +1,136 @@ +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import CustomMaterial, add_to_dict + + +class Lid(CompositeObject): + """ + Generates a square lid with a simple handle. + Args: + name (str): Name of this Lid object + lid_size (3-array): (length, width, thickness) of lid + handle_size (3-array): (thickness, length, height) of handle + transparent (bool): If True, lid will be semi-translucent + friction (3-array or None): If specified, sets friction values for this lid. None results in default values + density (float): Density value to use for all geoms. Defaults to 1000 + use_texture (bool): If true, geoms will be defined by realistic textures and rgba values will be ignored + rgba (4-array or None): If specified, sets rgba values for all geoms. None results in default values + """ + + def __init__( + self, + name, + lid_size=(0.3, 0.3, 0.01), + handle_size=(0.02, 0.08, 0.03), + transparent=True, + friction=None, + density=250.0, + use_texture=True, + rgba=(0.2, 0.1, 0.0, 1.0), + ): + # Set name + self._name = name + + # Set object attributes + self.lid_size = np.array(lid_size) + self.handle_size = np.array(handle_size) + self.transparent = transparent + self.friction = friction if friction is None else np.array(friction) + self.density = density + self.use_texture = use_texture + self.rgba = rgba + self.lid_mat_name = "dark_wood_mat" + + # Element references + self._handle_geom = "handle" + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "3 3", + "specular": "0.4", + "shininess": "0.1", + } + lid_mat = CustomMaterial( + texture="WoodDark", + tex_name="dark_wood", + mat_name=self.lid_mat_name, + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.append_material(lid_mat) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + full_height = self.lid_size[2] + self.handle_size[2] + full_size = np.array([self.lid_size[0], self.lid_size[1], full_height]) + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + base_args = { + "total_size": full_size / 2.0, + "name": self.name, + "locations_relative_to_center": True, + "obj_types": "all", + } + obj_args = {} + + # Top + if self.transparent: + top_rgba = (1.0, 1.0, 1.0, 0.3) + top_mat = None + else: + top_rgba = None if self.use_texture else self.rgba + top_mat = self.lid_mat_name if self.use_texture else None + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=(0, 0, (-full_size[2] + self.lid_size[2]) / 2), + geom_quats=(1, 0, 0, 0), + geom_sizes=np.array((full_size[0], full_size[1], self.lid_size[2])) / 2, + geom_names="top", + geom_rgbas=top_rgba, + geom_materials=top_mat, + geom_frictions=self.friction, + density=self.density, + ) + + # Handle + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=(0, 0, (full_size[2] - self.handle_size[2]) / 2), + geom_quats=(1, 0, 0, 0), + geom_sizes=self.handle_size / 2, + geom_names=self._handle_geom, + geom_rgbas=None if self.use_texture else self.rgba, + geom_materials=self.lid_mat_name if self.use_texture else None, + geom_frictions=self.friction, + density=self.density * 2, + ) + + # Add back in base args and site args + obj_args.update(base_args) + + # Return this dict + return obj_args + + @property + def handle_geoms(self): + """ + Returns: + list of str: geom names corresponding to lid handle + """ + return [self.correct_naming(self._handle_geom)] diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/needle.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/needle.py new file mode 100644 index 0000000..9dc6bb1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/needle.py @@ -0,0 +1,109 @@ +import numpy as np + +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import add_to_dict, CustomMaterial +import robosuite.utils.transform_utils as T + + +class NeedleObject(CompositeObject): + """ + Generates a needle with a handle (used in Threading task) + + Args: + name (str): Name of this Needle object + """ + + def __init__( + self, + name, + ): + + ### TODO: make this object more general (with more args and configuration options) later ### + + # Set object attributes + self._name = name + self.needle_mat_name = "darkwood_mat" + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "1 1", + "specular": "0.4", + "shininess": "0.1", + } + needle_mat = CustomMaterial( + texture="WoodDark", + tex_name="darkwood", + mat_name="darkwood_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.append_material(needle_mat) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + base_args = { + "total_size": [0.02, 0.08, 0.02], + "name": self.name, + "locations_relative_to_center": False, + "obj_types": "all", + "density": 100.0, + } + obj_args = {} + + # make a skinny needle object with a large handle + needle_size = [0.005, 0.06, 0.005] + handle_size = [0.02, 0.02, 0.02] + + # Needle + add_to_dict( + dic=obj_args, + geom_types="box", + # needle geom needs to be offset from boundary in (x, z) + geom_locations=( + (handle_size[0] - needle_size[0]), + 0.0, + (handle_size[2] - needle_size[2]), + ), + geom_quats=(1, 0, 0, 0), + geom_sizes=tuple(needle_size), + geom_names="needle", + geom_rgbas=None, + geom_materials=self.needle_mat_name, + # make the needle low friction to ensure easy insertion + geom_frictions=(0.3, 5e-3, 1e-4), + ) + + # Handle + add_to_dict( + dic=obj_args, + geom_types="box", + # handle geom needs to be offset in y + geom_locations=(0.0, 2.0 * needle_size[1], 0.0), + geom_quats=(1, 0, 0, 0), + geom_sizes=tuple(handle_size), + geom_names="handle", + geom_rgbas=None, + geom_materials=self.needle_mat_name, + geom_frictions=None, + ) + + # Add back in base args and site args + obj_args.update(base_args) + + # Return this dict + return obj_args diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/pot_with_handles.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/pot_with_handles.py new file mode 100644 index 0000000..0aebf35 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/pot_with_handles.py @@ -0,0 +1,396 @@ +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import ( + BLUE, + GREEN, + RED, + CustomMaterial, + add_to_dict, + array_to_string, +) + + +class PotWithHandlesObject(CompositeObject): + """ + Generates the Pot object with side handles (used in TwoArmLift) + + Args: + name (str): Name of this Pot object + + body_half_size (3-array of float): If specified, defines the (x,y,z) half-dimensions of the main pot + body. Otherwise, defaults to [0.07, 0.07, 0.07] + + handle_radius (float): Determines the pot handle radius + + handle_length (float): Determines the pot handle length + + handle_width (float): Determines the pot handle width + + handle_friction (float): Friction value to use for pot handles. Defauls to 1.0 + + density (float): Density value to use for all geoms. Defaults to 1000 + + use_texture (bool): If true, geoms will be defined by realistic textures and rgba values will be ignored + + rgba_body (4-array or None): If specified, sets pot body rgba values + + rgba_handle_0 (4-array or None): If specified, sets handle 0 rgba values + + rgba_handle_1 (4-array or None): If specified, sets handle 1 rgba values + + solid_handle (bool): If true, uses a single geom to represent the handle + + thickness (float): How thick to make the pot body walls + """ + + def __init__( + self, + name, + body_half_size=(0.07, 0.07, 0.07), + handle_radius=0.01, + handle_length=0.09, + handle_width=0.09, + handle_friction=1.0, + density=1000, + use_texture=True, + rgba_body=None, + rgba_handle_0=None, + rgba_handle_1=None, + solid_handle=False, + thickness=0.01, # For body + ): + # Set name + self._name = name + + # Set object attributes + self.body_half_size = np.array(body_half_size) + self.thickness = thickness + self.handle_radius = handle_radius + self.handle_length = handle_length + self.handle_width = handle_width + self.handle_friction = handle_friction + self.density = density + self.use_texture = use_texture + self.rgba_body = np.array(rgba_body) if rgba_body else RED + self.rgba_handle_0 = np.array(rgba_handle_0) if rgba_handle_0 else GREEN + self.rgba_handle_1 = np.array(rgba_handle_1) if rgba_handle_1 else BLUE + self.solid_handle = solid_handle + + # Element references to be filled when generated + self._handle0_geoms = None + self._handle1_geoms = None + self.pot_base = None + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "1 1", + "specular": "0.4", + "shininess": "0.1", + } + redwood = CustomMaterial( + texture="WoodRed", + tex_name="redwood", + mat_name="pot_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + greenwood = CustomMaterial( + texture="WoodGreen", + tex_name="greenwood", + mat_name="handle0_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + bluewood = CustomMaterial( + texture="WoodBlue", + tex_name="bluewood", + mat_name="handle1_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.append_material(redwood) + self.append_material(greenwood) + self.append_material(bluewood) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + full_size = np.array( + ( + self.body_half_size, + self.body_half_size + self.handle_length * 2, + self.body_half_size, + ) + ) + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + base_args = { + "total_size": full_size / 2.0, + "name": self.name, + "locations_relative_to_center": True, + "obj_types": "all", + } + site_attrs = [] + obj_args = {} + + # Initialize geom lists + self._handle0_geoms = [] + self._handle1_geoms = [] + + # Add main pot body + # Base geom + name = f"base" + self.pot_base = [name] + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=(0, 0, -self.body_half_size[2] + self.thickness / 2), + geom_quats=(1, 0, 0, 0), + geom_sizes=np.array( + [self.body_half_size[0], self.body_half_size[1], self.thickness / 2] + ), + geom_names=name, + geom_rgbas=None if self.use_texture else self.rgba_body, + geom_materials="pot_mat" if self.use_texture else None, + geom_frictions=None, + density=self.density, + ) + + # Walls + x_off = np.array( + [ + 0, + -(self.body_half_size[0] - self.thickness / 2), + 0, + self.body_half_size[0] - self.thickness / 2, + ] + ) + y_off = np.array( + [ + -(self.body_half_size[1] - self.thickness / 2), + 0, + self.body_half_size[1] - self.thickness / 2, + 0, + ] + ) + w_vals = np.array( + [ + self.body_half_size[0], + self.body_half_size[1], + self.body_half_size[0], + self.body_half_size[1], + ] + ) + r_vals = np.array([np.pi / 2, 0, -np.pi / 2, np.pi]) + for i, (x, y, w, r) in enumerate(zip(x_off, y_off, w_vals, r_vals)): + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=(x, y, 0), + geom_quats=T.convert_quat(T.axisangle2quat(np.array([0, 0, r])), to="wxyz"), + geom_sizes=np.array([self.thickness / 2, w, self.body_half_size[2]]), + geom_names=f"body{i}", + geom_rgbas=None if self.use_texture else self.rgba_body, + geom_materials="pot_mat" if self.use_texture else None, + geom_frictions=None, + density=self.density, + ) + + # Add handles + main_bar_size = np.array( + [ + self.handle_width / 2 + self.handle_radius, + self.handle_radius, + self.handle_radius, + ] + ) + side_bar_size = np.array([self.handle_radius, self.handle_length / 2, self.handle_radius]) + handle_z = self.body_half_size[2] - self.handle_radius + for i, (g_list, handle_side, rgba) in enumerate( + zip( + [self._handle0_geoms, self._handle1_geoms], + [1.0, -1.0], + [self.rgba_handle_0, self.rgba_handle_1], + ) + ): + handle_center = np.array( + ( + 0, + handle_side * (self.body_half_size[1] + self.handle_length), + handle_z, + ) + ) + # Solid handle case + if self.solid_handle: + handle_center = np.array( + ( + 0, + handle_side * (self.body_half_size[1] + self.handle_length / 2), + handle_z, + ) + ) + name = f"handle{i}" + g_list.append(name) + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=handle_center, + geom_quats=(1, 0, 0, 0), + geom_sizes=np.array( + [ + self.handle_width / 2, + self.handle_length / 2, + self.handle_radius, + ] + ), + geom_names=name, + geom_rgbas=None if self.use_texture else rgba, + geom_materials=f"handle{i}_mat" if self.use_texture else None, + geom_frictions=(self.handle_friction, 0.005, 0.0001), + density=self.density, + ) + # Hollow handle case + else: + # Center bar + name = f"handle{i}_c" + g_list.append(name) + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=handle_center, + geom_quats=(1, 0, 0, 0), + geom_sizes=main_bar_size, + geom_names=name, + geom_rgbas=None if self.use_texture else rgba, + geom_materials=f"handle{i}_mat" if self.use_texture else None, + geom_frictions=(self.handle_friction, 0.005, 0.0001), + density=self.density, + ) + # Side bars + for bar_side, suffix in zip([-1.0, 1.0], ["-", "+"]): + name = f"handle{i}_{suffix}" + g_list.append(name) + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=( + bar_side * self.handle_width / 2, + handle_side * (self.body_half_size[1] + self.handle_length / 2), + handle_z, + ), + geom_quats=(1, 0, 0, 0), + geom_sizes=side_bar_size, + geom_names=name, + geom_rgbas=None if self.use_texture else rgba, + geom_materials=f"handle{i}_mat" if self.use_texture else None, + geom_frictions=(self.handle_friction, 0.005, 0.0001), + density=self.density, + ) + # Add relevant site + handle_site = self.get_site_attrib_template() + handle_name = f"handle{i}" + handle_site.update( + { + "name": handle_name, + "pos": array_to_string(handle_center - handle_side * np.array([0, 0.005, 0])), + "size": "0.005", + "rgba": rgba, + } + ) + site_attrs.append(handle_site) + # Add to important sites + self._important_sites[f"handle{i}"] = self.naming_prefix + handle_name + + # Add pot body site + pot_site = self.get_site_attrib_template() + center_name = "center" + pot_site.update( + { + "name": center_name, + "size": "0.005", + } + ) + site_attrs.append(pot_site) + # Add to important sites + self._important_sites["center"] = self.naming_prefix + center_name + + # Add back in base args and site args + obj_args.update(base_args) + obj_args["sites"] = site_attrs # All sites are part of main (top) body + + # Return this dict + return obj_args + + @property + def handle_distance(self): + """ + Calculates how far apart the handles are + + Returns: + float: handle distance + """ + return self.body_half_size[1] * 2 + self.handle_length * 2 + + @property + def handle0_geoms(self): + """ + Returns: + list of str: geom names corresponding to handle0 (green handle) + """ + return self.correct_naming(self._handle0_geoms) + + @property + def handle1_geoms(self): + """ + Returns: + list of str: geom names corresponding to handle1 (blue handle) + """ + return self.correct_naming(self._handle1_geoms) + + @property + def handle_geoms(self): + """ + Returns: + list of str: geom names corresponding to both handles + """ + return self.handle0_geoms + self.handle1_geoms + + @property + def important_sites(self): + """ + Returns: + dict: In addition to any default sites for this object, also provides the following entries + + :`'handle0'`: Name of handle0 location site + :`'handle1'`: Name of handle1 location site + """ + # Get dict from super call and add to it + dic = super().important_sites + dic.update(self._important_sites) + return dic + + @property + def bottom_offset(self): + return np.array([0, 0, -1 * self.body_half_size[2]]) + + @property + def top_offset(self): + return np.array([0, 0, self.body_half_size[2]]) + + @property + def horizontal_radius(self): + return np.sqrt(2) * (max(self.body_half_size) + self.handle_length) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/ring_tripod.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/ring_tripod.py new file mode 100644 index 0000000..339c56e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite/ring_tripod.py @@ -0,0 +1,194 @@ +import numpy as np + +from robosuite.models.objects import CompositeObject +from robosuite.utils.mjcf_utils import add_to_dict +from robosuite.utils.mjcf_utils import CustomMaterial +import robosuite.utils.transform_utils as T + + +class RingTripodObject(CompositeObject): + """ + Generates a tripod base with a small ring for threading a needle through it (used in Threading task) + + Args: + name (str): Name of this RingTripod object + """ + + def __init__( + self, + name, + ): + + ### TODO: make this object more general (with more args and configuration options) later ### + + # Set object attributes + self._name = name + self.tripod_mat_name = "lightwood_mat" + + # Other private attributes + self._important_sites = {} + + # Create dictionary of values to create geoms for composite object and run super init + super().__init__(**self._get_geom_attrs()) + + # Define materials we want to use for this object + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "1 1", + "specular": "0.4", + "shininess": "0.1", + } + tripod_mat = CustomMaterial( + texture="WoodLight", + tex_name="lightwood", + mat_name="lightwood_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.append_material(tripod_mat) + + def _get_geom_attrs(self): + """ + Creates geom elements that will be passed to superclass CompositeObject constructor + + Returns: + dict: args to be used by CompositeObject to generate geoms + """ + # Initialize dict of obj args that we'll pass to the CompositeObject constructor + total_size = (0.05, 0.05, 0.1) + base_args = { + "total_size": total_size, + "name": self.name, + "locations_relative_to_center": False, + "obj_types": "all", + "density": 100.0, + # NOTE: this lower value of solref allows the thin hole wall to avoid penetration through it + "solref": (0.02, 1.0), + "solimp": (0.9, 0.95, 0.001), + } + obj_args = {} + + # pattern for threading ring + unit_size = [0.005, 0.002, 0.002] + pattern = np.ones((6, 1, 6)) + for i in range(1, 5): + pattern[i][0][1:5] = np.zeros(4) + ring_size = [ + unit_size[0] * pattern.shape[1], + unit_size[1] * pattern.shape[2], + unit_size[2] * pattern.shape[0], + ] + self.ring_size = np.array(ring_size) + + # ring offset for where the ring is located relative to the (0, 0, 0) corner + ring_offset = [ + total_size[0] - ring_size[0], + total_size[1] - ring_size[1], + 2.0 * (total_size[2] - ring_size[2]), + ] + + # RING-GEOMS: use the pattern to instantiate geoms corresponding to the threading ring + nz, nx, ny = pattern.shape + self.num_ring_geoms = 0 + for k in range(nz): + for i in range(nx): + for j in range(ny): + if pattern[k, i, j] > 0: + add_to_dict( + dic=obj_args, + geom_types="box", + # needle geom needs to be offset from boundary in (x, z) + geom_locations=( + (i * 2.0 * unit_size[0]) + ring_offset[0], + (j * 2.0 * unit_size[1]) + ring_offset[1], + (k * 2.0 * unit_size[2]) + ring_offset[2], + ), + geom_quats=(1, 0, 0, 0), + geom_sizes=tuple(unit_size), + geom_names="ring_{}".format(self.num_ring_geoms), + geom_rgbas=None, + geom_materials=self.tripod_mat_name, + # make the ring low friction to ensure easy insertion + geom_frictions=(0.3, 5e-3, 1e-4), + ) + self.num_ring_geoms += 1 + + # TRIPOD-GEOMS: legs of the tripod + tripod_capsule_r = 0.01 + tripod_capsule_h = 0.03 + tripod_geom_locations = [ + (0.0, 0.0, 0.0), + (0.0, 2.0 * total_size[1] - 2.0 * tripod_capsule_r, 0.0), + ( + 2.0 * total_size[0] - 2.0 * tripod_capsule_r, + total_size[1] - tripod_capsule_r, + 0.0, + ), + ] + # rotate the legs to resemble a tripod + tripod_center = np.array([total_size[0], total_size[1], 0.0]) + xy_offset = np.array([tripod_capsule_r, tripod_capsule_r, 0.0]) + rotation_angle = -np.pi / 6.0 # 30 degrees + tripod_geom_quats = [] + for i in range(3): + capsule_loc = np.array(tripod_geom_locations[i]) + xy_offset + capsule_loc[2] = 0.0 # only care about location in x-y plane + vec_to_center = tripod_center - capsule_loc + vec_to_center = vec_to_center / np.linalg.norm(vec_to_center) + # cross-product with z unit vector to get vector to rotate about + rot_vec = np.cross(vec_to_center, np.array([0.0, 0.0, 1.0])) + rot_quat = T.mat2quat(T.rotation_matrix(angle=rotation_angle, direction=rot_vec)) + tripod_geom_quats.append(T.convert_quat(rot_quat, to="wxyz")) + + for i in range(3): + add_to_dict( + dic=obj_args, + geom_types="capsule", + geom_locations=tripod_geom_locations[i], + geom_quats=tripod_geom_quats[i], + geom_sizes=(tripod_capsule_r, tripod_capsule_h), + geom_names="tripod_{}".format(i), + geom_rgbas=None, + geom_materials=self.tripod_mat_name, + geom_frictions=None, + ) + + # POST-GEOMS: mounted base + post + base_thickness = 0.005 + post_size = 0.005 + post_geom_sizes = [ + (total_size[0], total_size[1], base_thickness), + ( + post_size, + post_size, + total_size[2] - ring_size[2] - base_thickness - tripod_capsule_r - tripod_capsule_h, + ), + ] + post_geom_locations = [ + (0.0, 0.0, 2.0 * (tripod_capsule_r + tripod_capsule_h)), + ( + total_size[0] - post_size, + total_size[1] - post_size, + 2.0 * (tripod_capsule_r + tripod_capsule_h + base_thickness), + ), + ] + for i in range(2): + add_to_dict( + dic=obj_args, + geom_types="box", + geom_locations=post_geom_locations[i], + geom_quats=(1, 0, 0, 0), + geom_sizes=post_geom_sizes[i], + geom_names="post_{}".format(i), + geom_rgbas=None, + geom_materials=self.tripod_mat_name, + geom_frictions=None, + ) + + # Add back in base args and site args + obj_args.update(base_args) + + # Return this dict + return obj_args diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/__init__.py new file mode 100644 index 0000000..dcea559 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/__init__.py @@ -0,0 +1,8 @@ +from .cup import CupObject +from .coffee_machine import CoffeeMachineObject +from .stacked_cylinder import StackedCylinderObject +from .inverse_stacked_cylinder import InverseStackedCylinderObject +from .stacked_box import StackedBoxObject +from .sliding_box import SlidingBoxObject +from .bin_with_handles import BinWithHandles +from .spray_bottle import SprayBottleObject diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/bin_with_handles.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/bin_with_handles.py new file mode 100644 index 0000000..ef35b5e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/bin_with_handles.py @@ -0,0 +1,162 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject, Bin +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class BinWithHandles(CompositeBodyObject): + """ + Bin with simple square handles on each side. + """ + + def __init__( + self, + name, + bin_size, + bin_wall_thickness, + bin_transparent_walls, + bin_upside_down, + center_handle_size, + adjacent_handle_size, + joints="default", + rgba=(0.2, 0.1, 0.0, 1.0), + material=None, + density=1000.0, + friction=None, + ): + + # Object properties + + # FULL size of bin + self.bin_size = list(bin_size) + self.bin_wall_thickness = bin_wall_thickness + self.bin_transparent_walls = bin_transparent_walls + self.bin_upside_down = bin_upside_down + + # half-sizes of box geom used for center part of handle (which you grab) + self.center_handle_size = list(center_handle_size) + + # half-sizes of box geoms used for adjacent parts of handle (not grabbed) + self.adjacent_handle_size = list(adjacent_handle_size) + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # bin + self.bin = Bin( + name="bin", + bin_size=self.bin_size, + wall_thickness=self.bin_wall_thickness, + transparent_walls=self.bin_transparent_walls, + rgba=rgba, + material=material, + density=density, + friction=friction, + upside_down=bin_upside_down, + ) + objects.append(self.bin) + object_locations.append([0.0, 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # handles on each side + + left_handle_1_loc = [ + 0.0, + -( + self.bin_size[1] / 2.0 + + 2.0 * self.adjacent_handle_size[1] + + self.center_handle_size[1] + ), + 0.0, + ] + left_handle_1_size = self.center_handle_size + + left_handle_2_loc = [ + (self.center_handle_size[0] - self.adjacent_handle_size[0]), + -(self.bin_size[1] / 2.0 + self.adjacent_handle_size[1]), + 0.0, + ] + left_handle_2_size = self.adjacent_handle_size + + left_handle_3_loc = [ + -(self.center_handle_size[0] - self.adjacent_handle_size[0]), + -(self.bin_size[1] / 2.0 + self.adjacent_handle_size[1]), + 0.0, + ] + left_handle_3_size = self.adjacent_handle_size + + right_handle_1_loc = [ + 0.0, + ( + self.bin_size[1] / 2.0 + + 2.0 * self.adjacent_handle_size[1] + + self.center_handle_size[1] + ), + 0.0, + ] + right_handle_1_size = self.center_handle_size + + right_handle_2_loc = [ + (self.center_handle_size[0] - self.adjacent_handle_size[0]), + (self.bin_size[1] / 2.0 + self.adjacent_handle_size[1]), + 0.0, + ] + right_handle_2_size = self.adjacent_handle_size + + right_handle_3_loc = [ + -(self.center_handle_size[0] - self.adjacent_handle_size[0]), + (self.bin_size[1] / 2.0 + self.adjacent_handle_size[1]), + 0.0, + ] + right_handle_3_size = self.adjacent_handle_size + + handle_locs = [ + left_handle_1_loc, + left_handle_2_loc, + left_handle_3_loc, + right_handle_1_loc, + right_handle_2_loc, + right_handle_3_loc, + ] + handle_sizes = [ + left_handle_1_size, + left_handle_2_size, + left_handle_3_size, + right_handle_1_size, + right_handle_2_size, + right_handle_3_size, + ] + handle_ind = 1 + for b_loc, b_size in zip(handle_locs, handle_sizes): + this_handle = BoxObject( + name="handle_{}".format(handle_ind), + size=b_size, + rgba=rgba, + material=material, + density=density, + friction=friction, + joints=None, + ) + objects.append(this_handle) + object_locations.append(b_loc) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + handle_ind += 1 + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + # total_size=body_total_size, + # locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/coffee_machine.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/coffee_machine.py new file mode 100644 index 0000000..d3a4570 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/coffee_machine.py @@ -0,0 +1,244 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject +from robocasa.models.objects.composite_body.cup import ( + CupObject, +) +from robocasa.models.objects.xml_objects import ( + CoffeeMachineBodyObject, + CoffeeMachineLidObject, + CoffeeMachineBaseObject, +) +import numpy as np + +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class CoffeeMachineObject(CompositeBodyObject): + """ + Coffee machine object with a lid fixed on a hinge joint. + """ + + def __init__( + self, + name, + add_cup=True, + pod_holder_friction=None, + joints="default", + density=1000.0, + ): + + # pieces of the coffee machine + body = CoffeeMachineBodyObject(name="body") + body_size = body.get_bounding_box_half_size() + body_location = [0.0, 0.0, 0.0] + + lid = CoffeeMachineLidObject(name="lid") + lid_size = self.lid_size = lid.get_bounding_box_half_size() + # add tolerance to allow lid to open fully + lid_location = [ + body_size[0] - lid_size[0], + 2.0 * body_size[1] + 0.01, + 2.0 * (body_size[2] - lid_size[2]) + 0.005, + ] + + # add in hinge joint to lid + hinge_pos = [0.0, -lid_size[1], 0.0] + hinge_joint = dict( + type="hinge", + axis="1 0 0", + pos=array_to_string(hinge_pos), + limited="true", + range="{} {}".format(0, 2.0 * np.pi / 3.0), + damping="0.005", + ) + body_joints = dict(lid_main=[hinge_joint]) # note: "main" gets appended to body name + lid = CoffeeMachineLidObject(name="lid") + + base = CoffeeMachineBaseObject(name="base") + base_size = base.get_bounding_box_half_size() + base_location = [body_size[0] - base_size[0], 2.0 * body_size[1], 0.0] + + pod_holder_holder = BoxObject( + name="pod_holder_holder", + size=[ + 0.01, + # tolerance for having the lid stick out a little from the holder + 0.9 * (lid_size[1] - lid_size[0]), + 0.005, + ], + rgba=[0.839, 0.839, 0.839, 1], # silver + joints=None, + ) + pod_holder_holder_size = pod_holder_holder.get_bounding_box_half_size() + pod_holder_holder_location = [ + body_size[0] - pod_holder_holder_size[0], + 2.0 * body_size[1], + # put right underneath lid + 2.0 * (body_size[2] - lid_size[2] - pod_holder_holder_size[2]), + ] + + pod_holder = CupObject( + name="pod_holder", + outer_cup_radius=lid_size[0], + inner_cup_radius=0.028, + cup_height=0.028, + cup_ngeoms=64, # 8, + cup_base_height=0.005, + cup_base_offset=0.002, + add_handle=False, + rgba=[1, 0, 0, 1], + density=density, + joints=None, + friction=pod_holder_friction, + ) + pod_holder_size = self.pod_holder_size = pod_holder.get_bounding_box_half_size() + # pod_holder_size = self.pod_holder_size = np.array([0.0295, 0.0295, 0.028 ]) + pod_holder_location = [ + body_size[0] - pod_holder_size[0], + 2.0 * (body_size[1] + pod_holder_holder_size[1]), + # put right underneath lid + 2.0 * (body_size[2] - lid_size[2] - pod_holder_size[2]), + ] + + total_size = [ + body_size[0], + body_size[1] + base_size[1], + body_size[2], + ] + + objects = [ + body, + lid, + base, + pod_holder_holder, + pod_holder, + ] + + object_locations = [ + body_location, + lid_location, + base_location, + pod_holder_holder_location, + pod_holder_location, + ] + + object_quats = [ + [0.0, 0.0, 0.0, 1.0], # z-rotate body and base by 180 + [1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + ] + + # add a rigidly mounted cup to the base + self.add_cup = add_cup + if self.add_cup: + cup = CupObject( + name="cupppp", + outer_cup_radius=0.03, + inner_cup_radius=0.025, + cup_height=0.025, + cup_ngeoms=64, # 8, + cup_base_height=0.005, + cup_base_offset=0.005, + add_handle=True, + handle_outer_radius=0.015, + handle_inner_radius=0.010, + handle_thickness=0.003, + handle_ngeoms=64, + rgba=[0.839, 0.839, 0.839, 1], + density=1000.0, + joints=None, + ) + cup_total_size = cup.get_bounding_box_half_size() + # cup_total_size = np.array([0.03 , 0.045, 0.025]) + objects.append(cup) + object_locations.append( + [ + body_size[0] - cup_total_size[0], + 2.0 * (body_size[1] + pod_holder_holder_size[1]) + + pod_holder_size[1] + - cup_total_size[1], + 2.0 * base_size[2], + ] + ) + rot_angle = -np.pi / 2.0 + object_quats.append([np.cos(rot_angle / 2), 0, 0, np.sin(rot_angle / 2)]) + + object_parents = [None] * len(objects) + + """ + Variables to compare: + + objects + [, + , + , + , + , + ] + + object_locations + [[0.0, 0.0, 0.0], + [0.056999999999999995, 0.21100000000000002, 0.20700000000000002], + [0.04449999999999999, 0.201, 0.0], + [0.0765, 0.201, 0.192], + [0.056999999999999995, 0.22710000000000002, 0.14600000000000002], + [0.056499999999999995, 0.21160000000000007, 0.01]] + + object_quats + [[0.0, 0.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + [0.7071067811865476, 0, 0, -0.7071067811865475]] + + body_size + array([0.0865, 0.1005, 0.1105]) + + lid_size + array([0.0295, 0.044 , 0.0095]) + + lid_location + [0.056999999999999995, 0.21100000000000002, 0.20700000000000002] + + base_size + array([0.042, 0.05 , 0.005]) + + base_location + [0.04449999999999999, 0.201, 0.0] + + pod_holder_holder_size + array([0.01 , 0.01305, 0.005 ]) + + pod_holder_holder_location + [0.0765, 0.201, 0.192] + + pod_holder_size + array([0.0295, 0.0295, 0.028 ]) + + pod_holder_location + [0.056999999999999995, 0.22710000000000002, 0.14600000000000002] + + total_size + [0.0865, 0.15050000000000002, 0.1105] + + cup.total_size + array([0.03 , 0.045, 0.025]) + """ + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + body_joints=body_joints, # make sure hinge joint is added + joints=joints, + # joints="default", # coffee machine can move + # joints=None, # coffee machine does not move + total_size=total_size, + locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/cup.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/cup.py new file mode 100644 index 0000000..e5afb17 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/cup.py @@ -0,0 +1,183 @@ +from robosuite.models.objects import ( + CompositeBodyObject, + BoxObject, + CylinderObject, + HollowCylinderObject, +) +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class CupObject(CompositeBodyObject): + """ + Cup object with optional handle. + """ + + def __init__( + self, + name, + outer_cup_radius=0.0425, + inner_cup_radius=0.03, + cup_height=0.05, + cup_ngeoms=8, + cup_base_height=0.01, + cup_base_offset=0.005, + add_handle=False, + handle_outer_radius=0.03, + handle_inner_radius=0.015, + handle_thickness=0.005, + handle_ngeoms=8, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + ): + + # Object properties + + # radius of the inner cup hole and entire cup + self.r1 = inner_cup_radius + self.r2 = outer_cup_radius + + # number of geoms used to approximate the cylindrical shell + self.n = cup_ngeoms + + # cup half-height + self.cup_height = cup_height + + # cup base args + self.cup_base_height = cup_base_height + self.cup_base_offset = cup_base_offset + + # handle args + self.add_handle = add_handle + self.handle_outer_radius = handle_outer_radius + self.handle_inner_radius = handle_inner_radius + self.handle_thickness = handle_thickness + self.handle_ngeoms = handle_ngeoms + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # cup body + self.cup_body = HollowCylinderObject( + name="cup_body", + outer_radius=self.r2, + inner_radius=self.r1, + height=self.cup_height, + ngeoms=self.n, + rgba=rgba, + material=material, + density=density, + friction=friction, + ) + objects.append(self.cup_body) + object_locations.append([0.0, 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # cup base + self.cup_base = CylinderObject( + name="cup_base", + size=[self.cup_body.int_r, self.cup_base_height], + rgba=rgba, + material=material, + density=density, + solref=[0.02, 1.0], + solimp=[0.998, 0.998, 0.001], + joints=None, + ) + objects.append(self.cup_base) + object_locations.append( + [0.0, 0.0, -self.cup_height + self.cup_base_height + self.cup_base_offset] + ) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + if self.add_handle: + # cup handle is a hollow half-cylinder + self.cup_handle = HollowCylinderObject( + name="cup_handle", + outer_radius=self.handle_outer_radius, + inner_radius=self.handle_inner_radius, + height=self.handle_thickness, + ngeoms=self.handle_ngeoms, + rgba=rgba, + material=material, + density=density, + make_half=True, + friction=friction, + ) + # translate handle to right side of cup body, and rotate by +90 degrees about y-axis + # to orient the handle geoms on the cup body + objects.append(self.cup_handle) + object_locations.append([0.0, (self.cup_body.r2 + self.cup_handle.unit_box_width), 0.0]) + object_quats.append( + T.convert_quat( + T.mat2quat( + T.rotation_matrix(angle=np.pi / 2.0, direction=[0.0, 1.0, 0.0])[:3, :3] + ), + to="wxyz", + ) + ) + object_parents.append(None) + + # total size of cup + body_total_size = [self.r2, self.r2, self.cup_height] + if self.add_handle: + body_total_size[1] += self.handle_outer_radius + + """ + To match on cup 1: + + objects + [, + ] + + body_total_size + [0.0295, 0.0295, 0.028] + + object_locations + [[0.0, 0.0, 0.0], [0.0, 0.0, -0.020999999999999998]] + + object_quats + [[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]] + + + To match on cup 2: + + objects + [, + , + ] + + body_total_size + [0.03, 0.045, 0.025] + + object_locations + [[0.0, 0.0, 0.0], [0.0, 0.0, -0.015], [0.0, 0.03073601511491127, 0.0]] + + object_quats + [[1.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + array([ 0.70710677, -0. , 0.70710677, -0. ], dtype=float32)] + """ + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + total_size=body_total_size, + # locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/inverse_stacked_cylinder.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/inverse_stacked_cylinder.py new file mode 100644 index 0000000..13ce6f1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/inverse_stacked_cylinder.py @@ -0,0 +1,139 @@ +from robosuite.models.objects import ( + CompositeBodyObject, + BoxObject, + CylinderObject, + HollowCylinderObject, +) +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class InverseStackedCylinderObject(CompositeBodyObject): + """ + Inverse of stacked cylinder object, where the top piece is a hollow cylinder object + and the bottom piece is a cylinder object. Optionally add a square base for stability. + """ + + def __init__( + self, + name, + radius_1, + radius_2, + height_1, + height_2, + ngeoms=64, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + square_base_width=None, + square_base_height=None, + ): + + # Object properties + + # radius of first (bottom) cylinder and inner radius of second (top) hollow cylinder + self.r1 = radius_1 + self.r2 = radius_2 + + # half-height of first (bottom) cylinder and second (top) hollow cylinder + self.h1 = height_1 + self.h2 = height_2 + + # num geoms to approximate the hollow cylinder + self.ngeoms = ngeoms + + # whether to add square base + self.add_square_base = (square_base_width is not None) and (square_base_height is not None) + + # half-width and half-height for square base + self.square_base_width = square_base_width + self.square_base_height = square_base_height + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # NOTE: we will place the object frame at the vertical center of the two stacked cylinders + z_center = (self.h1 + self.h2) / 2.0 + c1_offset = self.h1 - z_center + c2_offset = 2.0 * self.h1 + self.h2 - z_center + + # first (bottom) cylinder + self.cylinder_1 = CylinderObject( + name="cylinder_1", + size=[self.r1, self.h1], + rgba=rgba, + material=material, + density=density, + friction=friction, + solref=[0.02, 1.0], + # solimp=[0.998, 0.998, 0.001], + solimp=[0.9, 0.95, 0.001], + joints=None, + ) + objects.append(self.cylinder_1) + object_locations.append([0.0, 0.0, c1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # second (top) hollow cylinder + self.cylinder_2 = HollowCylinderObject( + name="cylinder_2", + outer_radius=self.r1, # match radius of first cylinder + inner_radius=self.r2, + height=self.h2, + ngeoms=self.ngeoms, + rgba=rgba, + material=material, + density=density, + friction=friction, + # TODO: maybe tune solimp and try (0.998, 0.998, 0.001) + solref=[0.02, 1.0], + solimp=[0.9, 0.95, 0.001], + # solimp=(0.998, 0.998, 0.001), + ) + objects.append(self.cylinder_2) + object_locations.append([0.0, 0.0, c2_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # # total size of object + # max_r = max(self.r1, self.r2) + # body_total_size = [max_r, max_r, self.h1 + self.h2] + + if self.add_square_base: + # add square base underneath bottom cylinder + s1_offset = c1_offset - (self.square_base_height + self.h1) + self.square_base = BoxObject( + name="square_base", + size=[ + self.square_base_width, + self.square_base_width, + self.square_base_height, + ], + rgba=rgba, + material=material, + ) + objects.append(self.square_base) + object_locations.append([0.0, 0.0, s1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + # total_size=body_total_size, + # locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/lightbulb.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/lightbulb.py new file mode 100644 index 0000000..7d8d584 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/lightbulb.py @@ -0,0 +1,153 @@ +from robosuite.models.objects import ( + CompositeBodyObject, + BoxObject, + CylinderObject, + BallObject, +) +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string, new_site +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class BallObjectWithSite(BallObject): + """ + A ball object with a inner site (used for the bulb). + """ + + def _get_object_subtree(self): + # tree = super()._get_object_subtree() + tree = self._get_object_subtree_(ob_type="sphere") + site_element_attr = self.get_site_attrib_template() + + site_element_attr["pos"] = "0 0 0.1" + site_element_attr["name"] = "center_site" + site_element_attr["size"] = "{} {} {}".format( + 2.0 * self.size[0], 2.0 * self.size[0], 2.0 * self.size[0] + ) + site_element_attr["rgba"] = "1.0 0.0 0.0 1.0" + site_element_attr["group"] = "1" + # site_element_attr["rgba"] = "1.0 0.976 0.839 0.9" + # site_element_attr["rgba"] = "1.0 0.976 0.839 0.0" + tree.append(new_site(**site_element_attr)) + return tree + + +class LightbulbObject(CompositeBodyObject): + """ + A simple lightbulb constructed out of a base of alternating radius cylinders + and a sphere on top. + """ + + def __init__( + self, + name, + radius_low, + radius_high, + cylinder_height, + num_cylinders, + sphere_radius, + joints="default", + density=100.0, + friction=None, + ): + + # Object properties + + # radii of alternating cylinders for base + self.radius_low = radius_low + self.radius_high = radius_high + + # half-height of each cylinder + self.cylinder_height = cylinder_height + + # number of cylinders for base + self.num_cylinders = num_cylinders + + # radius of sphere at top + self.sphere_radius = sphere_radius + + # toggle between translucent and yellow + self.translucent_rgba = (1.0, 1.0, 1.0, 0.3) + self.yellow_rgba = (1.0, 0.976, 0.839, 0.7) + # self.yellow_rgba = (0.0, 0.0, 0.0, 0.0) + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # NOTE: we will place the object frame at the vertical center of all the stacked objects + self.z_center = ( + (2.0 * self.cylinder_height) * self.num_cylinders + 2.0 * self.sphere_radius + ) / 2.0 + + metal = CustomMaterial( + texture="Metal", + tex_name="metal", + mat_name="MatMetal", + tex_attrib={"type": "cube"}, + mat_attrib={"specular": "1", "shininess": "0.3", "rgba": "0.9 0.9 0.9 1"}, + ) + + # we will define all objects relative to the bottom of the object, and then subtract the z_center value + for cylinder_ind in range(self.num_cylinders): + r = self.radius_low if ((cylinder_ind % 2) == 0) else self.radius_high + cyl_obj = CylinderObject( + name="cylinder_{}".format(cylinder_ind), + size=[r, self.cylinder_height], + rgba=None, + material=metal, + density=density, + friction=friction, + solref=[0.02, 1.0], + solimp=[0.9, 0.95, 0.001], + joints=None, + ) + objects.append(cyl_obj) + z_cyl = (2.0 * cylinder_ind + 1) * self.cylinder_height + object_locations.append([0.0, 0.0, z_cyl]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # then add translucent sphere at top + self.bulb = BallObject( + name="bulb", + size=[self.sphere_radius], + density=density, + friction=friction, + rgba=self.translucent_rgba, + material=None, + joints=None, + ) + objects.append(self.bulb) + z_bulb = object_locations[-1][2] + self.cylinder_height + self.sphere_radius + object_locations.append([0.0, 0.0, z_bulb]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # do frame conversion from bottom of object to z_center + object_locations = [[loc[0], loc[1], loc[2] - self.z_center] for loc in object_locations] + + # add site that can be toggled to turn lightbulb on + sites = [ + dict( + name="bulb_on", + pos=array_to_string(object_locations[-1]), + size="{}".format(0.95 * self.sphere_radius), + rgba=array_to_string(self.yellow_rgba), + ) + ] + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + sites=sites, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/sliding_box.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/sliding_box.py new file mode 100644 index 0000000..0e6542b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/sliding_box.py @@ -0,0 +1,132 @@ +import numpy as np + +from robosuite.models.objects import BoxObject, CompositeBodyObject, CylinderObject +from robosuite.utils.mjcf_utils import BLUE, RED, CustomMaterial, array_to_string + + +class SlidingBoxObject(CompositeBodyObject): + """ + An example object that demonstrates the CompositeBodyObject functionality. This object consists of two cube bodies + joined together by a slide joint allowing one box to slide on top of the other. + + Args: + name (str): Name of this object + + box1_size (3-array): (L, W, H) half-sizes for the first box + + box2_size (3-array): (L, W, H) half-sizes for the second box + + use_texture (bool): set True if using wood textures for the blocks + """ + + def __init__( + self, + name, + box1_size=(0.1, 0.1, 0.02), + box2_size=(0.02, 0.02, 0.02), + use_texture=True, + ): + # Set box sizes + self.box1_size = np.array(box1_size) + self.box2_size = np.array(box2_size) + + # Set box densities + self.box1_density = 10000.0 + self.box2_density = 100.0 + + # Set texture attributes + self.use_texture = use_texture + self.box1_material = None + self.box2_material = None + self.box1_rgba = RED + self.box2_rgba = BLUE + + # Define materials we want to use for this object + if self.use_texture: + # Remove RGBAs + self.box1_rgba = None + self.box2_rgba = None + + # Set materials for each box + tex_attrib = { + "type": "cube", + } + mat_attrib = { + "texrepeat": "3 3", + "specular": "0.4", + "shininess": "0.1", + } + self.box1_material = CustomMaterial( + texture="WoodRed", + tex_name="box1_tex", + mat_name="box1_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + self.box2_material = CustomMaterial( + texture="WoodBlue", + tex_name="box2_tex", + mat_name="box2_mat", + tex_attrib=tex_attrib, + mat_attrib=mat_attrib, + ) + + # Create objects + objects = [] + for i, (size, mat, rgba, density) in enumerate( + zip( + (self.box1_size, self.box2_size), + (self.box1_material, self.box2_material), + (self.box1_rgba, self.box2_rgba), + (self.box1_density, self.box2_density), + ) + ): + objects.append( + BoxObject( + name=f"box{i + 1}", + size=size, + rgba=rgba, + material=mat, + ) + ) + + # Define slide joint + rel_joint_pos = [0, 0, 0] # at second box + joint_lim = self.box1_size[1] - self.box2_size[1] + slide_joint = { + "name": "box_slide", + "type": "slide", + "axis": "0 1 0", # y-axis slide + "pos": array_to_string(rel_joint_pos), + "springref": "0", + "springdamper": "0.1 1.0", # mass-spring system with 0.1 time constant, 1.0 damping ratio + "limited": "true", + "range": "{} {}".format(-joint_lim, joint_lim), + } + + # Define positions -- second box should lie on top of first box + positions = [ + np.zeros(3), # First box is centered at top-level body anyways + np.array([0, 0, self.box1_size[2] + self.box2_size[2]]), + ] + + quats = [ + None, # Default quaternion for box 1 + None, # Default quaternion for box 2 + ] + + # Define parents -- which body each is aligned to + parents = [ + None, # box 1 attached to top-level body + objects[0].root_body, # box 2 attached to box 1 + ] + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=positions, + object_quats=quats, + object_parents=parents, + body_joints={objects[1].root_body: [slide_joint]}, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/socket.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/socket.py new file mode 100644 index 0000000..4869a23 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/socket.py @@ -0,0 +1,334 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject, CylinderObject +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial +from .stacked_cylinder import StackedCylinderObject + + +class SocketObject(CompositeBodyObject): + """ + Box geom connected to series of geoms with ball joints (wire) and connected to + cylinder representing plug. + """ + + def __init__( + self, + name, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + socket_base_size=(0.02, 0.02, 0.03), + wire_box_geom_size=(0.02, 0.005, 0.005), + wire_box_geom_rgba=(0.0, 0.0, 0.0, 1.0), + num_box_geoms_up=6, + num_box_geoms_left=2, + num_box_geoms_down=9, + merge_box_geoms=False, + merge_size=1, + cylinder_args=None, + ): + + # Object properties + + # box geom used for socket base + self.socket_base_size = socket_base_size + + # box geoms used for wire + self.wire_box_geom_size = wire_box_geom_size + self.wire_box_geom_rgba = wire_box_geom_rgba + + # wire parameters - number of geoms to use for up, left, and down portions + self.num_box_geoms_up = num_box_geoms_up + self.num_box_geoms_left = num_box_geoms_left + self.num_box_geoms_down = num_box_geoms_down + + # if true, merge the box geoms along each direction of the wire into a single box geom + self.merge_box_geoms = merge_box_geoms + + # number of box geoms to use for each merged size (set to higher than 1 to merge the geoms into more + # than one box geom) + self.merge_size = merge_size + + if cylinder_args is None: + # default cylinder args + cylinder_args = dict( + # bottom cylinder radius and half-height + radius_1=0.03, + height_1=0.075, + # top cylinder radius and half-height + radius_2=0.02, + height_2=0.025, + rgba=[0.839, 0.839, 0.839, 1], + density=1000.0, + # add square base + square_base_width=0.03, + square_base_height=0.005, + ) + self.cylinder_args = dict(cylinder_args) + self.cylinder_args["joints"] = None + + # materials + box_geom_material = CustomMaterial( + texture="Metal", + tex_name="metal", + mat_name="MatMetal", + tex_attrib={"type": "cube"}, + mat_attrib={"specular": "1", "shininess": "0.3", "rgba": "0.9 0.9 0.9 1"}, + ) + socket_material = CustomMaterial( + texture="WoodLight", + tex_name="lightwood", + mat_name="lightwood_mat", + tex_attrib={"type": "cube"}, + mat_attrib={"texrepeat": "1 1", "specular": "0.4", "shininess": "0.1"}, + ) + + # params for ball joints used + + # + ball_joint_spec = { + "type": "ball", + "pos": "{} 0 0".format(self.wire_box_geom_size[0]), + "springref": "0", + "springdamper": "0.1 1.0", # mass-spring system with 0.1 time constant, 1.0 damping ratio + "limited": "true", + "range": "0 {}".format(np.pi / 4), + } + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + object_joints = dict() + + # NOTE: For absolute object locations (objects not defined relative to parent) we will use the socket base frame + # as a frame of reference, and add an offset from the center of the object (approximated via full width) + # to it. + + # get an approximate x-y bounding box below, and place the center there, and define offset relative to stove base center + approx_full_width = self.num_box_geoms_left * (2.0 * self.wire_box_geom_size[0]) + ( + 2.0 * self.socket_base_size[1] + ) + approx_full_height = (self.num_box_geoms_down + 2) * (2.0 * self.wire_box_geom_size[0]) + + base_x_off = ( + self.socket_base_size[0] + (self.num_box_geoms_up) * (2.0 * self.wire_box_geom_size[0]) + ) - (approx_full_height / 2.0) + base_y_off = (self.num_box_geoms_left * self.wire_box_geom_size[0]) + self.socket_base_size[ + 1 + ] + + # base of socket + self.socket_base = BoxObject( + name="stove_base", + size=list(self.socket_base_size), + rgba=rgba, + material=socket_material, + joints=None, + ) + objects.append(self.socket_base) + object_locations.append([base_x_off, base_y_off, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # chain above base + chain_ind = 0 + up_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_up - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + up_chain_size[0] *= self.num_box_geoms_up + up_chain_size[0] /= self.merge_size + # number of additional geoms to add + num_geoms_iter = self.merge_size - 1 + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(up_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + objects.append(chain_obj) + object_locations.append( + [ + base_x_off - (self.socket_base_size[0] + up_chain_size[0]), + base_y_off, + 0.0, + ] + ) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + if self.merge_box_geoms: + # add ball joint and make sure to place it at the edge of the geom + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(up_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + for i in range(num_geoms_iter): + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(up_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(chain_obj) + object_locations.append([-2.0 * up_chain_size[0], 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + # add ball joint and make sure to place it at the edge of the geom + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(up_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + # add chain in left direction + rot_quat = T.convert_quat( + T.axisangle2quat(np.array([0.0, 0.0, 1.0]) * (np.pi / 2.0)), to="wxyz" + ) + left_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_left - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + left_chain_size[0] *= self.num_box_geoms_left + # HACK: merge entire left chain since this side is small + left_merge_size = 1 + left_chain_size[0] /= left_merge_size + # number of additional geoms to add + num_geoms_iter = left_merge_size - 1 + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(left_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(chain_obj) + object_locations.append([-up_chain_size[0], -left_chain_size[0], 0.0]) + object_quats.append(rot_quat) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(left_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + for i in range(num_geoms_iter): + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(left_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(chain_obj) + object_locations.append([-2.0 * left_chain_size[0], 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(left_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + # add chain in downward direction + rot_quat = T.convert_quat( + T.axisangle2quat(np.array([0.0, 0.0, 1.0]) * (np.pi / 2.0)), to="wxyz" + ) + down_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_down - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + down_chain_size[0] *= self.num_box_geoms_down + down_chain_size[0] /= self.merge_size + # number of additional geoms to add + num_geoms_iter = self.merge_size - 1 + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(down_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(chain_obj) + object_locations.append([-left_chain_size[0], -down_chain_size[0], 0.0]) + object_quats.append(rot_quat) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(down_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + for i in range(num_geoms_iter): + chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(down_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(chain_obj) + object_locations.append([-2.0 * down_chain_size[0], 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "{} 0 0".format(down_chain_size[0]) + object_joints[chain_obj.root_body] = [ball_joint] + + # add cylinder object + self.cylinder_obj = StackedCylinderObject( + name="cylinder_obj", + **self.cylinder_args, + ) + + x_off = -(down_chain_size[0] + self.cylinder_obj.square_base_width) + y_off = 0.0 + z_off = ((self.cylinder_obj.h1 + self.cylinder_obj.h2) / 2.0) - down_chain_size[2] + parent = objects[-1].root_body + objects.append(self.cylinder_obj) + object_locations.append([x_off, y_off, z_off]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + # # add debug site to see object center + # sites = [ + # dict( + # name="TMP", + # pos=array_to_string([0., 0., 0.]), + # size="{}".format(0.1), + # rgba=array_to_string([1., 0., 0., 1.]), + # ) + # ] + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + body_joints=object_joints, + # sites=sites, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/spray_bottle.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/spray_bottle.py new file mode 100644 index 0000000..d8d2e1e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/spray_bottle.py @@ -0,0 +1,207 @@ +import numpy as np + +from robosuite.models.objects import ( + BoxObject, + CompositeBodyObject, + CylinderObject, + ConeObject, +) +from robosuite.utils.mjcf_utils import BLUE, RED, CustomMaterial, array_to_string + + +class SprayBottleObject(CompositeBodyObject): + """ + A spray bottle object where the trigger is modeled via a slide joint. + """ + + def __init__( + self, + name, + base_cylinder_radius, + base_cylinder_height, + cone_outer_radius, + cone_inner_radius, + cone_height, + cone_ngeoms, + neck_height, + top_length, + top_thickness, + trigger_height, + trigger_width, + trigger_length, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + square_base_width=None, + square_base_height=None, + ): + # Sizes + self.base_cylinder_radius = base_cylinder_radius + self.base_cylinder_height = base_cylinder_height + self.cone_outer_radius = cone_outer_radius + self.cone_inner_radius = cone_inner_radius + self.cone_height = cone_height + self.cone_ngeoms = cone_ngeoms + self.neck_height = neck_height + self.top_length = top_length + self.top_thickness = top_thickness + self.trigger_height = trigger_height + self.trigger_width = trigger_width + self.trigger_length = trigger_length + + # whether to add square base + self.add_square_base = (square_base_width is not None) and (square_base_height is not None) + + # half-width and half-height for square base + self.square_base_width = square_base_width + self.square_base_height = square_base_height + + assert self.top_length > self.cone_inner_radius, "top needs to stick out past neck" + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # base cylinder + self.base_cylinder = CylinderObject( + name="base_cylinder", + size=[self.base_cylinder_radius, self.base_cylinder_height], + rgba=rgba, + material=material, + density=density, + friction=friction, + solref=[0.02, 1.0], + # solimp=[0.998, 0.998, 0.001], + solimp=[0.9, 0.95, 0.001], + joints=None, + ) + objects.append(self.base_cylinder) + object_locations.append([0.0, 0.0, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # cone that is part of neck + # z_cone = self.base_cylinder_height # cone frame is bottom center of cone + z_cone = self.base_cylinder_height + (self.cone_height / 2.0) + self.neck_cone = ConeObject( + name="neck_cone", + outer_radius=self.cone_outer_radius, + inner_radius=self.cone_inner_radius, + height=self.cone_height, # full height, not half-height + ngeoms=self.cone_ngeoms, + use_box=False, + rgba=rgba, + material=material, + density=density, + solref=(0.02, 1.0), + # solimp=[0.998, 0.998, 0.001], + solimp=(0.9, 0.95, 0.001), + friction=friction, + ) + objects.append(self.neck_cone) + object_locations.append([0.0, 0.0, z_cone]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # cylinder that is part of neck + z_cyl = self.base_cylinder_height + self.cone_height + self.neck_height + self.neck_cylinder = CylinderObject( + name="neck_cylinder", + size=[self.cone_inner_radius, self.neck_height], # match cone radius + rgba=rgba, + material=material, + density=density, + friction=friction, + solref=[0.02, 1.0], + # solimp=[0.998, 0.998, 0.001], + solimp=[0.9, 0.95, 0.001], + joints=None, + ) + objects.append(self.neck_cylinder) + object_locations.append([0.0, 0.0, z_cyl]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # top box object + box_x_off = -(self.top_length - self.cone_inner_radius) + box_z_off = z_cyl + self.neck_height + self.top_thickness + self.top_box = BoxObject( + name="top_box", + size=[self.top_length, self.cone_inner_radius, self.top_thickness], + density=density, + friction=friction, + rgba=rgba, + material=material, + joints=None, + ) + objects.append(self.top_box) + object_locations.append([box_x_off, 0.0, box_z_off]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # trigger object, which is defined relative to top box object + trigger_x_off = -(0.5 * self.top_length) # place halfway along length of top object + trigger_z_off = -(self.top_thickness + self.trigger_height) + self.trigger = BoxObject( + name="trigger", + size=[self.trigger_length, self.trigger_width, self.trigger_height], + density=density, + friction=friction, + rgba=rgba, + material=material, + joints=None, + ) + objects.append(self.trigger) + object_locations.append([trigger_x_off, 0.0, trigger_z_off]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(objects[-2].root_body) # NOTE: relative to top box object + + # define slide joint for trigger + rel_joint_pos = [0, 0, 0] # at trigger + joint_lim_min = 0.0 + joint_lim_max = ( + self.top_length + ) # trigger was shifted to left by 0.5 * top_length, so allow it to go to right by top_length + slide_joint = { + "name": "trigger_slide", + "type": "slide", + "axis": "1 0 0", # x-axis slide + "pos": array_to_string(rel_joint_pos), + "springref": "0", + "springdamper": "0.1 1.0", # mass-spring system with 0.1 time constant, 1.0 damping ratio + "limited": "true", + "range": "{} {}".format(joint_lim_min, joint_lim_max), + } + + if self.add_square_base: + # add square base underneath bottom cylinder + s1_offset = -(self.square_base_height + self.base_cylinder_height) + self.square_base = BoxObject( + name="square_base", + size=[ + self.square_base_width, + self.square_base_width, + self.square_base_height, + ], + rgba=rgba, + material=material, + ) + objects.append(self.square_base) + object_locations.append([0.0, 0.0, s1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + body_joints={objects[-1].root_body: [slide_joint]}, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_box.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_box.py new file mode 100644 index 0000000..c2c7991 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_box.py @@ -0,0 +1,111 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject, CylinderObject, Bin +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class StackedBoxObject(CompositeBodyObject): + """ + Two boxes - one stacked on top of the other. + """ + + def __init__( + self, + name, + box_1_size, + box_2_size, + joints="default", + box_1_rgba=None, + box_2_rgba=None, + box_1_material=None, + box_2_material=None, + density=100.0, + friction=None, + make_box_2_transparent=False, + ): + + # Object properties + + # half-sizes of first (bottom) box + self.box_1_size = list(box_1_size) + + # half-sizes of second (top) box + self.box_2_size = list(box_2_size) + + # maybe make box 2 have transparent top and bottom walls + self.make_box_2_transparent = make_box_2_transparent + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # NOTE: we will place the object frame at the vertical center of the two stacked boxes + z_center = (self.box_1_size[2] + self.box_2_size[2]) / 2.0 + b1_offset = self.box_1_size[2] - z_center + b2_offset = 2.0 * self.box_1_size[2] + self.box_2_size[2] - z_center + + # first (bottom) box + self.box_1 = BoxObject( + name="box_1", + size=self.box_1_size, + rgba=box_1_rgba, + material=box_1_material, + density=density, + friction=friction, + joints=None, + ) + objects.append(self.box_1) + object_locations.append([0.0, 0.0, b1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # second (top) box + if self.make_box_2_transparent: + self.box_2 = Bin( + name="box_2", + bin_size=( + 2.0 * self.box_2_size[0], + 2.0 * self.box_2_size[1], + 2.0 * self.box_2_size[2], + ), + wall_thickness=0.01, + transparent_walls=False, + friction=friction, + density=density, + use_texture=True, + rgba=box_2_rgba, + material=box_2_material, + upside_down=False, + add_second_base=True, + transparent_base=True, + ) + else: + self.box_2 = BoxObject( + name="box_2", + size=self.box_2_size, + rgba=box_2_rgba, + material=box_2_material, + density=density, + friction=friction, + joints=None, + ) + objects.append(self.box_2) + object_locations.append([0.0, 0.0, b2_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + # total_size=body_total_size, + # locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_cylinder.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_cylinder.py new file mode 100644 index 0000000..cf8cf14 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stacked_cylinder.py @@ -0,0 +1,127 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject, CylinderObject +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial + + +class StackedCylinderObject(CompositeBodyObject): + """ + Two cylinders - one stacked on top of the other. + Optionally add a square base for stability. + """ + + def __init__( + self, + name, + radius_1, + radius_2, + height_1, + height_2, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + square_base_width=None, + square_base_height=None, + ): + + # Object properties + + # radius of first (bottom) cylinder and second (top) cylinder + self.r1 = radius_1 + self.r2 = radius_2 + + # half-height of first (bottom) cylinder and second (top) cylinder + self.h1 = height_1 + self.h2 = height_2 + + # whether to add square base + self.add_square_base = (square_base_width is not None) and (square_base_height is not None) + + # half-width and half-height for square base + self.square_base_width = square_base_width + self.square_base_height = square_base_height + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + + # NOTE: we will place the object frame at the vertical center of the two stacked cylinders + z_center = (self.h1 + self.h2) / 2.0 + c1_offset = self.h1 - z_center + c2_offset = 2.0 * self.h1 + self.h2 - z_center + + # first (bottom) cylinder + self.cylinder_1 = CylinderObject( + name="cylinder_1", + size=[self.r1, self.h1], + rgba=rgba, + material=material, + density=density, + friction=friction, + solref=[0.02, 1.0], + solimp=[0.9, 0.95, 0.001], + # solimp=[0.998, 0.998, 0.001], + joints=None, + ) + objects.append(self.cylinder_1) + object_locations.append([0.0, 0.0, c1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # second (top) cylinder + self.cylinder_2 = CylinderObject( + name="cylinder_2", + size=[self.r2, self.h2], + rgba=rgba, + material=material, + density=density, + friction=friction, + solref=[0.02, 1.0], + # solimp=[0.998, 0.998, 0.001], + solimp=[0.9, 0.95, 0.001], + joints=None, + ) + objects.append(self.cylinder_2) + object_locations.append([0.0, 0.0, c2_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # # total size of object + # max_r = max(self.r1, self.r2) + # body_total_size = [max_r, max_r, self.h1 + self.h2] + + if self.add_square_base: + # add square base underneath bottom cylinder + s1_offset = c1_offset - (self.square_base_height + self.h1) + self.square_base = BoxObject( + name="square_base", + size=[ + self.square_base_width, + self.square_base_width, + self.square_base_height, + ], + rgba=rgba, + material=material, + ) + objects.append(self.square_base) + object_locations.append([0.0, 0.0, s1_offset]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + # total_size=body_total_size, + # locations_relative_to_corner=True, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stove_plug.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stove_plug.py new file mode 100644 index 0000000..4f71ef9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/composite_body/stove_plug.py @@ -0,0 +1,396 @@ +from robosuite.models.objects import CompositeBodyObject, BoxObject, CylinderObject +import numpy as np + +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string +from robosuite.utils.mjcf_utils import RED, BLUE, CustomMaterial +from .inverse_stacked_cylinder import InverseStackedCylinderObject +from .lightbulb import LightbulbObject + +import robosuite_task_zoo +from robosuite_task_zoo.models.kitchen import StoveObject + + +class StoveObjectNew(StoveObject): + """ + Override some offsets for placement sampler. + """ + + @property + def bottom_offset(self): + # unused since we directly hardcode z + return np.array([0, 0, -0.02]) + + @property + def top_offset(self): + # unused since we directly hardcode z + return np.array([0, 0, 0.02]) + + @property + def horizontal_radius(self): + return 0.1 + + +class StovePlugObject(CompositeBodyObject): + """ + Stove on wooden block with chain (plug) connected. Optionally replace the stove + with a lightbulb if @lightbulb_args is provided. + """ + + def __init__( + self, + name, + joints="default", + rgba=None, + material=None, + density=100.0, + friction=None, + stove_base_size=(0.12, 0.12, 0.01), + stove_z_half_size=0.025, + wire_box_geom_size=(0.005, 0.02, 0.005), + wire_box_geom_rgba=(0.0, 0.0, 0.0, 1.0), + num_box_geoms_left=5, + num_box_geoms_vert=8, + num_box_geoms_right=8, + merge_box_geoms=False, + merge_size=1, + cylinder_args=None, + lightbulb_args=None, + ): + + # Object properties + + # half sizes for stove base box object + # self.stove_base_size = (0.1, 0.1, 0.02) + self.stove_base_size = stove_base_size + self.stove_z_half_size = stove_z_half_size # note: estimated approximately + + # box geoms used for wire + self.wire_box_geom_size = wire_box_geom_size + self.wire_box_geom_rgba = wire_box_geom_rgba + + # wire parameters - number of geoms to use for left, down, and right portions + self.num_box_geoms_left = num_box_geoms_left + self.num_box_geoms_vert = num_box_geoms_vert + self.num_box_geoms_right = num_box_geoms_right + + # if true, merge the box geoms along each direction of the wire into a single box geom + self.merge_box_geoms = merge_box_geoms + + # number of box geoms to use for each merged size (set to higher than 1 to merge the geoms into more + # than one box geom) + self.merge_size = merge_size + + if cylinder_args is None: + # default cylinder args + cylinder_args = dict( + # bottom cylinder radius and half-height + radius_1=0.03, + height_1=0.01, + # top hollow cylinder inner radius and half-height + radius_2=0.025, + height_2=0.025, + # NOTE: reduce to 8 geoms if desired + ngeoms=64, + rgba=[0.839, 0.839, 0.839, 1], + density=1000.0, + # add square base + square_base_width=0.03, + square_base_height=0.005, + ) + self.cylinder_args = dict(cylinder_args) + self.cylinder_args["joints"] = None + + self.use_lightbulb = lightbulb_args is not None + if self.use_lightbulb: + self.lightbulb_args = dict(lightbulb_args) + self.lightbulb_args["joints"] = None + + # materials + box_geom_material = CustomMaterial( + texture="Metal", + tex_name="metal", + mat_name="MatMetal", + tex_attrib={"type": "cube"}, + mat_attrib={"specular": "1", "shininess": "0.3", "rgba": "0.9 0.9 0.9 1"}, + ) + stove_base_material = CustomMaterial( + texture="WoodLight", + tex_name="lightwood", + mat_name="lightwood_mat", + tex_attrib={"type": "cube"}, + mat_attrib={"texrepeat": "1 1", "specular": "0.4", "shininess": "0.1"}, + ) + + # params for ball joints used + + # + ball_joint_spec = { + "type": "ball", + "pos": "0 {} 0".format(self.wire_box_geom_size[1]), + "springref": "0", + "springdamper": "0.1 1.0", # mass-spring system with 0.1 time constant, 1.0 damping ratio + "limited": "true", + "range": "0 {}".format(np.pi / 4), + } + + # Create objects + objects = [] + object_locations = [] + object_quats = [] + object_parents = [] + object_joints = dict() + + # NOTE: For absolute object locations (objects not defined relative to parent) we will use the stove base frame + # as a frame of reference, and add an offset from the center of the object (approximated via full width) + # to it. + + # get an approximate x-y bounding box below, and place the center there, and define offset relative to stove base cente + approx_full_width = self.num_box_geoms_left * (2.0 * self.wire_box_geom_size[1]) + ( + 2.0 * self.stove_base_size[1] + ) + approx_full_height = (self.num_box_geoms_vert + 2) * (2.0 * self.wire_box_geom_size[1]) + + stove_base_x_off = -(approx_full_height / 2.0) + self.stove_base_size[0] + stove_base_y_off = (approx_full_width / 2.0) - self.stove_base_size[1] + + # base of stove + self.stove_base = BoxObject( + name="stove_base", + size=list(self.stove_base_size), + rgba=rgba, + material=stove_base_material, + joints=None, + ) + objects.append(self.stove_base) + object_locations.append([stove_base_x_off, stove_base_y_off, 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + if self.use_lightbulb: + # lightbulb + self.lightbulb = LightbulbObject( + name="lightbulb", + **self.lightbulb_args, + ) + objects.append(self.lightbulb) + object_locations.append( + [ + stove_base_x_off, + stove_base_y_off, + (self.stove_base_size[2] + self.lightbulb.z_center), + ] + ) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + else: + # stove + self.stove = StoveObjectNew( + name="new_stove", + joints=None, + ) + objects.append(self.stove) + object_locations.append( + [ + stove_base_x_off, + stove_base_y_off, + (self.stove_base_size[2] + self.stove_z_half_size), + ] + ) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + + # chain to the left of stove base + chain_ind = 0 + left_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_left - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + left_chain_size[1] *= self.num_box_geoms_left + left_chain_size[1] /= self.merge_size + # number of additional geoms to add + num_geoms_iter = self.merge_size - 1 + left_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(left_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + objects.append(left_chain_obj) + object_locations.append( + [ + stove_base_x_off - 0.75 * self.stove_base_size[0], + stove_base_y_off - (self.stove_base_size[1] + left_chain_size[1]), + 0.0, + ] + ) + # object_locations.append([0., -(self.stove_base_size[1] + left_chain_size[1]), 0.]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(None) + if self.merge_box_geoms: + # add ball joint and make sure to place it at the edge of the geom + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(left_chain_size[1]) + object_joints[left_chain_obj.root_body] = [ball_joint] + for i in range(num_geoms_iter): + left_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(left_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(left_chain_obj) + object_locations.append([0.0, -2.0 * left_chain_size[1], 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + # add ball joint and make sure to place it at the edge of the geom + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(left_chain_size[1]) + object_joints[left_chain_obj.root_body] = [ball_joint] + + # add chain in downward direction + rot_quat = T.convert_quat( + T.axisangle2quat(np.array([0.0, 0.0, 1.0]) * (np.pi / 2.0)), to="wxyz" + ) + vert_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_vert - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + vert_chain_size[1] *= self.num_box_geoms_vert + vert_chain_size[1] /= self.merge_size + # number of additional geoms to add + num_geoms_iter = self.merge_size - 1 + vert_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(vert_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(vert_chain_obj) + object_locations.append([vert_chain_size[1], -left_chain_size[1], 0.0]) + object_quats.append(rot_quat) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(vert_chain_size[1]) + object_joints[vert_chain_obj.root_body] = [ball_joint] + + for i in range(num_geoms_iter): + vert_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(vert_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(vert_chain_obj) + object_locations.append([0.0, -2.0 * vert_chain_size[1], 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(vert_chain_size[1]) + object_joints[vert_chain_obj.root_body] = [ball_joint] + + # add chain in rightward direction + rot_quat = T.convert_quat( + T.axisangle2quat(np.array([0.0, 0.0, 1.0]) * (np.pi / 2.0)), to="wxyz" + ) + right_chain_size = list(self.wire_box_geom_size) + num_geoms_iter = self.num_box_geoms_right - 1 + if self.merge_box_geoms: + # only one big geom instead of chain of geoms + right_chain_size[1] *= self.num_box_geoms_right + right_chain_size[1] /= self.merge_size + # number of additional geoms to add + num_geoms_iter = self.merge_size - 1 + right_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(right_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(right_chain_obj) + object_locations.append([right_chain_size[1], -vert_chain_size[1], 0.0]) + object_quats.append(rot_quat) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(right_chain_size[1]) + object_joints[right_chain_obj.root_body] = [ball_joint] + + for i in range(num_geoms_iter): + right_chain_obj = BoxObject( + name="chain_{}".format(chain_ind), + size=list(right_chain_size), + rgba=list(self.wire_box_geom_rgba), + material=box_geom_material, + joints=None, + ) + chain_ind += 1 + parent = objects[-1].root_body + objects.append(right_chain_obj) + object_locations.append([0.0, -2.0 * right_chain_size[1], 0.0]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + ball_joint = dict(ball_joint_spec) + ball_joint["name"] = "ball_joint_{}".format(chain_ind) + ball_joint["pos"] = "0 {} 0".format(right_chain_size[1]) + object_joints[right_chain_obj.root_body] = [ball_joint] + + # add cylinder object + self.cylinder_obj = InverseStackedCylinderObject( + name="cylinder_obj", + **self.cylinder_args, + ) + + x_off = 0.0 + y_off = -(right_chain_size[1] + self.cylinder_obj.square_base_width) + z_off = ((self.cylinder_obj.h1 + self.cylinder_obj.h2) / 2.0) - right_chain_size[2] + parent = objects[-1].root_body + objects.append(self.cylinder_obj) + object_locations.append([x_off, y_off, z_off]) + object_quats.append([1.0, 0.0, 0.0, 0.0]) + object_parents.append(parent) + + # # add debug site to see object center + # sites = [ + # dict( + # name="TMP", + # pos=array_to_string([0., 0., 0.]), + # size="{}".format(0.1), + # rgba=array_to_string([1., 0., 0., 1.]), + # ) + # ] + + # Run super init + super().__init__( + name=name, + objects=objects, + object_locations=object_locations, + object_quats=object_quats, + object_parents=object_parents, + joints=joints, + body_joints=object_joints, + # sites=sites, + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/objects.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/objects.py new file mode 100644 index 0000000..2a07649 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/objects.py @@ -0,0 +1,276 @@ +import os +import time +import xml.etree.ElementTree as ET + +import mujoco +import numpy as np +import robosuite +import robosuite.utils.transform_utils as T +from robosuite.models.objects import MujocoXMLObject +from robosuite.utils.mjcf_utils import array_to_string, string_to_array +from robosuite.environments.robot_env import RobotEnv + + +class MJCFObject(MujocoXMLObject): + """ + Blender object with support for changing the scaling + """ + + def __init__( + self, + name, + mjcf_path, + scale=1.0, + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=100, + friction=(0.95, 0.3, 0.1), + margin=None, + rgba=None, + priority=None, + static=False, + ): + # get scale in x, y, z + if isinstance(scale, float): + scale = [scale, scale, scale] + elif isinstance(scale, tuple) or isinstance(scale, list): + assert len(scale) == 3 + scale = tuple(scale) + else: + raise Exception("got invalid scale: {}".format(scale)) + scale = np.array(scale) + + self.solimp = solimp + self.solref = solref + self.density = density + self.friction = friction + self.margin = margin + + self.priority = priority + + self.rgba = rgba + + # read default xml + xml_path = mjcf_path + folder = os.path.dirname(xml_path) + tree = ET.parse(xml_path) + root = tree.getroot() + + # write modified xml (and make sure to postprocess any paths just in case) + xml_str = ET.tostring(root, encoding="utf8").decode("utf8") + xml_str = self.postprocess_model_xml(xml_str) + time_str = str(time.time()).replace(".", "_") + new_xml_path = os.path.join(folder, "{}_{}.xml".format(time_str, os.getpid())) + f = open(new_xml_path, "w") + f.write(xml_str) + f.close() + + # initialize object with new xml we wrote + super().__init__( + fname=new_xml_path, + name=name, + joints=None if static else [dict(type="free", damping="0.0005")], + obj_type="all", + duplicate_collision_geoms=False, + scale=scale, + ) + + self.spawns = [] + self._disabled_spawns = set() + for s in self.worldbody.findall("./body/body/geom"): + geom_name = s.get("name") + if geom_name and geom_name.startswith("{}spawn_".format(self.naming_prefix)): + self.spawns.append(s) + + # clean up xml - we don't need it anymore + if os.path.exists(new_xml_path): + os.remove(new_xml_path) + + def postprocess_model_xml(self, xml_str): + """ + New version of postprocess model xml that only replaces robosuite file paths if necessary (otherwise + there is an error with the "max" operation) + """ + + path = os.path.split(robosuite.__file__)[0] + path_split = path.split("/") + + # replace mesh and texture file paths + tree = ET.fromstring(xml_str) + root = tree + asset = root.find("asset") + meshes = asset.findall("mesh") + textures = asset.findall("texture") + all_elements = meshes + textures + + for elem in all_elements: + old_path = elem.get("file") + if old_path is None: + continue + + old_path_split = old_path.split("/") + # maybe replace all paths to robosuite assets + check_lst = [loc for loc, val in enumerate(old_path_split) if val == "robosuite"] + if len(check_lst) > 0: + ind = max(check_lst) # last occurrence index + new_path_split = path_split + old_path_split[ind + 1 :] + new_path = "/".join(new_path_split) + elem.set("file", new_path) + + return ET.tostring(root, encoding="utf8").decode("utf8") + + def _get_geoms(self, root, _parent=None): + """ + Helper function to recursively search through element tree starting at @root and returns + a list of (parent, child) tuples where the child is a geom element + + Args: + root (ET.Element): Root of xml element tree to start recursively searching through + + _parent (ET.Element): Parent of the root element tree. Should not be used externally; only set + during the recursive call + + Returns: + list: array of (parent, child) tuples where the child element is a geom type + """ + geom_pairs = super(MJCFObject, self)._get_geoms(root=root, _parent=_parent) + + # modify geoms according to the attributes + for i, (parent, element) in enumerate(geom_pairs): + element.set("solref", array_to_string(self.solref)) + element.set("solimp", array_to_string(self.solimp)) + element.set("density", str(self.density)) + element.set("friction", array_to_string(self.friction)) + if self.margin is not None: + element.set("margin", str(self.margin)) + + if (self.rgba is not None) and (element.get("group") == "1"): + element.set("rgba", array_to_string(self.rgba)) + + if self.priority is not None: + # set high priorit + element.set("priority", str(self.priority)) + + return geom_pairs + + def get_joint(self, joint_name: str): + _, _, joints = self._get_elements_by_name( + geom_names=[], body_names=[], joint_names=[joint_name] + ) + return joints[joint_name] + + @property + def horizontal_radius(self): + horizontal_radius_site = self.worldbody.find( + "./body/site[@name='{}horizontal_radius_site']".format(self.naming_prefix) + ) + site_values = string_to_array(horizontal_radius_site.get("pos")) + return np.linalg.norm(site_values[0:2]) + + def get_bbox_points(self, trans=None, rot=None) -> list[np.ndarray]: + """ + Get the full 8 bounding box points of the object + rot: a rotation matrix + """ + bottom_offset = self.bottom_offset + top_offset = self.top_offset + horizontal_radius_site = self.worldbody.find( + "./body/site[@name='{}horizontal_radius_site']".format(self.naming_prefix) + ) + horiz_radius = string_to_array(horizontal_radius_site.get("pos"))[:2] + return self._get_bbox_points( + bottom_offset=bottom_offset, + top_offset=top_offset, + radius=horiz_radius, + trans=trans, + rot=rot, + ) + + @staticmethod + def _get_bbox_points( + bottom_offset, top_offset, radius, trans=None, rot=None + ) -> list[np.ndarray]: + """ + Helper function to get the full 8 bounding box points of the object. + """ + center = np.mean([bottom_offset, top_offset], axis=0) + half_size = [radius[0], radius[1], top_offset[2] - center[2]] + + bbox_offsets = [ + center + half_size * np.array([-1, -1, -1]), # p0 + center + half_size * np.array([1, -1, -1]), # px + center + half_size * np.array([-1, 1, -1]), # py + center + half_size * np.array([-1, -1, 1]), # pz + center + half_size * np.array([1, 1, 1]), + center + half_size * np.array([-1, 1, 1]), + center + half_size * np.array([1, -1, 1]), + center + half_size * np.array([1, 1, -1]), + ] + + if trans is None: + trans = np.array([0, 0, 0]) + if rot is not None: + rot = T.quat2mat(rot) + else: + rot = np.eye(3) + + points = [(np.matmul(rot, p) + trans) for p in bbox_offsets] + return points + + @staticmethod + def get_spawn_bottom_offset(site: ET.Element) -> np.array: + """ + Get bottom offset of the spawn zone. + """ + site_pos = string_to_array(site.get("pos")) + site_size = string_to_array(site.get("size")) if site.get("type") == "box" else np.zeros(3) + return site_pos - np.array([0, 0, site_size[-1]]) + + def get_random_spawn(self, rng, exclude_disabled: bool = False) -> tuple[int, ET.Element]: + """ + Get random spawn site. + """ + options = [o for o in range(0, len(self.spawns))] + if exclude_disabled: + options = [o for o in options if o not in self._disabled_spawns] + spawn_id = rng.choice(options) + return spawn_id, self.spawns[spawn_id] + + def set_spawn_active(self, spawn_id: int, active: bool): + """ + Update the activity state of a spawn site. Disabled sites are excluded from random sampling. + """ + if active and spawn_id in self._disabled_spawns: + self._disabled_spawns.remove(spawn_id) + elif not active: + self._disabled_spawns.add(spawn_id) + + def closest_spawn_id(self, env: RobotEnv, obj: "MJCFObject", max_distance: float = 1.0) -> int: + if len(self.spawns) == 0: + return -1 + if not env.check_contact(self, obj): + return -1 + obj_pos = env.sim.data.body_xpos[env.sim.model.body_name2id(obj.root_body)] + distances = [] + for spawn_id in range(len(self.spawns)): + spawn_pos = env.sim.data.get_geom_xpos(self.spawns[spawn_id].get("name")) + distance = np.linalg.norm(spawn_pos - obj_pos) + distances.append((spawn_id, distance)) + distances = sorted(distances, key=lambda item: item[1]) + obj_geom_ids = [env.sim.model.geom_name2id(g) for g in obj.contact_geoms] + for spawn_id, distance in distances: + spawn_geom_id = env.sim.model.geom_name2id(self.spawns[spawn_id].get("name")) + for obj_geom_id in obj_geom_ids: + real_distance = mujoco.mj_geomDistance( + m=env.sim.model._model, + d=env.sim.data._data, + geom1=spawn_geom_id, + geom2=obj_geom_id, + distmax=max_distance, + fromto=None, + ) + if real_distance <= 0: + return spawn_id + if real_distance >= max_distance: + return -1 + return -1 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/xml_objects.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/xml_objects.py new file mode 100644 index 0000000..347232d --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/objects/xml_objects.py @@ -0,0 +1,268 @@ +import os +import time +import xml.etree.ElementTree as ET +import numpy as np +from robosuite.models.objects import MujocoXMLObject +from robosuite.utils.mjcf_utils import array_to_string, find_elements, string_to_array +import robocasa + +XML_ASSETS_BASE_PATH = os.path.join(robocasa.__path__[0], "models/assets/gear_kitchen") + + +class BlenderObject(MujocoXMLObject): + """ + Blender object with support for changing the scaling + """ + + def __init__( + self, + name, + mjcf_path, + scale=1.0, + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=100, + friction=(0.95, 0.3, 0.1), + margin=None, + rgba=None, + ): + # get scale in x, y, z + if isinstance(scale, float): + scale = [scale, scale, scale] + elif isinstance(scale, tuple) or isinstance(scale, list): + assert len(scale) == 3 + scale = tuple(scale) + else: + raise Exception("got invalid scale: {}".format(scale)) + scale = np.array(scale) + + self.solimp = solimp + self.solref = solref + self.density = density + self.friction = friction + self.margin = margin + + self.rgba = rgba + + # read default xml + xml_path = mjcf_path + folder = os.path.dirname(xml_path) + tree = ET.parse(xml_path) + root = tree.getroot() + + # modify mesh scales + asset = root.find("asset") + meshes = asset.findall("mesh") + for mesh in meshes: + # if a scale already exists, multiply the scales + scale_to_set = scale + existing_scale = mesh.get("scale") + if existing_scale is not None: + scale_to_set = string_to_array(existing_scale) * scale + mesh.set("scale", array_to_string(scale_to_set)) + + # modify sites for collision (assumes we can just scale up the locations - may or may not work) + for n in ["bottom_site", "top_site", "horizontal_radius_site"]: + site = root.find("worldbody/body/site[@name='{}']".format(n)) + pos = string_to_array(site.get("pos")) + pos = scale * pos + site.set("pos", array_to_string(pos)) + + # write modified xml (and make sure to postprocess any paths just in case) + xml_str = ET.tostring(root, encoding="utf8").decode("utf8") + # xml_str = postprocess_model_xml(xml_str) + time_str = str(time.time()).replace(".", "_") + new_xml_path = os.path.join(folder, "{}_{}.xml".format(time_str, os.getpid())) + f = open(new_xml_path, "w") + f.write(xml_str) + f.close() + # print(f"Write to {new_xml_path}") + + # initialize object with new xml we wrote + super().__init__( + fname=new_xml_path, + name=name, + joints=[dict(type="free", damping="0.0005")], + obj_type="all", + duplicate_collision_geoms=False, + ) + + # clean up xml - we don't need it anymore + if os.path.exists(new_xml_path): + os.remove(new_xml_path) + + def _get_geoms(self, root, _parent=None): + """ + Helper function to recursively search through element tree starting at @root and returns + a list of (parent, child) tuples where the child is a geom element + + Args: + root (ET.Element): Root of xml element tree to start recursively searching through + _parent (ET.Element): Parent of the root element tree. Should not be used externally; only set + during the recursive call + + Returns: + list: array of (parent, child) tuples where the child element is a geom type + """ + geom_pairs = super(BlenderObject, self)._get_geoms(root=root, _parent=_parent) + + # modify geoms according to the attributes + for i, (parent, element) in enumerate(geom_pairs): + element.set("solref", array_to_string(self.solref)) + element.set("solimp", array_to_string(self.solimp)) + element.set("density", str(self.density)) + element.set("friction", array_to_string(self.friction)) + if self.margin is not None: + element.set("margin", str(self.margin)) + + if (self.rgba is not None) and (element.get("group") == "1"): + element.set("rgba", array_to_string(self.rgba)) + + return geom_pairs + + +class CoffeeMachinePodObject(MujocoXMLObject): + """ + Coffee pod object (used in Coffee task). + """ + + def __init__(self, name, margin=1.0): + self._horizontal_radius_margin = margin + super().__init__( + os.path.join(XML_ASSETS_BASE_PATH, "objects/coffee_pod.xml"), + # name=name, joints=[dict(type="free", damping="0.0005")], + name=name, + joints=[dict(type="free")], + obj_type="all", + duplicate_collision_geoms=True, + ) + + @property + def horizontal_radius(self): + horizontal_radius_site = self.worldbody.find( + "./body/site[@name='{}horizontal_radius_site']".format(self.naming_prefix) + ) + return ( + self._horizontal_radius_margin * string_to_array(horizontal_radius_site.get("pos"))[0] + ) + + +class CoffeeMachineBodyObject(MujocoXMLObject): + """ + Coffee machine body piece (used in Coffee task). Note that the piece is rigid (no joint is added). + """ + + def __init__(self, name): + super().__init__( + os.path.join(XML_ASSETS_BASE_PATH, "objects/coffee_body.xml"), + name=name, + joints=None, + obj_type="all", + duplicate_collision_geoms=True, + ) + + +class CoffeeMachineLidObject(MujocoXMLObject): + """ + Coffee machine lid piece (used in Coffee task). + """ + + def __init__(self, name): + super().__init__( + os.path.join(XML_ASSETS_BASE_PATH, "objects/coffee_lid.xml"), + name=name, + joints=None, + obj_type="all", + duplicate_collision_geoms=True, + ) + + +class CoffeeMachineBaseObject(MujocoXMLObject): + """ + Coffee machine base piece (used in Coffee task). Note that the piece is rigid (no joint is added). + """ + + def __init__(self, name): + super().__init__( + os.path.join(XML_ASSETS_BASE_PATH, "objects/coffee_base.xml"), + name=name, + joints=None, + obj_type="all", + duplicate_collision_geoms=True, + ) + + +class DrawerObject(MujocoXMLObject): + """ + Custom version of cabinet object that differs from BUDs. It has manually specified top, bottom, and horizontal sites. + """ + + def __init__(self, name, joints=None): + + # original cabinet xml + # path_to_cabinet_xml = os.path.join(robosuite_task_zoo.__path__[0], "models/hammer_place/cabinet.xml") + + # our custom cabinet xml - names the drawer bottom geom for easy contact checking + # path_to_cabinet_xml = os.path.join(mimicgen.__path__[0], "envs/robosuite/assets/cabinet.xml") + path_to_cabinet_xml = os.path.join(XML_ASSETS_BASE_PATH, "objects/cabinet.xml") + + # path to modified xml from Danfei's student + # path_to_cabinet_xml = os.path.join(XML_ASSETS_BASE_PATH, "objects/cabinet1.xml") + + super().__init__( + path_to_cabinet_xml, + name=name, + joints=None, + obj_type="all", + duplicate_collision_geoms=True, + ) + + # NOTE: had to manually set these to get placement sampler working okay + @property + def bottom_offset(self): + return np.array([0, 0, -0.065]) + + @property + def top_offset(self): + return np.array([0, 0, 0.065]) + + @property + def horizontal_radius(self): + return 0.15 + + @property + def body_size(self): + return (0.5, 0.4, 0.3) + + +class LongDrawerObject(MujocoXMLObject): + """ + Drawer that has longer platform for easier grasping of objects inside. + """ + + def __init__(self, name, joints=None): + + # our custom cabinet xml - has some longer geoms for the drawer platform + path_to_cabinet_xml = os.path.join(XML_ASSETS_BASE_PATH, "objects/drawer_long.xml") + + super().__init__( + path_to_cabinet_xml, + name=name, + joints=None, + obj_type="all", + duplicate_collision_geoms=True, + ) + + # NOTE: had to manually set these to get placement sampler working okay + @property + def bottom_offset(self): + return np.array([0, 0, -0.065]) + + @property + def top_offset(self): + return np.array([0, 0, 0.065]) + + @property + def horizontal_radius(self): + # TODO: this might need to change now + return 0.15 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/__init__.py new file mode 100644 index 0000000..3abca86 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/__init__.py @@ -0,0 +1,779 @@ +from numpy import dtype, ndarray + + +from typing import Any + + +import mujoco +import numpy as np + +from robosuite.controllers.parts.generic.joint_pos import JointPositionController +from robosuite.controllers.parts.gripper.simple_grip import SimpleGripController +from robosuite.controllers.composite.composite_controller import HybridMobileBase +from robosuite.models.grippers import ( + PandaGripper, + InspireLeftHand, + InspireRightHand, + FourierLeftHand, + FourierRightHand, +) + +from robocasa.models.grippers import G1ThreeFingerLeftHand, G1ThreeFingerRightHand + +from .manipulators import * + + +# This function returns in the order of the gripper XML's actuators. +def unformat_gripper_space(gripper, formatted_action): + formatted_action = np.array(formatted_action) + if isinstance(gripper, InspireLeftHand) or isinstance(gripper, InspireRightHand): + action = formatted_action[[0, 2, 4, 6, 8, 11]] + elif isinstance(gripper, FourierLeftHand) or isinstance(gripper, FourierRightHand): + action = formatted_action[[0, 1, 4, 6, 8, 10]] + elif isinstance(gripper, PandaGripper): + action = formatted_action + else: + raise TypeError + return action + + +def remove_mimic_joints(gripper, formatted_action) -> ndarray[Any, dtype[Any]]: + """_summary_ + + This is to remove the mimic joints from the actuator-ordered action + """ + formatted_action = np.array(formatted_action) + if isinstance(gripper, InspireLeftHand) or isinstance(gripper, InspireRightHand): + raise NotImplementedError + elif isinstance(gripper, FourierLeftHand) or isinstance(gripper, FourierRightHand): + action = formatted_action[[0, 2, 4, 6, 8, 10]] + elif isinstance(gripper, G1ThreeFingerLeftHand) or isinstance(gripper, G1ThreeFingerRightHand): + action = formatted_action + elif isinstance(gripper, PandaGripper): + action = formatted_action + return action + + +def _reconstruct_latest_action_gr1_impl(robot, verbose=False): + action_dict = {} + cc = robot.composite_controller + pf = robot.robot_model.naming_prefix + + for part_name, controller in cc.part_controllers.items(): + if isinstance(controller, JointPositionController): + assert controller.input_type == "absolute" + act = controller.goal_qpos + elif isinstance(controller, SimpleGripController): + assert not controller.use_action_scaling + act = unformat_gripper_space(cc.grippers[part_name], controller.goal_qvel) + else: + raise TypeError + action_dict[f"{pf}{part_name}"] = act + + if verbose: + print("Actions:", [(k, len(action_dict[k])) for k in action_dict]) + + return action_dict + + +def _reconstruct_latest_action_panda_impl(robot, action, verbose=False): + action_dict = {} + cc = robot.composite_controller + pf = robot.robot_model.naming_prefix + + for part_name, controller in cc.part_controllers.items(): + start_idx, end_idx = cc._action_split_indexes[part_name] + act = action[start_idx:end_idx] + # split action deeper for OSC + action_dict[f"{pf}{part_name}"] = act + + if verbose: + print("Actions:", [(k, len(action_dict[k])) for k in action_dict]) + + return action_dict + + +# This function tries to get all joints value, but sometimes we want to keep original action +# The function must be called after robot.control() +def reconstruct_latest_actions(env, actions=None, verbose=False): + if actions is None: + actions = np.zeros(env.action_dim) + action_dict = {} + cutoff = 0 + for robot in env.robots: + cc = robot.composite_controller + pf = robot.robot_model.naming_prefix + robot_action = actions[cutoff : cutoff + robot.action_dim] + cutoff += robot.action_dim + if "GR1" in robot.name: + action_dict.update(_reconstruct_latest_action_gr1_impl(robot)) + elif "Panda" in robot.name: + action_dict.update(_reconstruct_latest_action_panda_impl(robot, robot_action)) + else: + raise ValueError(f"Unknown robot name: {robot.name}") + if isinstance(cc, HybridMobileBase): + action_dict[f"{pf}base_mode"] = robot_action[-1] + + if verbose: + print("Actions:", [(k, len(action_dict[k])) for k in action_dict]) + + return action_dict + + +def gather_robot_observations(env, verbose=False, format_gripper_space=True): + observations = {} + + for robot_id, robot in enumerate(env.robots): + sim = robot.sim + gripper_names = {robot.get_gripper_name(arm): robot.gripper[arm] for arm in robot.arms} + for part_name, indexes in robot._ref_joints_indexes_dict.items(): + qpos_values = [] + for joint_id in indexes: + qpos_addr = sim.model.jnt_qposadr[joint_id] + # https://mujoco.readthedocs.io/en/stable/APIreference/APItypes.html#mjtjoint + joint_type = sim.model.jnt_type[joint_id] + if joint_type == mujoco.mjtJoint.mjJNT_FREE: + qpos_size = 7 # Free joint has 7 DOFs + elif joint_type == mujoco.mjtJoint.mjJNT_BALL: + qpos_size = 4 # Ball joint has 4 DOFs (quaternion) + else: + qpos_size = 1 # Revolute or prismatic joint has 1 DOF + qpos_values = np.append( + qpos_values, sim.data.qpos[qpos_addr : qpos_addr + qpos_size] + ) + if part_name in gripper_names.keys(): + gripper = gripper_names[part_name] + # Reverse the order to match the real robot + # TODO: how to adapt to other grippers? + if format_gripper_space: + qpos_values = unformat_gripper_space(gripper, qpos_values)[::-1] + if len(qpos_values) > 0: + observations[f"robot{robot_id}_{part_name}"] = qpos_values + + if verbose: + print("States:", [(k, len(observations[k])) for k in observations]) + + return observations + + +from gymnasium import spaces + + +class RobotKeyConverter: + @classmethod + def get_camera_config(cls): + raise NotImplementedError + + @classmethod + def map_obs(cls, input_obs): + raise NotImplementedError + + @classmethod + def map_action(cls, input_action): + raise NotImplementedError + + @classmethod + def unmap_action(cls, input_action): + raise NotImplementedError + + @classmethod + def get_metadata(cls, name): + raise NotImplementedError + + @classmethod + def map_obs_in_eval(cls, input_obs): + output_obs = {} + mapped_obs = cls.map_obs(input_obs) + for k, v in mapped_obs.items(): + assert k.startswith("hand.") or k.startswith("body.") + output_obs["state." + k[5:]] = v + return output_obs + + @classmethod + def get_missing_keys_in_dumping_dataset(cls): + return {} + + @classmethod + def convert_to_float64(cls, input): + for k, v in input.items(): + if isinstance(v, np.ndarray) and v.dtype == np.float32: + input[k] = v.astype(np.float64) + return input + + @classmethod + def deduce_observation_space(cls, env): + # Calling env._get_observations(force_update=True) will pollute the observables. + # This is safe because it is only called from the constructor and will be overwritten later in reset(). + obs = ( + env.viewer._get_observations(force_update=True) + if env.viewer_get_obs + else env._get_observations(force_update=True) + ) + obs.update(gather_robot_observations(env)) + obs = cls.map_obs(obs) + observation_space = spaces.Dict() + + for k, v in obs.items(): + if k.startswith("hand.") or k.startswith("body."): + observation_space["state." + k[5:]] = spaces.Box( + low=-1, high=1, shape=(len(v),), dtype=np.float32 + ) + else: + raise ValueError(f"Unknown key: {k}") + + return observation_space + + @classmethod + def deduce_action_space(cls, env): + action = cls.map_action(reconstruct_latest_actions(env)) + action_space = spaces.Dict() + for k, v in action.items(): + if isinstance(v, np.int64): + action_space["action." + k[5:]] = spaces.Discrete(2) + elif isinstance(v, np.ndarray): + action_space["action." + k[5:]] = spaces.Box( + low=-1, high=1, shape=(len(v),), dtype=np.float32 + ) + else: + raise ValueError(f"Unknown type: {type(v)}") + return action_space + + +class GR1ArmsOnlyKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = ["video.ego_view_pad_res256_freq20"] + camera_names = [ + "egoview", + ] + camera_widths, camera_heights = 1280, 800 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.right_hand": input_obs["robot0_right_gripper"], + "hand.left_hand": input_obs["robot0_left_gripper"], + "body.right_arm": input_obs["robot0_right"], + "body.left_arm": input_obs["robot0_left"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.left_hand": input_action["robot0_left_gripper"], + "hand.right_hand": input_action["robot0_right_gripper"], + "body.left_arm": input_action["robot0_left"], + "body.right_arm": input_action["robot0_right"], + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_left_gripper": input_action["action.left_hand"], + "robot0_right_gripper": input_action["action.right_hand"], + "robot0_left": input_action["action.left_arm"], + "robot0_right": input_action["action.right_arm"], + } + return output_action + + @classmethod + def get_missing_keys_in_dumping_dataset(cls): + return { + "body.waist": np.zeros(3, dtype=np.float64), + "body.neck": np.zeros(3, dtype=np.float64), + "body.right_leg": np.zeros(6, dtype=np.float64), + "body.left_leg": np.zeros(6, dtype=np.float64), + } + + @classmethod + def get_metadata(cls, name): + return { + "absolute": True, + "rotation_type": None, + } + + +class GR1ArmsAndWaistKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = ["video.ego_view_pad_res256_freq20"] + camera_names = [ + "egoview", + ] + camera_widths, camera_heights = 1280, 800 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.right_hand": input_obs["robot0_right_gripper"], + "hand.left_hand": input_obs["robot0_left_gripper"], + "body.right_arm": input_obs["robot0_right"], + "body.left_arm": input_obs["robot0_left"], + "body.waist": input_obs["robot0_torso"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.left_hand": input_action["robot0_left_gripper"], + "hand.right_hand": input_action["robot0_right_gripper"], + "body.left_arm": input_action["robot0_left"], + "body.right_arm": input_action["robot0_right"], + "body.waist": input_action["robot0_torso"], + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_left_gripper": input_action["action.left_hand"], + "robot0_right_gripper": input_action["action.right_hand"], + "robot0_left": input_action["action.left_arm"], + "robot0_right": input_action["action.right_arm"], + "robot0_torso": input_action["action.waist"], + } + return output_action + + @classmethod + def get_missing_keys_in_dumping_dataset(cls): + return { + "body.neck": np.zeros(3, dtype=np.float64), + "body.right_leg": np.zeros(6, dtype=np.float64), + "body.left_leg": np.zeros(6, dtype=np.float64), + } + + @classmethod + def get_metadata(cls, name): + return { + "absolute": True, + "rotation_type": None, + } + + +class GR1FixedLowerBodyKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = ["video.agentview_pad_res256_freq20"] + camera_names = [ + "agentview", + ] + camera_widths, camera_heights = 1280, 800 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.right_hand": input_obs["robot0_right_gripper"], + "hand.left_hand": input_obs["robot0_left_gripper"], + "body.right_arm": input_obs["robot0_right"], + "body.left_arm": input_obs["robot0_left"], + "body.waist": input_obs["robot0_torso"], + "body.neck": input_obs["robot0_head"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.left_hand": input_action["robot0_left_gripper"], + "hand.right_hand": input_action["robot0_right_gripper"], + "body.left_arm": input_action["robot0_left"], + "body.right_arm": input_action["robot0_right"], + "body.waist": input_action["robot0_torso"], + "body.neck": input_action["robot0_head"], + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_left_gripper": input_action["action.left_hand"], + "robot0_right_gripper": input_action["action.right_hand"], + "robot0_left": input_action["action.left_arm"], + "robot0_right": input_action["action.right_arm"], + "robot0_torso": input_action["action.waist"], + "robot0_head": input_action["action.neck"], + } + return output_action + + @classmethod + def get_missing_keys_in_dumping_dataset(cls): + return { + "body.right_leg": np.zeros(6, dtype=np.float64), + "body.left_leg": np.zeros(6, dtype=np.float64), + } + + @classmethod + def get_metadata(cls, name): + return { + "absolute": True, + "rotation_type": None, + } + + +class PandaOmronKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = [ + "video.res256_image_side_0", + "video.res256_image_side_1", + "video.res256_image_wrist_0", + ] + camera_names = [ + "robot0_agentview_left", + "robot0_agentview_right", + "robot0_eye_in_hand", + ] + camera_widths, camera_heights = 512, 512 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.gripper_qpos": input_obs["robot0_gripper_qpos"], + "body.base_position": input_obs["robot0_base_pos"], + "body.base_rotation": input_obs["robot0_base_quat"], + "body.end_effector_position_relative": input_obs["robot0_base_to_eef_pos"], + "body.end_effector_rotation_relative": input_obs["robot0_base_to_eef_quat"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.gripper_close": ( + np.int64(0) if input_action["robot0_right_gripper"] < 0 else np.int64(1) + ), + "body.end_effector_position": input_action["robot0_right"][..., 0:3], + "body.end_effector_rotation": input_action["robot0_right"][..., 3:6], + "body.base_motion": np.concatenate( + ( + input_action["robot0_base"], + input_action["robot0_torso"], + ), + axis=-1, + ), + "body.control_mode": ( + np.int64(0) if input_action["robot0_base_mode"] < 0 else np.int64(1) + ), + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_right_gripper": (-1.0 if input_action["action.gripper_close"] < 0.5 else 1.0), + "robot0_right": np.concatenate( + ( + input_action["action.end_effector_position"], + input_action["action.end_effector_rotation"], + ), + axis=-1, + ), + "robot0_base": input_action["action.base_motion"][..., 0:3], + "robot0_torso": input_action["action.base_motion"][..., 3:4], + "robot0_base_mode": (-1.0 if input_action["action.control_mode"] < 0.5 else 1.0), + } + return output_action + + @classmethod + def get_metadata(cls, name): + # This function and import is only enabled in gr00t exporter + from gr00t_wbc.data.schema.raw.metadata import RotationType + + if name in [ + "body.base_position", + "body.end_effector_position_relative", + "body.end_effector_position", + ]: + return { + "absolute": False, + "rotation_type": None, + } + elif name in ["body.base_rotation", "body.end_effector_rotation_relative"]: + return { + "absolute": False, + "rotation_type": RotationType.QUATERNION, + } + elif name in ["body.end_effector_rotation"]: + return { + "absolute": False, + "rotation_type": RotationType.AXIS_ANGLE, + } + else: + return { + "absolute": True, + "rotation_type": None, + } + + +class PandaPandaKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = [ + "video.agentview_pad_res256_freq20", + "video.robot0_eye_in_hand_pad_res256_freq20", + "video.robot1_eye_in_hand_pad_res256_freq20", + ] + camera_names = [ + "agentview", + "robot0_eye_in_hand", + "robot1_eye_in_hand", + ] + camera_widths, camera_heights = 1280, 800 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.right_gripper_qpos": input_obs["robot0_right_gripper"], + "hand.left_gripper_qpos": input_obs["robot1_right_gripper"], + "body.right_arm_eef_pos": input_obs["robot0_eef_pos"], + "body.right_arm_eef_quat": input_obs["robot0_eef_quat"], + "body.right_arm_joint_pos": input_obs["robot0_joint_pos"], + "body.left_arm_eef_pos": input_obs["robot1_eef_pos"], + "body.left_arm_eef_quat": input_obs["robot1_eef_quat"], + "body.left_arm_joint_pos": input_obs["robot1_joint_pos"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.right_gripper_close": ( + np.int64(0) if input_action["robot0_right_gripper"] < 0 else np.int64(1) + ), + "hand.left_gripper_close": ( + np.int64(0) if input_action["robot1_right_gripper"] < 0 else np.int64(1) + ), + "body.right_arm_eef_pos": input_action["robot0_right"][..., 0:3], + "body.right_arm_eef_rot": input_action["robot0_right"][..., 3:6], + "body.left_arm_eef_pos": input_action["robot1_right"][..., 0:3], + "body.left_arm_eef_rot": input_action["robot1_right"][..., 3:6], + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_right_gripper": ( + -1.0 if input_action["action.right_gripper_close"] < 0.5 else 1.0 + ), + "robot1_right_gripper": ( + -1.0 if input_action["action.left_gripper_close"] < 0.5 else 1.0 + ), + "robot0_right": np.concatenate( + ( + input_action["action.right_arm_eef_pos"], + input_action["action.right_arm_eef_rot"], + ), + axis=-1, + ), + "robot1_right": np.concatenate( + ( + input_action["action.left_arm_eef_pos"], + input_action["action.left_arm_eef_rot"], + ), + axis=-1, + ), + } + return output_action + + @classmethod + def get_metadata(cls, name): + # This function and import is only enabled in gr00t exporter + from gr00t_wbc.data.schema.raw.metadata import RotationType + + if name in ["body.right_arm_eef_pos", "body.left_arm_eef_pos"]: + return { + "absolute": False, + "rotation_type": None, + } + elif name in ["body.right_arm_eef_quat", "body.right_arm_eef_quat"]: + return { + "absolute": False, + "rotation_type": RotationType.QUATERNION, + } + elif name in ["body.right_arm_eef_rot", "body.left_arm_eef_rot"]: + return { + "absolute": False, + "rotation_type": RotationType.AXIS_ANGLE, + } + else: + return { + "absolute": True, + "rotation_type": None, + } + + +class PandaDexRHPandaDexRHKeyConverter(RobotKeyConverter): + @classmethod + def get_camera_config(cls): + mapped_names = [ + "video.agentview_pad_res256_freq20", + "video.robot0_eye_in_hand_pad_res256_freq20", + "video.robot1_eye_in_hand_pad_res256_freq20", + ] + camera_names = [ + "agentview", + "robot0_eye_in_hand", + "robot1_eye_in_hand", + ] + camera_widths, camera_heights = 1280, 800 + return mapped_names, camera_names, camera_widths, camera_heights + + @classmethod + def map_obs(cls, input_obs): + output_obs = type(input_obs)() + output_obs = { + "hand.right_hand": input_obs["robot0_right_gripper"], + "hand.left_hand": input_obs["robot1_right_gripper"], + "body.right_arm_eef_pos": input_obs["robot0_eef_pos"], + "body.right_arm_eef_quat": input_obs["robot0_eef_quat"], + "body.right_arm_joint_pos": input_obs["robot0_joint_pos"], + "body.left_arm_eef_pos": input_obs["robot1_eef_pos"], + "body.left_arm_eef_quat": input_obs["robot1_eef_quat"], + "body.left_arm_joint_pos": input_obs["robot1_joint_pos"], + } + return output_obs + + @classmethod + def map_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "hand.right_hand": input_action["robot0_right_gripper"], + "hand.left_hand": input_action["robot1_right_gripper"], + "body.right_arm_eef_pos": input_action["robot0_right"][0:3], + "body.right_arm_eef_rot": input_action["robot0_right"][3:6], + "body.left_arm_eef_pos": input_action["robot1_right"][0:3], + "body.left_arm_eef_rot": input_action["robot1_right"][3:6], + } + return output_action + + @classmethod + def unmap_action(cls, input_action): + output_action = type(input_action)() + output_action = { + "robot0_right_gripper": input_action["action.right_hand"], + "robot1_right_gripper": input_action["action.left_hand"], + "robot0_right": np.concatenate( + ( + input_action["action.right_arm_eef_pos"], + input_action["action.right_arm_eef_rot"], + ), + axis=-1, + ), + "robot1_right": np.concatenate( + ( + input_action["action.left_arm_eef_pos"], + input_action["action.left_arm_eef_rot"], + ), + axis=-1, + ), + } + return output_action + + @classmethod + def get_metadata(cls, name): + # This function and import is only enabled in gr00t exporter + from gr00t_wbc.data.schema.raw.metadata import RotationType + + if name in ["body.right_arm_eef_pos", "body.left_arm_eef_pos"]: + return { + "absolute": False, + "rotation_type": None, + } + elif name in ["body.right_arm_eef_quat", "body.right_arm_eef_quat"]: + return { + "absolute": False, + "rotation_type": RotationType.QUATERNION, + } + elif name in ["body.right_arm_eef_rot", "body.left_arm_eef_rot"]: + return { + "absolute": False, + "rotation_type": RotationType.AXIS_ANGLE, + } + else: + return { + "absolute": True, + "rotation_type": None, + } + + +# The values are only used in gr00t dataset embodiment tag and env name +GR00T_ROBOCASA_ENVS_GR1_ARMS_ONLY = { + "GR1ArmsOnly": "gr1_arms_only_fourier_hands", + "GR1ArmsOnlyInspireHands": "gr1_arms_only_inspire_hands", + "GR1ArmsOnlyFourierHands": "gr1_arms_only_fourier_hands", +} +GR00T_ROBOCASA_ENVS_GR1_ARMS_AND_WAIST = { + "GR1ArmsAndWaistFourierHands": "gr1_arms_waist_fourier_hands", +} +GR00T_ROBOCASA_ENVS_GR1_FIXED_LOWER_BODY = { + "GR1FixedLowerBody": "gr1_fixed_lower_body_fourier_hands", + "GR1FixedLowerBodyInspireHands": "gr1_fixed_lower_body_inspire_hands", + "GR1FixedLowerBodyFourierHands": "gr1_fixed_lower_body_fourier_hands", +} +GR00T_ROBOCASA_ENVS_PANDA = { + "PandaMobile": "panda_mobile", + "PandaOmron": "panda_omron", +} +GR00T_ROBOCASA_ENVS_BIMANUAL_GRIPPER = { + "Panda_Panda": "bimanual_panda_parallel_gripper", +} +GR00T_ROBOCASA_ENVS_BIMANUAL_HAND = { + "PandaDexRH_PandaDexLH": "bimanual_panda_inspire_hand", +} +GR00T_ROBOCASA_ENVS_G1 = { + "G1": "g1_sim", + "G1ArmsOnly": "g1_sim", + "G1FixedLowerBody": "g1_sim", + "G1FixedBase": "g1_sim", +} +GR00T_ROBOCASA_ENVS_ROBOTS = { + **GR00T_ROBOCASA_ENVS_GR1_ARMS_ONLY, + **GR00T_ROBOCASA_ENVS_GR1_ARMS_AND_WAIST, + **GR00T_ROBOCASA_ENVS_GR1_FIXED_LOWER_BODY, + **GR00T_ROBOCASA_ENVS_PANDA, + **GR00T_ROBOCASA_ENVS_BIMANUAL_GRIPPER, + **GR00T_ROBOCASA_ENVS_BIMANUAL_HAND, + **GR00T_ROBOCASA_ENVS_G1, +} +GR00T_LOCOMANIP_ENVS_ROBOTS = {**GR00T_ROBOCASA_ENVS_G1} + + +def make_key_converter(robots_name): + if robots_name in GR00T_ROBOCASA_ENVS_GR1_ARMS_ONLY: + return GR1ArmsOnlyKeyConverter + elif robots_name in GR00T_ROBOCASA_ENVS_GR1_ARMS_AND_WAIST: + return GR1ArmsAndWaistKeyConverter + elif robots_name in GR00T_ROBOCASA_ENVS_GR1_FIXED_LOWER_BODY: + return GR1FixedLowerBodyKeyConverter + elif robots_name in GR00T_ROBOCASA_ENVS_PANDA: + return PandaOmronKeyConverter + elif robots_name in GR00T_ROBOCASA_ENVS_BIMANUAL_GRIPPER: + return PandaPandaKeyConverter + elif robots_name in GR00T_ROBOCASA_ENVS_BIMANUAL_HAND: + return PandaDexRHPandaDexRHKeyConverter + else: + raise ValueError(f"Unknown robot name: {robots_name}") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/__init__.py new file mode 100644 index 0000000..a0e7504 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/__init__.py @@ -0,0 +1 @@ +from .g1_robot import * diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/g1_robot.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/g1_robot.py new file mode 100644 index 0000000..a4b59e0 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/robots/manipulators/g1_robot.py @@ -0,0 +1,391 @@ +import numpy as np +from robosuite.models.robots.manipulators.legged_manipulator_model import ( + LeggedManipulatorModel, +) +from robosuite.robots import register_robot_class +from robosuite.utils.mjcf_utils import find_elements, xml_path_completion + +import robocasa.models + + +@register_robot_class("LeggedRobot") +class G1(LeggedManipulatorModel): + """ + G1 is a mobile manipulator robot created by Unitree. + + Args: + idn (int or str): Number or some other unique identification string for this robot instance + """ + + arms = ["right", "left"] + + def __init__(self, idn: int = 0): + super().__init__( + xml_path_completion( + "robots/unitree_g1/g1_29dof_rev_1_0.xml", + root=robocasa.models.assets_root, + ), + idn=idn, + ) + + def _print_joint_info(self): + """Print formatted information about joints""" + # ANSI color codes + BLUE = "\033[94m" + GREEN = "\033[92m" + RED = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + + print(f"\n{BOLD}{BLUE}================ joints info ================{ENDC}") + + # Print joints by category + categories = { + "Arms": self._arms_joints, + "Base": self._base_joints, + "Torso": self._torso_joints, + "Head": self._head_joints, + "Legs": self._legs_joints, + } + + for category, joints in categories.items(): + if joints: + print(f"{GREEN}{category} ({len(joints)}){ENDC}: {', '.join(joints)}") + else: + print(f"{RED}{category} (disabled){ENDC}") + print() + + def _print_actuator_info(self): + """Print formatted information about actuators""" + # ANSI color codes + BLUE = "\033[94m" + GREEN = "\033[92m" + RED = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + + print(f"\n{BOLD}{BLUE}================ actuators info ================{ENDC}") + + # Print actuators by category + categories = { + "Arms": self._arms_actuators, + "Base": self._base_actuators, + "Torso": self._torso_actuators, + "Head": self._head_actuators, + "Legs": self._legs_actuators, + } + + for category, actuators in categories.items(): + if actuators: + print(f"{GREEN}{category} ({len(actuators)}){ENDC}: {', '.join(actuators)}") + else: + print(f"{RED}{category} (disabled){ENDC}") + print() + + def update_joints(self): + """internal function to update joint lists""" + for joint in self.all_joints: + if "waist" in joint: + self.torso_joints.append(joint) + elif "base" in joint: + self.base_joints.append(joint) + elif "knee" in joint or "hip" in joint or "ankle" in joint: + self.legs_joints.append(joint) + + for joint in self.all_joints: + if ( + joint not in self._base_joints + and joint not in self._torso_joints + and joint not in self._head_joints + and joint not in self._legs_joints + ): + self._arms_joints.append(joint) + + # adjust the order of the joints, make sure the right arm joints are before the left arm joints + self._arms_joints = [joint for joint in self._arms_joints if "right" in joint] + [ + joint for joint in self._arms_joints if "left" in joint + ] + + # self._print_joint_info() + + def update_actuators(self): + """internal function to update actuator lists""" + for actuator in self.all_actuators: + if "waist" in actuator: + self.torso_actuators.append(actuator) + elif "base" in actuator: + self.base_actuators.append(actuator) + elif "knee" in actuator or "hip" in actuator or "ankle" in actuator: + self.legs_actuators.append(actuator) + + for actuator in self.all_actuators: + if ( + actuator not in self._base_actuators + and actuator not in self._torso_actuators + and actuator not in self._head_actuators + and actuator not in self._legs_actuators + ): + self._arms_actuators.append(actuator) + + # adjust the order of the actuators, make sure the right arm actuators are before the left arm actuators + self._arms_actuators = [ + actuator for actuator in self._arms_actuators if "right" in actuator + ] + [actuator for actuator in self._arms_actuators if "left" in actuator] + # self._print_actuator_info() + + @property + def default_base(self): + return "NullBase" + # return "NoActuationBase" + + @property + def default_gripper(self): + # return {"right": None, "left": None} + return {"right": "G1ThreeFingerRightHand", "left": "G1ThreeFingerLeftHand"} + + @property + def init_qpos(self): + DEFAULT_DOF_ANGLES = { + "left_hip_pitch_joint": -0.1, + "left_hip_roll_joint": 0.0, + "left_hip_yaw_joint": 0.0, + "left_knee_joint": 0.3, + "left_ankle_pitch_joint": -0.2, + "left_ankle_roll_joint": 0.0, + "right_hip_pitch_joint": -0.1, + "right_hip_roll_joint": 0.0, + "right_hip_yaw_joint": 0.0, + "right_knee_joint": 0.3, + "right_ankle_pitch_joint": -0.2, + "right_ankle_roll_joint": 0.0, + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, # 0.3, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": 0.0, # 1.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, # -0.3, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, # 1.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + } + + joints = self.worldbody.findall(".//joint") + joints = [joint for joint in joints if joint.get("name").startswith("robot")] + init_qpos = np.array([0.0] * len(joints)) + for joint_id, joint in enumerate(joints): + init_qpos[joint_id] = DEFAULT_DOF_ANGLES[ + joint.get("name")[joint.get("name").find("_") + 1 :] + ] + return init_qpos + + @property + def base_xpos_offset(self): + return { + "bins": (-0.30, -0.1, 0.95), + "empty": (-0.29, 0, 0.95), + "table": lambda table_length: (-0.15 - table_length / 2, 0, 0.95), + } + + @property + def top_offset(self): + return np.array((0, 0, 1.0)) + + @property + def _horizontal_radius(self): + return 0.5 + + @property + def arm_type(self): + return "bimanual" + + @property + def _eef_name(self): + return {"right": "right_eef", "left": "left_eef"} + + @property + def torso_body(self): + return ["torso_link"] + + def get_camera_configs(self): + _cam_config = {} + + # egoview camera + egoview_mount_body = find_elements( + root=self.root, + tags="body", + attribs={"name": f"{self.naming_prefix}torso_link"}, + ) + if egoview_mount_body is not None: + # realsense D435i, 1920 * 1080, 69° × 42° + _cam_config[f"{self.naming_prefix}rs_egoview"] = dict( + pos=[0.07555294, 0.02579919, 0.49719054], + # quat=[0.69263989, 0.16169103, -0.19008497, -0.67673754], + quat=[0.67876761, 0.20864099, -0.20567003, -0.67338199], + camera_attribs=dict(fovy="40"), + parent_body=f"{self.naming_prefix}torso_link", + ) + + # OAK-D W, OV9782, 640*480 + _cam_config[f"{self.naming_prefix}oak_egoview"] = dict( + pos=[0.10209156, -0.00937542, 0.42446595], + quat=[0.64367383, 0.26523914, -0.27106013, -0.66472446], + camera_attribs=dict(fovy="79.5"), + parent_body=f"{self.naming_prefix}torso_link", + ) + + _cam_config[f"{self.naming_prefix}oak_left_monoview"] = dict( + pos=[0.10209156, 0.02857542, 0.42446595], + quat=[0.64367383, 0.26523914, -0.27106013, -0.66472446], + camera_attribs=dict(fovy="79.5"), + parent_body=f"{self.naming_prefix}torso_link", + ) + + _cam_config[f"{self.naming_prefix}oak_right_monoview"] = dict( + pos=[0.10209156, -0.04657542, 0.42446595], + quat=[0.64367383, 0.26523914, -0.27106013, -0.66472446], + camera_attribs=dict(fovy="79.5"), + parent_body=f"{self.naming_prefix}torso_link", + ) + + # tppview camera + tpp_mount_body = find_elements( + root=self.root, + tags="body", + attribs={"name": f"{self.naming_prefix}pelvis"}, + ) + if tpp_mount_body is not None: + _cam_config[f"{self.naming_prefix}rs_tppview"] = dict( + pos=[-1.131, -0.626, 1.247 - 0.793], + quat=[0.67953146, 0.46872971, -0.3204774, -0.46456828], + camera_attribs=dict(fovy="60"), + parent_body=f"{self.naming_prefix}pelvis", + ) + + return _cam_config + + def _disable_collisions(self, body_parts): + """ + Disable collision detection for specified body parts by setting + contype="0" and conaffinity="0" for all collision geometries in those parts. + + Args: + body_parts (list): List of strings identifying body parts to disable collisions for. + Examples: ["hip", "knee", "ankle"], ["waist"], ["shoulder"], etc. + """ + # Find all body elements in the worldbody + for body in self.worldbody.findall(".//body"): + body_name = body.get("name", "") + # Check if this body belongs to specified body parts + if any(part in body_name for part in body_parts): + # Find all geom elements in this body + for geom in body.findall(".//geom"): + # Check if this is a collision geom (doesn't have contype="0" and conaffinity="0") + contype = geom.get("contype") + conaffinity = geom.get("conaffinity") + + # If contype and conaffinity are not already "0", set them to disable collision + if contype != "0" or conaffinity != "0": + geom.set("contype", "0") + geom.set("conaffinity", "0") + + +@register_robot_class("LeggedRobot") +class G1FixedBase(G1): + def __init__(self, idn: int = 0): + super().__init__(idn=idn) + + # Remove base actuation + self._remove_free_joint() + + +@register_robot_class("LeggedRobot") +class G1FixedLowerBody(G1): + def __init__(self, idn: int = 0): + super().__init__(idn=idn) + + # Remove lower body actuation + self._remove_joint_actuation("knee") + self._remove_joint_actuation("hip") + self._remove_joint_actuation("ankle") + + self._remove_free_joint() + + +@register_robot_class("LeggedRobot") +class G1ArmsOnly(G1): + def __init__(self, idn: int = 0): + super().__init__(idn=idn) + + # Remove lower body actuation + self._remove_joint_actuation("knee") + self._remove_joint_actuation("hip") + self._remove_joint_actuation("ankle") + + # Remove spine & head actuation + self._remove_joint_actuation("waist") + + self._remove_free_joint() + + +@register_robot_class("LeggedRobot") +class G1ArmsOnlyFloating(G1): + def __init__(self, idn: int = 0): + super().__init__(idn=idn) + + # Remove lower body actuation + self._remove_joint_actuation("knee") + self._remove_joint_actuation("hip") + self._remove_joint_actuation("ankle") + + # Remove spine & head actuation + self._remove_joint_actuation("waist") + + self._remove_free_joint() + + @property + def default_base(self): + return "FloatingLeggedBase" + + @property + def base_xpos_offset(self): + return { + "bins": (-0.30, -0.1, 0.97), + "empty": (-0.29, 0, 0.97), + "table": lambda table_length: (-0.15 - table_length / 2, 0, 0.97), + } + + +@register_robot_class("LeggedRobot") +class G1FloatingBody(G1): + def __init__(self, idn: int = 0): + super().__init__(idn=idn) + + # Remove lower body actuation + self._remove_joint_actuation("knee") + self._remove_joint_actuation("hip") + self._remove_joint_actuation("ankle") + + # Disable collision detection for lower body parts + self._disable_collisions(["hip", "knee", "ankle"]) + + self._remove_free_joint() + + @property + def default_base(self): + return "FloatingLeggedBase" + + @property + def base_xpos_offset(self): + return { + "bins": (-0.30, -0.1, 0.97), + "empty": (-0.29, 0, 0.97), + "table": lambda table_length: (-0.15 - table_length / 2, 0, 0.97), + } diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/__init__.py new file mode 100644 index 0000000..2a22ff2 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/__init__.py @@ -0,0 +1,3 @@ +from .ground_arena import GroundArena +from .lab_arena import LabArena +from .factory_arena import FactoryArena diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/factory_arena.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/factory_arena.py new file mode 100644 index 0000000..056ab42 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/factory_arena.py @@ -0,0 +1,14 @@ +from robosuite.models.arenas import Arena +from robosuite.utils.mjcf_utils import xml_path_completion +import robocasa + + +class FactoryArena(Arena): + """Factory workspace.""" + + def __init__(self): + super().__init__( + xml_path_completion( + "arenas/gear_factory/gear_factory.xml", root=robocasa.models.assets_root + ) + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/ground_arena.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/ground_arena.py new file mode 100644 index 0000000..cf94b8a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/ground_arena.py @@ -0,0 +1,13 @@ +from robosuite.models.arenas import Arena +from robosuite.utils.mjcf_utils import xml_path_completion + +import robocasa + + +class GroundArena(Arena): + """Empty workspace.""" + + def __init__(self): + super().__init__( + xml_path_completion("arenas/ground_arena.xml", root=robocasa.models.assets_root) + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/lab_arena.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/lab_arena.py new file mode 100644 index 0000000..84ca87e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/models/scenes/lab_arena.py @@ -0,0 +1,12 @@ +from robosuite.models.arenas import Arena +from robosuite.utils.mjcf_utils import xml_path_completion +import robocasa + + +class LabArena(Arena): + """Lab workspace.""" + + def __init__(self): + super().__init__( + xml_path_completion("arenas/gear_lab/gear_lab.xml", root=robocasa.models.assets_root) + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/__init__.py new file mode 100644 index 0000000..fd4d6a4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/__init__.py @@ -0,0 +1 @@ +from robosuite.utils import * diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/camera_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/camera_utils.py new file mode 100644 index 0000000..537e7c6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/camera_utils.py @@ -0,0 +1,246 @@ +""" +Collection of constants for cameras / robots / etc +in kitchen environments +""" + +import numpy as np +import matplotlib.pyplot as plt + + +# https://github.com/yusukeurakami/mujoco_2d_projection +def global2label(obj_pos, cam_pos, cam_ori, output_size=[64, 64], fov=90, s=1): + """ + :param obj_pos: 3D coordinates of the joint from MuJoCo in nparray [m] + :param cam_pos: 3D coordinates of the camera from MuJoCo in nparray [m] + :param cam_ori: camera 3D rotation (Rotation order of x->y->z) from MuJoCo in nparray [rad] + :param fov: field of view in integer [degree] + :return: Heatmap of the object in the 2D pixel space. + """ + + e = np.array([output_size[0] / 2, output_size[1] / 2, 1]) + fov = np.array([fov]) + + # Converting the MuJoCo coordinate into typical computer vision coordinate. + cam_ori_cv = np.array([cam_ori[1], cam_ori[0], -cam_ori[2]]) + obj_pos_cv = np.array([obj_pos[1], obj_pos[0], -obj_pos[2]]) + cam_pos_cv = np.array([cam_pos[1], cam_pos[0], -cam_pos[2]]) + + obj_pos_in_2D, obj_pos_from_cam = get_2D_from_3D(obj_pos_cv, cam_pos_cv, cam_ori_cv, fov, e) + label = gkern( + output_size[0], + output_size[1], + (obj_pos_in_2D[1], output_size[0] - obj_pos_in_2D[0]), + sigma=s, + ) + return label + + +def get_2D_from_3D(a, c, theta, fov, e): + """ + :param a: 3D coordinates of the joint in nparray [m] + :param c: 3D coordinates of the camera in nparray [m] + :param theta: camera 3D rotation (Rotation order of x->y->z) in nparray [rad] + :param fov: field of view in integer [degree] + :param e: + :return: + - (bx, by) ==> 2D coordinates of the obj [pixel] + - d ==> 3D coordinates of the joint (relative to the camera) [m] + """ + + # Get the vector from camera to object in global coordinate. + ac_diff = a - c + + # Rotate the vector in to camera coordinate + x_rot = np.array( + [ + [1, 0, 0], + [0, np.cos(theta[0]), np.sin(theta[0])], + [0, -np.sin(theta[0]), np.cos(theta[0])], + ] + ) + + y_rot = np.array( + [ + [np.cos(theta[1]), 0, -np.sin(theta[1])], + [0, 1, 0], + [np.sin(theta[1]), 0, np.cos(theta[1])], + ] + ) + + z_rot = np.array( + [ + [np.cos(theta[2]), np.sin(theta[2]), 0], + [-np.sin(theta[2]), np.cos(theta[2]), 0], + [0, 0, 1], + ] + ) + + transform = z_rot.dot(y_rot.dot(x_rot)) + d = transform.dot(ac_diff) + + # scaling of projection plane using fov + fov_rad = np.deg2rad(fov) + e[2] *= e[0] * 1 / np.tan(fov_rad / 2.0) + + # Projection from d to 2D + bx = e[2] * d[0] / (d[2]) + e[0] + by = e[2] * d[1] / (d[2]) + e[1] + + return (bx, by), d + + +def gkern(h, w, center, sigma=1): + x = np.arange(0, w, 1, float) + y = np.arange(0, h, 1, float) + y = y[:, np.newaxis] + x0 = center[0] + y0 = center[1] + return np.exp(-1 * ((x - x0) ** 2 + (y - y0) ** 2) / sigma**2) + + +def compute_2d_projection( + obj_pos=np.array([0.2, 0.25, 1.0]), + cam_pos=np.array([0.7, 0, 1.5]), + cam_ori=np.array([0.2, 1.2, 1.57]), + fov=90, + output_size=[64, 64], +): + e = np.array([output_size[0] / 2, output_size[1] / 2, 1]) + fov = np.array([fov]) + + # Converting the MuJoCo coordinate into typical computer vision coordinate. + cam_ori_cv = np.array([cam_ori[1], cam_ori[0], -cam_ori[2]]) + obj_pos_cv = np.array([obj_pos[1], obj_pos[0], -obj_pos[2]]) + cam_pos_cv = np.array([cam_pos[1], cam_pos[0], -cam_pos[2]]) + + obj_pos_in_2D, obj_pos_from_cam = get_2D_from_3D(obj_pos_cv, cam_pos_cv, cam_ori_cv, fov, e) + + return obj_pos_in_2D[1], output_size[0] - obj_pos_in_2D[0] + + +def global2label(obj_pos, cam_pos, cam_ori, output_size=[64, 64], fov=90, s=1): + """ + :param obj_pos: 3D coordinates of the joint from MuJoCo in nparray [m] + :param cam_pos: 3D coordinates of the camera from MuJoCo in nparray [m] + :param cam_ori: camera 3D rotation (Rotation order of x->y->z) from MuJoCo in nparray [rad] + :param fov: field of view in integer [degree] + :return: Heatmap of the object in the 2D pixel space. + """ + + e = np.array([output_size[0] / 2, output_size[1] / 2, 1]) + fov = np.array([fov]) + + # Converting the MuJoCo coordinate into typical computer vision coordinate. + cam_ori_cv = np.array([cam_ori[1], cam_ori[0], -cam_ori[2]]) + obj_pos_cv = np.array([obj_pos[1], obj_pos[0], -obj_pos[2]]) + cam_pos_cv = np.array([cam_pos[1], cam_pos[0], -cam_pos[2]]) + + obj_pos_in_2D, obj_pos_from_cam = get_2D_from_3D(obj_pos_cv, cam_pos_cv, cam_ori_cv, fov, e) + label = gkern( + output_size[0], + output_size[1], + (obj_pos_in_2D[1], output_size[0] - obj_pos_in_2D[0]), + sigma=s, + ) + return label + + +def visualize_2d_projection( + obj_pos=np.array([0.2, 0.25, 1.0]), + cam_pos=np.array([0.7, 0, 1.5]), + cam_ori=np.array([0.2, 1.2, 1.57]), + fov=90, + output_size=[64, 64], +): + s = 1 # std for heapmap signal + label = global2label(obj_pos, cam_pos, cam_ori, output_size, fov=fov, s=s) + plt.imshow(label) + plt.show() + + +# default free cameras for different kitchen layouts +LAYOUT_CAMS = { + 0: dict( + lookat=[2.26593463, -1.00037131, 1.38769295], + distance=3.0505089839567323, + azimuth=90.71563812375285, + elevation=-12.63948837207208, + ), + 1: dict( + lookat=[2.66147999, -1.00162429, 1.2425155], + distance=3.7958766287746255, + azimuth=89.75784013699234, + elevation=-15.177406642875091, + ), + 2: dict( + lookat=[3.02344359, -1.48874618, 1.2412914], + distance=3.6684844368165512, + azimuth=51.67880851867874, + elevation=-13.302619131542388, + ), + # 3: dict( + # lookat=[11.44842548, -11.47664723, 11.24115989], + # distance=43.923271794728187, + # azimuth=227.12928449329333, + # elevation=-16.495686334624907, + # ), + 4: dict( + lookat=[1.6, -1.0, 1.0], + distance=5, + azimuth=89.70301806083651, + elevation=-18.02177994296577, + ), +} + +DEFAULT_LAYOUT_CAM = { + "lookat": [2.25, -1, 1.05312667], + "distance": 5, + "azimuth": 89.70301806083651, + "elevation": -18.02177994296577, +} + +CAM_CONFIGS = dict( + robot0_agentview_center=dict( + pos=[-0.6, 0.0, 1.15], + quat=[ + 0.636945903301239, + 0.3325185477733612, + -0.3199238181114197, + -0.6175596117973328, + ], + parent_body="mobilebase0_support", + ), + robot0_agentview_left=dict( + pos=[-0.5, 0.35, 1.05], + quat=[0.55623853, 0.29935253, -0.37678665, -0.6775092], + camera_attribs=dict(fovy="60"), + parent_body="mobilebase0_support", + ), + robot0_agentview_right=dict( + pos=[-0.5, -0.35, 1.05], + quat=[ + 0.6775091886520386, + 0.3767866790294647, + -0.2993525564670563, + -0.55623859167099, + ], + camera_attribs=dict(fovy="60"), + parent_body="mobilebase0_support", + ), + robot0_frontview=dict( + pos=[-0.50, 0, 0.95], + quat=[ + 0.6088936924934387, + 0.3814677894115448, + -0.3673907518386841, + -0.5905545353889465, + ], + camera_attribs=dict(fovy="60"), + parent_body="mobilebase0_support", + ), + robot0_eye_in_hand=dict( + pos=[0.05, 0, 0], + quat=[0, 0.707107, 0.707107, 0], + parent_body="robot0_right_hand", + ), +) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/config_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/config_utils.py new file mode 100644 index 0000000..bbb5faf --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/config_utils.py @@ -0,0 +1,63 @@ +import copy +import os +import pathlib +import robosuite +from robosuite.controllers import load_composite_controller_config + + +def is_stale_controller_config(config: dict): + """ + Checks if a controller config is in the format from robosuite versions <= 1.4.1. + Does not check for config validity, only the format type. + Args: + config (dict): Controller configuration + Returns: + bool: True if the config is in the old format, False otherwise + """ + + OLD_CONTROLLER_TYPES = [ + "JOINT_VELOCITY", + "JOINT_TORQUE", + "JOINT_POSITION", + "OSC_POSITION", + "OSC_POSE", + "IK_POSE", + ] + if ( + "body_parts" not in config or "body_parts_controller_configs" not in config + ) and "type" in config: + return config["type"] in OLD_CONTROLLER_TYPES + return False + + +def refactor_composite_controller_config(controller_config, robot_type, arms): + """ + Checks if a controller config is in the format from robosuite versions <= 1.4.1. + If this is the case, converts the controller config to the new composite controller + config format in robosuite versions >= 1.5. If the robot has a default + controller config use that and override the arms with the old controller config. + If not just use the old controller config for arms. + Args: + old_controller_config (dict): Old controller config + Returns: + dict: New controller config + """ + if not is_stale_controller_config(controller_config): + return controller_config + + config_dir = pathlib.Path(robosuite.__file__).parent / "controllers/config/robots/" + name = robot_type.lower() + configs = os.listdir(config_dir) + if f"default_{name}.json" in configs: + new_controller_config = load_composite_controller_config(robot=name) + else: + new_controller_config = {} + new_controller_config["type"] = "BASIC" + new_controller_config["body_parts"] = {} + + for arm in arms: + new_arm_config = copy.deepcopy(controller_config) + if "gripper" not in new_arm_config: + new_arm_config["gripper"] = {"type": "GRIP"} + new_controller_config["body_parts"][arm] = new_arm_config + return new_controller_config diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/cotrain_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/cotrain_utils.py new file mode 100644 index 0000000..bd07dfa --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/cotrain_utils.py @@ -0,0 +1,25 @@ +""" +Utility functions and constants for co-training with real robot. +""" + +from robosuite.models.robots.manipulators.gr1_robot import GR1 + +# Initial pose configuration for co-training with real robot +COTRAIN_REAL_MATCHED_ROBOT_INITIAL_POSE = { + GR1: { + "robot0_r_shoulder_pitch": -0.22963779, + "robot0_r_shoulder_roll": -0.38363408, + "robot0_r_shoulder_yaw": 0.14360377, + "robot0_r_elbow_pitch": -1.5289252, + "robot0_r_wrist_yaw": -0.2897802, + "robot0_r_wrist_roll": -0.07134621, + "robot0_r_wrist_pitch": -0.04550289, + "robot0_l_shoulder_pitch": -0.10933163, + "robot0_l_shoulder_roll": 0.43292055, + "robot0_l_shoulder_yaw": -0.15983289, + "robot0_l_elbow_pitch": -1.48233023, + "robot0_l_wrist_yaw": 0.2359135, + "robot0_l_wrist_roll": 0.26184522, + "robot0_l_wrist_pitch": 0.00830735, + } +} diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dataset_registry.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dataset_registry.py new file mode 100644 index 0000000..fededf1 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dataset_registry.py @@ -0,0 +1,352 @@ +import os +from collections import OrderedDict +from copy import deepcopy +from pathlib import Path + +import robocasa +import robocasa.macros as macros + +SINGLE_STAGE_TASK_DATASETS = OrderedDict( + PnPCounterToCab=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPCounterToCab/2024-04-24", + mg_path="v0.1/single_stage/kitchen_pnp/PnPCounterToCab/mg/2024-05-04-22-12-27_and_2024-05-07-07-39-33", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/c0a0vdyqeqh6o9z4c57sk61aypladizj.hdf5", + human_im="https://utexas.box.com/shared/static/gznii250ip99731ii2r0ml6be7gzt2cp.hdf5", + mg_im="https://utexas.box.com/shared/static/7y9csrcx6uhhq4p3yctmm2df3rjqpw6g.hdf5", + ), + ), + PnPCabToCounter=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPCabToCounter/2024-04-24", + mg_path="v0.1/single_stage/kitchen_pnp/PnPCabToCounter/mg/2024-07-12-04-33-29", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/fgd165p91ts9qurfb496ubi1xdqgrz97.hdf5", + human_im="https://utexas.box.com/shared/static/pdfh88slq4bazfglixaw80whvbtlbw6r.hdf5", + mg_im="https://utexas.box.com/shared/static/mfltre5w8qi8ps1fqicblx40p008425z.hdf5", + ), + ), + PnPCounterToSink=dict( + horizon=700, + human_path="v0.1/single_stage/kitchen_pnp/PnPCounterToSink/2024-04-25", + mg_path="v0.1/single_stage/kitchen_pnp/PnPCounterToSink/mg/2024-05-04-22-14-06_and_2024-05-07-07-40-17", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/nc0x55xwlgs4acj97ngdj1q6pv450ooo.hdf5", + human_im="https://utexas.box.com/shared/static/0z1wr8iwfusfqqpp56j3vfxvvse1x0vc.hdf5", + mg_im="https://utexas.box.com/shared/static/5vkb2fpthnw554q888fynk3zqwsgk2e5.hdf5", + ), + ), + PnPSinkToCounter=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPSinkToCounter/2024-04-26_2", + mg_path="v0.1/single_stage/kitchen_pnp/PnPSinkToCounter/mg/2024-05-04-22-14-34_and_2024-05-07-07-40-21", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/uyhepwjyr0880a5q03fc61qmhbrhwy7k.hdf5", + human_im="https://utexas.box.com/shared/static/pe03k5qvcuq1nzbpfag5pfo2t8fzvl9i.hdf5", + mg_im="https://utexas.box.com/shared/static/ppj3j8jnxkpusi7ft44f5ut7szkmu8yr.hdf5", + ), + ), + PnPCounterToMicrowave=dict( + horizon=600, + human_path="v0.1/single_stage/kitchen_pnp/PnPCounterToMicrowave/2024-04-27", + mg_path="v0.1/single_stage/kitchen_pnp/PnPCounterToMicrowave/mg/2024-05-04-22-13-21_and_2024-05-07-07-41-17", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/z1qwxsuczjmv68p7267nylbv9ebc6g4p.hdf5", + human_im="https://utexas.box.com/shared/static/2kun70ehqzanl5h0hbgpdezrfrvuimq8.hdf5", + mg_im="https://utexas.box.com/shared/static/vdm6pzl68xqqxvk9iy7i3l62j37t3sbp.hdf5", + ), + ), + PnPMicrowaveToCounter=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPMicrowaveToCounter/2024-04-26", + mg_path="v0.1/single_stage/kitchen_pnp/PnPMicrowaveToCounter/mg/2024-05-04-22-14-26_and_2024-05-07-07-41-42", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/emnl5i3s621sf5smgek5lu4twbc326jt.hdf5", + human_im="https://utexas.box.com/shared/static/z2t1kyto32thprw7xvq3r4tjvkdz0dl2.hdf5", + mg_im="https://utexas.box.com/shared/static/3gs5cvp5qyzvk7gplmp0lkwfs1x1mkrj.hdf5", + ), + ), + PnPCounterToStove=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPCounterToStove/2024-04-26", + mg_path="v0.1/single_stage/kitchen_pnp/PnPCounterToStove/mg/2024-05-04-22-14-20", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/9z2k5iqspwdo2153yuutcvqb9zzzvtdu.hdf5", + human_im="https://utexas.box.com/shared/static/penrv5sysn192gxwj7n7f6k0gjceovfn.hdf5", + mg_im="https://utexas.box.com/shared/static/1ytpoks6mp9f56wh36ni8k02fz0kbvmb.hdf5", + ), + ), + PnPStoveToCounter=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_pnp/PnPStoveToCounter/2024-05-01", + mg_path="v0.1/single_stage/kitchen_pnp/PnPStoveToCounter/mg/2024-05-04-22-14-40", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/1m3v1rkh7gna5ujt0vcwzej7stv5rdio.hdf5", + human_im="https://utexas.box.com/shared/static/rht9bfh38gaue5ig17ubpsibb4jrgtyv.hdf5", + mg_im="https://utexas.box.com/shared/static/uvrx2d7zxungplafbn9kg6rbe23ysund.hdf5", + ), + ), + OpenSingleDoor=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_doors/OpenSingleDoor/2024-04-24", + mg_path="v0.1/single_stage/kitchen_doors/OpenSingleDoor/mg/2024-05-04-22-37-39", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/hjukxdpqvnrghtbjbhin28idzkysq7oa.hdf5", + human_im="https://utexas.box.com/shared/static/vhvlbuza3g1lpqa9m6zvmbzh8xquvqy8.hdf5", + mg_im="https://utexas.box.com/shared/static/ekf6qwp9s07jnkpazbohlv03d6b73vth.hdf5", + ), + ), + CloseSingleDoor=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_doors/CloseSingleDoor/2024-04-24", + mg_path="v0.1/single_stage/kitchen_doors/CloseSingleDoor/mg/2024-05-04-22-34-56", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/gea28p5mrvj9lnw1s6c8fqvzxk5f779i.hdf5", + human_im="https://utexas.box.com/shared/static/2wnm0u1x9fp9ni02pmzqzhpjsb1kgfrr.hdf5", + mg_im="https://utexas.box.com/shared/static/5mg9vkhilkaxza83l48aht3sk76086tm.hdf5", + ), + ), + OpenDoubleDoor=dict( + horizon=1000, + human_path="v0.1/single_stage/kitchen_doors/OpenDoubleDoor/2024-04-26", + mg_path="v0.1/single_stage/kitchen_doors/OpenDoubleDoor/mg/2024-05-04-22-35-53", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/xl23utiszzdbjdqlveq1mxxca9m3kbvw.hdf5", + human_im="https://utexas.box.com/shared/static/8swihowjd5fdk1vpf0k94gl72f8nbjeb.hdf5", + mg_im="https://utexas.box.com/shared/static/5djutjyyksic6qpl2uuq9cvzj3r78jvf.hdf5", + ), + ), + CloseDoubleDoor=dict( + horizon=700, + human_path="v0.1/single_stage/kitchen_doors/CloseDoubleDoor/2024-04-29", + mg_path="v0.1/single_stage/kitchen_doors/CloseDoubleDoor/mg/2024-05-04-22-22-42_and_2024-05-08-06-02-36", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/zozyctzejc2nrlpjwqjq4w761830kq45.hdf5", + human_im="https://utexas.box.com/shared/static/14f2ssfhwfhyo9cvj0s3kq303bv38g6t.hdf5", + mg_im="https://utexas.box.com/shared/static/7t269kmcrsycc4wz5wonw62ijopjhyv7.hdf5", + ), + ), + OpenDrawer=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_drawer/OpenDrawer/2024-05-03", + mg_path="v0.1/single_stage/kitchen_drawer/OpenDrawer/mg/2024-05-04-22-38-42", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/990d2f339zvalvlw50s4s1l6sfvt018m.hdf5", + human_im="https://utexas.box.com/shared/static/d8a0g5827kbm4ufk3p1tbuz3idstgum8.hdf5", + mg_im="https://utexas.box.com/shared/static/t0sio5vrdax5bdfqkumfh63inq6o9b88.hdf5", + ), + ), + CloseDrawer=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_drawer/CloseDrawer/2024-04-30", + mg_path="v0.1/single_stage/kitchen_drawer/CloseDrawer/mg/2024-05-09-09-32-19", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/ooekd1zdy02hfu234xm5h63f7mzbez4b.hdf5", + human_im="https://utexas.box.com/shared/static/4r5w0a6i4jtgv5qmqx09fnqh5d7c45oi.hdf5", + mg_im="https://utexas.box.com/shared/static/aohabqltp8c6ze61u4h2uhtc9p9w35zb.hdf5", + ), + ), + TurnOnSinkFaucet=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_sink/TurnOnSinkFaucet/2024-04-25", + mg_path="v0.1/single_stage/kitchen_sink/TurnOnSinkFaucet/mg/2024-05-04-22-17-46", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/7vogk85ed3sm1o1vo8fzast9rs8zshgk.hdf5", + human_im="https://utexas.box.com/shared/static/f0brygtzwgmmwccg58b6m82yapa4jnji.hdf5", + mg_im="https://utexas.box.com/shared/static/p8x9njpsf5j9fkmnu7lwajupzm1t7aap.hdf5", + ), + ), + TurnOffSinkFaucet=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_sink/TurnOffSinkFaucet/2024-04-25", + mg_path="v0.1/single_stage/kitchen_sink/TurnOffSinkFaucet/mg/2024-05-04-22-17-26", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/ukkycoa2c6k5d6bpt1cdplzk2nrg879t.hdf5", + human_im="https://utexas.box.com/shared/static/ceewfn4ydhprupdcdppfe8wu4x61oxdg.hdf5", + mg_im="https://utexas.box.com/shared/static/r392ma0dje2t5ov4dug4vbqhn0z6hblk.hdf5", + ), + ), + TurnSinkSpout=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_sink/TurnSinkSpout/2024-04-29", + mg_path="v0.1/single_stage/kitchen_sink/TurnSinkSpout/mg/2024-05-09-09-31-12", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/zxop70u9s6rl4udz7fhp4besrvu508pj.hdf5", + human_im="https://utexas.box.com/shared/static/nnojt3760k143fxwhf2u0kzh1i56n7x1.hdf5", + mg_im="https://utexas.box.com/shared/static/34l1y45xquau66nk8j7hrpfvlad4b9fg.hdf5", + ), + ), + TurnOnStove=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_stove/TurnOnStove/2024-05-02", + mg_path="v0.1/single_stage/kitchen_stove/TurnOnStove/mg/2024-05-08-09-20-31", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/ow4npc933yty4blxg8rc2pgjp6f1yc36.hdf5", + human_im="https://utexas.box.com/shared/static/dewbngq5wk6dipb29x6984x6dygu2gck.hdf5", + mg_im="https://utexas.box.com/shared/static/7gzolvj3fyzhhfjoxswmmpjpxu5q4too.hdf5", + ), + ), + TurnOffStove=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_stove/TurnOffStove/2024-05-02", + mg_path="v0.1/single_stage/kitchen_stove/TurnOffStove/mg/2024-05-08-09-20-45", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/vp6u3huv9c2vg5r09105gkbmwguycnn0.hdf5", + human_im="https://utexas.box.com/shared/static/8tukea09szcjb2ncbe43zt65n7dzg4eh.hdf5", + mg_im="https://utexas.box.com/shared/static/6rv14y77hfknrwfbwn5qtlfdydkr8tog.hdf5", + ), + ), + CoffeeSetupMug=dict( + horizon=600, + human_path="v0.1/single_stage/kitchen_coffee/CoffeeSetupMug/2024-04-25", + mg_path="v0.1/single_stage/kitchen_coffee/CoffeeSetupMug/mg/2024-05-04-22-22-13_and_2024-05-08-05-52-13", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/zxytzos86xes7jo8z0gp7lklawbiyo8g.hdf5", + human_im="https://utexas.box.com/shared/static/pv2i49t4p7238gp34txhm7jcl7domw58.hdf5", + mg_im="https://utexas.box.com/shared/static/uagjbu3y2ds0hzm7789nuj0ppxp9dxxl.hdf5", + ), + ), + CoffeeServeMug=dict( + horizon=600, + human_path="v0.1/single_stage/kitchen_coffee/CoffeeServeMug/2024-05-01", + mg_path="v0.1/single_stage/kitchen_coffee/CoffeeServeMug/mg/2024-05-04-22-21-50", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/tse3mkxx913pf4d4ij7fplchtv1rchkq.hdf5", + human_im="https://utexas.box.com/shared/static/ts3537f93dzjpkux19syy0pndw5re231.hdf5", + mg_im="https://utexas.box.com/shared/static/ikkr7qu89v9e0o1kwiqttkxllaxvv2n4.hdf5", + ), + ), + CoffeePressButton=dict( + horizon=300, + human_path="v0.1/single_stage/kitchen_coffee/CoffeePressButton/2024-04-25", + mg_path="v0.1/single_stage/kitchen_coffee/CoffeePressButton/mg/2024-05-04-22-21-32", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/t6oblmeilexe9ndca4ccaxdclw1tzps5.hdf5", + human_im="https://utexas.box.com/shared/static/l5dnmcfd0r36vhdqgjchxo20vajt7ohl.hdf5", + mg_im="https://utexas.box.com/shared/static/y5zm9mlslfg8p4jkpwnxlizcsvsca1mb.hdf5", + ), + ), + TurnOnMicrowave=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_microwave/TurnOnMicrowave/2024-04-25", + mg_path="v0.1/single_stage/kitchen_microwave/TurnOnMicrowave/mg/2024-05-04-22-40-00", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/o1sp7qrcd97b6jo68olx58l35wffpbna.hdf5", + human_im="https://utexas.box.com/shared/static/t6eromogy5is2no3s1e5bk1nb2hgab2k.hdf5", + mg_im="https://utexas.box.com/shared/static/t1n5oijudf8yn85bj8v64b1codde7kye.hdf5", + ), + ), + TurnOffMicrowave=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_microwave/TurnOffMicrowave/2024-04-25", + mg_path="v0.1/single_stage/kitchen_microwave/TurnOffMicrowave/mg/2024-05-04-22-39-23", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/m2exfytqjd0496y70b3ou2i8993ryb50.hdf5", + human_im="https://utexas.box.com/shared/static/0drm2h7fgd5857x8xgcj1lph23srpbj1.hdf5", + mg_im="https://utexas.box.com/shared/static/7c8bku46us9a8sddwg3zk6z3p4ce6ya0.hdf5", + ), + ), + NavigateKitchen=dict( + horizon=500, + human_path="v0.1/single_stage/kitchen_navigate/NavigateKitchen/2024-05-09", + mg_path=None, + download_links=dict( + human_raw="https://utexas.box.com/shared/static/pzvzuvmkla74kro84yw13uwjr2g89z8m.hdf5", + human_im="https://utexas.box.com/shared/static/mbi6011svy2zb436wsf2hhc337aduwmw.hdf5", + ), + ), +) + + +MULTI_STAGE_TASK_DATASETS = OrderedDict( + ArrangeVegetables=dict( + human_path="v0.1/multi_stage/chopping_food/ArrangeVegetables/2024-05-11", + horizon=1200, + activity="chopping_food", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/hiwbv68h3vty9ksqsdhcw2xhuvkwcf3o.hdf5", + human_im="https://utexas.box.com/shared/static/ovzt0bod4nrrack6dak12jn1fx2cnwbw.hdf5", + ), + ), + MicrowaveThawing=dict( + human_path="v0.1/multi_stage/defrosting_food/MicrowaveThawing/2024-05-11", + horizon=1000, + activity="defrosting_food", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/wreambog4jak8og9nbhm0nr2t6w5ygdt.hdf5", + human_im="https://utexas.box.com/shared/static/ehwo6728r3qyi8g0h70zhawijojqotnm.hdf5", + ), + ), + RestockPantry=dict( + human_path="v0.1/multi_stage/restocking_supplies/RestockPantry/2024-05-10", + horizon=1000, + activity="restocking_supplies", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/t2g6rdyh006bw09cxxxamd7thton5grl.hdf5", + human_im="https://utexas.box.com/shared/static/35yr2xmsmwq0xkknvlmlivh9ifste71q.hdf5", + ), + ), + PreSoakPan=dict( + human_path="v0.1/multi_stage/washing_dishes/PreSoakPan/2024-05-10", + horizon=1500, + activity="washing_dishes", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/d4vg2pag7465xp99vej1skk3frmohzk1.hdf5", + human_im="https://utexas.box.com/shared/static/etotr8tn0z57ttb311bibh0mdmc65j96.hdf5", + ), + ), + PrepareCoffee=dict( + human_path="v0.1/multi_stage/brewing/PrepareCoffee/2024-05-07", + horizon=1000, + activity="brewing", + download_links=dict( + human_raw="https://utexas.box.com/shared/static/qhtle2ccvm17g65ecg2vrudpkl9v5wkk.hdf5", + human_im="https://utexas.box.com/shared/static/qq3p4ctlmiv751jx5av92aaz9dxylw76.hdf5", + ), + ), +) + + +def get_ds_path(task, ds_type, return_info=False): + if task in SINGLE_STAGE_TASK_DATASETS: + ds_config = SINGLE_STAGE_TASK_DATASETS[task] + elif task in MULTI_STAGE_TASK_DATASETS: + ds_config = MULTI_STAGE_TASK_DATASETS[task] + else: + raise ValueError + + if ds_type == "human_raw": + folder = ds_config["human_path"] + fname = "demo.hdf5" + elif ds_type == "human_im": + folder = ds_config["human_path"] + if task in SINGLE_STAGE_TASK_DATASETS: + fname = "demo_gentex_im128_randcams.hdf5" + elif task in MULTI_STAGE_TASK_DATASETS: + fname = "demo_im128.hdf5" + elif ds_type == "mg_im": + # mg dataset is not available for all tasks + folder = ds_config.get("mg_path", None) + fname = "demo_gentex_im128_randcams.hdf5" + else: + raise ValueError + + # if dataset type is not registered, return None + if folder is None: + ret = (None, None) if return_info is True else None + return ret + + if macros.DATASET_BASE_PATH is None: + ds_base_path = os.path.join(Path(robocasa.__path__[0]).parent.absolute(), "datasets") + else: + ds_base_path = macros.DATASET_BASE_PATH + ds_path = os.path.join(ds_base_path, folder, fname) + + if return_info is False: + return ds_path + + ds_info = {} + ds_info["url"] = ds_config["download_links"][ds_type] + ds_info["horizon"] = ds_config["horizon"] + return ds_path, ds_info diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dexmg_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dexmg_utils.py new file mode 100644 index 0000000..4a7edbd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/dexmg_utils.py @@ -0,0 +1,99 @@ +class DexMGConfigHelper: + """ + Helper class for multi-inheritance scenarios, specifically designed to support task configuration + and environment interaction in robotic manipulation tasks. + + Example Usage: + This class is intended to be used in multi-inheritance cases such as: + class PnPCounterToSink(Kitchen, DexMGConfigHelper) + which will automatically generate a new configuration class: + class PnPCounterToSink_Config(MG_Config) + + Behavior: + 1. In the implementation of the MG_RoboSuiteHumanoidGeneric class: + - `self.env` will be an instance of PnPCounterToSink, enabling access to: + - `self.env.get_object()` to retrieve key objects. + - `self.env.get_subtask_term_signals()` to obtain signal information. + 2. In the implementation of PnPCounterToSink_Config: + - `self.task_config()` delegates to `PnPCounterToSink.task_config`, which defines + subtask divisions based on objects and signals. + + Attributes: + subclasses (list): A list storing tuples of subclass names and their respective classes. + """ + + class AttrDict(dict): + def __getattr__(self, key): + if key not in self: + self[key] = ( + DexMGConfigHelper.AttrDict() + ) # Create a new AttrDict if key doesn't exist + return self[key] + + def __setattr__(self, key, value): + self[key] = value + + def to_dict(self): + """Recursively converts AttrDict to a normal dictionary""" + return { + key: (value.to_dict() if isinstance(value, DexMGConfigHelper.AttrDict) else value) + for key, value in self.items() + } + + subclasses = [] + + def __init_subclass__(cls, **kwargs): + """ + Automatically registers each subclass of DexMGConfigHelper. + + Args: + cls: The subclass being registered. + kwargs: Additional keyword arguments. + """ + super().__init_subclass__(**kwargs) + DexMGConfigHelper.subclasses.append((cls.__name__, cls)) + + def __init__(self): + """ + Initialize a DexMGConfigHelper instance. + Note: This is an abstract class and should not be instantiated directly. + """ + pass + + # Misc functions copied from dexmimicgen.mimicgen.env_interfaces.robosuite_humanoid + def get_grippers(self): + """ + Get grippers for the environment. + """ + return self.robots[0].gripper["right"], self.robots[0].gripper["left"] + + def get_object(self): + """ + Retrieve a key object required for the task. + This method must be implemented in subclasses. + + Raises: + NotImplementedError: If the method is not overridden in a subclass. + """ + raise NotImplementedError + + def get_subtask_term_signals(self): + """ + Retrieve signals used to define subtask termination conditions. + This method must be implemented in subclasses. + + Raises: + NotImplementedError: If the method is not overridden in a subclass. + """ + raise NotImplementedError + + @staticmethod + def task_config(): + """ + Define the configuration for dividing a task into subtasks. + This method must be implemented in subclasses. + + Raises: + NotImplementedError: If the method is not overridden in a subclass. + """ + raise NotImplementedError diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/env_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/env_utils.py new file mode 100644 index 0000000..d075ca7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/env_utils.py @@ -0,0 +1,105 @@ +from robocasa.utils.dataset_registry import ( + get_ds_path, + SINGLE_STAGE_TASK_DATASETS, + MULTI_STAGE_TASK_DATASETS, +) +from robocasa.scripts.playback_dataset import get_env_metadata_from_dataset +from robosuite.controllers import load_composite_controller_config +import os +import robosuite +import imageio +import numpy as np +from tqdm import tqdm +from termcolor import colored + + +def create_env( + env_name, + # robosuite-related configs + robots="PandaOmron", + camera_names=[ + "robot0_agentview_left", + "robot0_agentview_right", + "robot0_eye_in_hand", + ], + camera_widths=128, + camera_heights=128, + seed=None, + # robocasa-related configs + obj_instance_split=None, + generative_textures=None, + randomize_cameras=False, + layout_and_style_ids=None, + layout_ids=None, + style_ids=None, +): + controller_config = load_composite_controller_config( + controller=None, + robot=robots if isinstance(robots, str) else robots[0], + ) + + env_kwargs = dict( + env_name=env_name, + robots=robots, + controller_configs=controller_config, + camera_names=camera_names, + camera_widths=camera_widths, + camera_heights=camera_heights, + has_renderer=False, + has_offscreen_renderer=True, + ignore_done=True, + use_object_obs=True, + use_camera_obs=True, + camera_depths=False, + seed=seed, + obj_instance_split=obj_instance_split, + generative_textures=generative_textures, + randomize_cameras=randomize_cameras, + layout_and_style_ids=layout_and_style_ids, + layout_ids=layout_ids, + style_ids=style_ids, + translucent_robot=False, + ) + + env = robosuite.make(**env_kwargs) + return env + + +def run_random_rollouts(env, num_rollouts, num_steps, video_path=None): + video_writer = None + if video_path is not None: + video_writer = imageio.get_writer(video_path, fps=20) + + info = {} + num_success_rollouts = 0 + for rollout_i in tqdm(range(num_rollouts)): + obs = env.reset() + for step_i in range(num_steps): + # sample and execute random action + action = np.random.uniform(low=env.action_spec[0], high=env.action_spec[1]) + obs, _, _, _ = env.step(action) + + if video_writer is not None: + video_img = env.sim.render( + height=512, width=512, camera_name="robot0_agentview_center" + )[::-1] + video_writer.append_data(video_img) + + if env._check_success(): + num_success_rollouts += 1 + break + + if video_writer is not None: + video_writer.close() + print(colored(f"Saved video of rollouts to {video_path}", color="yellow")) + + info["num_success_rollouts"] = num_success_rollouts + + return info + + +if __name__ == "__main__": + # select random task to run rollouts for + env_name = np.random.choice(list(SINGLE_STAGE_TASK_DATASETS) + list(MULTI_STAGE_TASK_DATASETS)) + env = create_eval_env(env_name=env_name) + info = run_random_rollouts(env, num_rollouts=3, num_steps=100, video_path="/tmp/test.mp4") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/__init__.py new file mode 100644 index 0000000..af2ec96 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/__init__.py @@ -0,0 +1,4 @@ +from .gymnasium_basic import RoboCasaEnv # noqa: F401 + + +# from .gymnasium_gearbc import GearBCRoboCasaEnv # noqa: F401 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/gymnasium_basic.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/gymnasium_basic.py new file mode 100644 index 0000000..e652c6f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/gym_utils/gymnasium_basic.py @@ -0,0 +1,417 @@ +import datetime, uuid +from copy import deepcopy +import gymnasium as gym +import h5py +import numpy as np +import os +import robocasa.utils.transform_utils as T +import robosuite +from gymnasium import spaces +from robocasa.models.robots import ( + GR00T_ROBOCASA_ENVS_GR1_ARMS_ONLY, + GR00T_ROBOCASA_ENVS_GR1_ARMS_AND_WAIST, + GR00T_ROBOCASA_ENVS_GR1_FIXED_LOWER_BODY, + gather_robot_observations, + make_key_converter, +) +from robosuite.controllers import load_composite_controller_config +from robosuite.controllers.composite.composite_controller import HybridMobileBase +from robocasa.wrappers.ik_wrapper import IKWrapper + + +ALLOWED_LANGUAGE_CHARSET = ( + "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789 ,.\n\t[]{}()!?'_:" +) + + +def create_env_robosuite( + env_name, + # robosuite-related configs + robots="PandaOmron", + controller_configs=None, + camera_names=[ + "egoview", + "robot0_eye_in_left_hand", + "robot0_eye_in_right_hand", + ], + camera_widths=128, + camera_heights=128, + render_camera=None, + enable_render=True, + seed=None, + # robocasa-related configs + obj_instance_split=None, + generative_textures=None, + randomize_cameras=False, + layout_and_style_ids=None, + layout_ids=None, + style_ids=None, + onscreen=False, + renderer="mujoco", + translucent_robot=False, + control_freq=20, +): + if controller_configs is None: + controller_configs = load_composite_controller_config( + controller=None, + robot=robots if isinstance(robots, str) else robots[0], + ) + env_kwargs = dict( + env_name=env_name, + robots=robots, + controller_configs=controller_configs, + camera_names=camera_names, + camera_widths=camera_widths, + camera_heights=camera_heights, + has_renderer=onscreen, + has_offscreen_renderer=enable_render, + renderer=renderer, + ignore_done=True, + use_object_obs=True, + use_camera_obs=enable_render, + camera_depths=False, + seed=seed, + translucent_robot=translucent_robot, + control_freq=control_freq, + render_camera=render_camera, + randomize_cameras=randomize_cameras, + ) + + env = robosuite.make(**env_kwargs) + return env, env_kwargs + + +class RoboCasaEnv(gym.Env): + def __init__( + self, + env_name=None, + robots_name=None, + camera_names=None, + camera_widths=None, + camera_heights=None, + enable_render=True, + render_camera=None, + onscreen=False, + dump_rollout_dataset_dir=None, + rollout_hdf5=None, + rollout_trainset=None, + controller_configs=None, + ik_indicator=False, + **kwargs, # Accept additional kwargs + ): + self.key_converter = make_key_converter(robots_name) + ( + _, + camera_names, + default_camera_widths, + default_camera_heights, + ) = self.key_converter.get_camera_config() + + if camera_widths is None: + camera_widths = default_camera_widths + if camera_heights is None: + camera_heights = default_camera_heights + + if controller_configs is not None: + controller_configs = os.path.join( + os.path.dirname(os.path.abspath(__file__)), + "../../../", + controller_configs, + ) + controller_configs = load_composite_controller_config( + controller=controller_configs, + robot=robots_name.split("_")[0], + ) + if ( + robots_name in GR00T_ROBOCASA_ENVS_GR1_ARMS_ONLY + or robots_name in GR00T_ROBOCASA_ENVS_GR1_ARMS_AND_WAIST + or robots_name in GR00T_ROBOCASA_ENVS_GR1_FIXED_LOWER_BODY + ): + controller_configs["type"] = "BASIC" + controller_configs["composite_controller_specific_configs"] = {} + controller_configs["control_delta"] = False + + self.env, self.env_kwargs = create_env_robosuite( + env_name=env_name, + robots=robots_name.split("_"), + controller_configs=controller_configs, + camera_names=camera_names, + camera_widths=camera_widths, + camera_heights=camera_heights, + render_camera=render_camera, + enable_render=enable_render, + onscreen=onscreen, + **kwargs, # Forward kwargs to create_env_robosuite + ) + + if ik_indicator: + self.env = IKWrapper(self.env, ik_indicator=True) + + # TODO: the following info should be output by gr00trobocasa + self.camera_names = camera_names + self.camera_widths = camera_widths + self.camera_heights = camera_heights + self.enable_render = enable_render and not onscreen + self.render_obs_key = f"{camera_names[0]}_image" + self.render_cache = None + + # setup spaces + action_space = spaces.Dict() + for robot in self.env.robots: + cc = robot.composite_controller + pf = robot.robot_model.naming_prefix + for part_name, controller in cc.part_controllers.items(): + min_value, max_value = -1, 1 + start_idx, end_idx = cc._action_split_indexes[part_name] + shape = [end_idx - start_idx] + this_space = spaces.Box( + low=min_value, high=max_value, shape=shape, dtype=np.float32 + ) + action_space[f"{pf}{part_name}"] = this_space + if isinstance(cc, HybridMobileBase): + this_space = spaces.Discrete(2) + action_space[f"{pf}base_mode"] = this_space + + action_space = spaces.Dict(action_space) + self.action_space = action_space + + # Calling env._get_observations(force_update=True) will pollute the observables. + # This is safe because it is only called from the constructor and will be overwritten later in reset(). + obs = ( + self.env.viewer._get_observations(force_update=True) + if self.env.viewer_get_obs + else self.env._get_observations(force_update=True) + ) + obs.update(gather_robot_observations(self.env)) + observation_space = spaces.Dict() + for obs_name, obs_value in obs.items(): + shape = list(obs_value.shape) + if obs_name.endswith("_image"): + continue + min_value, max_value = -1, 1 + this_space = spaces.Box(low=min_value, high=max_value, shape=shape, dtype=np.float32) + observation_space[obs_name] = this_space + + for camera_name in camera_names: + shape = [camera_heights, camera_widths, 3] + this_space = spaces.Box(low=0, high=255, shape=shape, dtype=np.uint8) + observation_space[f"{camera_name}_image"] = this_space + + observation_space["language"] = spaces.Text( + max_length=256, charset=ALLOWED_LANGUAGE_CHARSET + ) + + self.observation_space = observation_space + + self.dump_rollout_dataset_dir = dump_rollout_dataset_dir + self.gr00t_exporter = None + self.np_exporter = None + + self.rollout_hdf5 = rollout_hdf5 + self.rollout_trainset = rollout_trainset + self.rollout_initial_state = {} + + def begin_rollout_dataset_dump(self): + if self.dump_rollout_dataset_dir is not None: + gr00t_env_meta = dict( + env_name=self.env_kwargs["env_name"], + env_version=robosuite.__version__, + type=1, + env_kwargs=deepcopy(self.env_kwargs), + ) + gr00t_dir = os.path.join( + self.dump_rollout_dataset_dir, + self.env_kwargs["env_name"] + + "_" + + "_".join(robot.name for robot in self.env.robots) + + "_Env", + f"{datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}-{str(uuid.uuid4())[:8]}", + ) + if not os.path.exists(gr00t_dir): + os.makedirs(gr00t_dir, exist_ok=True) + self.gr00t_exporter = Gr00tExporter( + gr00t_dir, + self.env, + gr00t_env_meta, + ) + self.np_exporter = {} + self.np_exporter["success"] = [] + for signal in self.env.get_subtask_term_signals(): + self.np_exporter[signal] = [] + + def process_rollout_dataset_dump_before_step(self, env_action): + if self.gr00t_exporter is not None: + self.gr00t_exporter.add_record_before_step(self.env, env_action) + self.np_exporter["success"].append(self.env.reward()) + for signal, value in self.env.get_subtask_term_signals().items(): + self.np_exporter[signal].append(value) + + def process_rollout_dataset_dump_after_step(self, env_action): + if self.gr00t_exporter is not None: + self.gr00t_exporter.add_record_after_step(self.env, env_action) + + def complete_rollout_dataset_dump(self): + if self.gr00t_exporter is not None and self.np_exporter is not None: + data = {k: np.array(v, dtype=np.float32) for k, v in self.np_exporter.items()} + np.savez(os.path.join(self.gr00t_exporter.gr00t_dir, "rewards.npz"), **data) + self.gr00t_exporter.finish() + self.np_exporter = None + self.gr00t_exporter = None + + def get_basic_observation(self, raw_obs): + raw_obs.update(gather_robot_observations(self.env)) + + # Image are in (H, W, C), flip it upside down + def process_img(img): + return np.copy(img[::-1, :, :]) + + for obs_name, obs_value in raw_obs.items(): + if obs_name.endswith("_image"): + # image observations + raw_obs[obs_name] = process_img(obs_value) + else: + # non-image observations + raw_obs[obs_name] = obs_value.astype(np.float32) + + # Return black image if rendering is disabled + if not self.enable_render: + for name in self.camera_names: + raw_obs[f"{name}_image"] = np.zeros( + (self.camera_heights, self.camera_widths, 3), dtype=np.uint8 + ) + + self.render_cache = raw_obs[self.render_obs_key] + raw_obs["language"] = self.env.get_ep_meta().get("lang", "") + + return raw_obs + + def reset(self, seed=None, options=None): + if seed is not None and self.rollout_trainset is not None and seed < self.rollout_trainset: + if seed not in self.rollout_initial_state: + f = h5py.File(self.rollout_hdf5, "r") + demos = list(f["data"].keys()) + inds = np.argsort([int(elem[5:]) for elem in demos]) + demos = [demos[i] for i in inds] + ep = demos[seed] + self.rollout_initial_state[seed] = { + "states": f["data/{}/states".format(ep)][0], + "ep_meta": f["data/{}".format(ep)].attrs.get("ep_meta", None), + "model": f["data/{}".format(ep)].attrs["model_file"], + } + reset_to(self.env, self.rollout_initial_state[seed]) + obs = None + else: + # NOTE: self.env can be either a robosuite environment or ik wrapper + if isinstance(self.env, IKWrapper): + self.env.unwrapped.seed = seed + self.env.unwrapped.rng = np.random.default_rng(seed=seed) + else: + self.env.seed = seed + self.env.rng = np.random.default_rng(seed=seed) + raw_obs = self.env.reset() + obs = self.get_basic_observation(raw_obs) + + info = {} + info["success"] = False + info["intermediate_signals"] = {} + + self.complete_rollout_dataset_dump() + self.begin_rollout_dataset_dump() + + return obs, info + + def step(self, action_dict): + env_action = [] + for robot in self.env.robots: + cc = robot.composite_controller + pf = robot.robot_model.naming_prefix + action = np.zeros(cc.action_limits[0].shape) + for part_name, controller in cc.part_controllers.items(): + start_idx, end_idx = cc._action_split_indexes[part_name] + act = action_dict.pop(f"{pf}{part_name}") + action[start_idx:end_idx] = act + + # set external torque compensation if available + if "gripper" not in part_name and controller.use_external_torque_compensation: + controller.external_torque_compensation = action_dict.pop( + f"{pf}{part_name}_tau" + ) + if isinstance(cc, HybridMobileBase): + action[-1] = action_dict.pop(f"{pf}base_mode") + env_action.append(action) + + env_action = np.concatenate(env_action) + + self.process_rollout_dataset_dump_before_step(env_action) + raw_obs, reward, done, info = self.env.step(env_action) + self.process_rollout_dataset_dump_after_step(env_action) + + obs = self.get_basic_observation(raw_obs) + + truncated = False + + info["success"] = reward > 0 + info["intermediate_signals"] = {} + if hasattr(self.env, "_get_intermediate_signals"): + info["intermediate_signals"] = self.env._get_intermediate_signals() + + if hasattr(self.env, "_get_auxiliary_states"): + info["auxiliary_states"] = self.env._get_auxiliary_states() + + if hasattr(self.env, "obj_body_id"): + for obj_name in self.env.obj_body_id.keys(): + info[f"{obj_name}_pos"] = list( + self.env.sim.data.body_xpos[self.env.obj_body_id[obj_name]] + ) + info[f"{obj_name}_quat_xyzw"] = list( + T.convert_quat( + np.array(self.env.sim.data.body_xquat[self.env.obj_body_id[obj_name]]), + to="xyzw", + ) + ) + + return obs, reward, done, truncated, info + + def render(self): + if self.render_cache is None: + raise RuntimeError("Must run reset or step before render.") + return self.render_cache + + def close(self): + self.complete_rollout_dataset_dump() + self.env.close() + + def get_actuator_names(self): + joint_names = self.get_joint_names() + + model = self.env.sim.model + + actuated_joint_names = [] + for ii in range(model.nu): + joint_id = model.actuator_trnid[ii, 0] + actuated_joint_names.append(model.joint(joint_id).name) + + actuator_names = {} + actuator_indices = {} + for part_name, _joint_names in joint_names.items(): + actuator_names[part_name] = [] + actuator_indices[part_name] = [] + for _jn in _joint_names: + if _jn in actuated_joint_names: + actuator_names[part_name].append(_jn) + actuator_indices[part_name].append(actuated_joint_names.index(_jn)) + actuator_names[part_name] = ( + np.array(actuator_names[part_name])[np.argsort(actuator_indices[part_name])] + ).tolist() + return actuator_names + + def get_joint_names(self): + joint_names = {} + for _robot in self.env.robots: + cc = _robot.composite_controller + for part_name, part_controller in cc.part_controllers.items(): + if "gripper" in part_name: + joint_names[part_name] = part_controller.joint_index + else: + joint_names[part_name] = part_controller.joint_names + return joint_names diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/__init__.py new file mode 100644 index 0000000..4eb69cb --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/__init__.py @@ -0,0 +1,3 @@ +import os + +assets_root = os.path.join(os.path.dirname(__file__), "assets") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/file_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/file_utils.py new file mode 100644 index 0000000..c246eb9 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/file_utils.py @@ -0,0 +1,35 @@ +from pathlib import Path + +# import robosuite_model_zoo +import os +import shutil + + +def get_ext(path): + return Path(path).suffix + + +def get_stem(path): + return Path(path).stem + + +def get_name(path): + return Path(path).name + + +def make_asset_path(base_asset_path, model_prefix, model_name): + if base_asset_path is None: + base_asset_path = os.path.join( + os.path.dirname(robosuite_model_zoo.__file__), + "assets_private", + ) + if model_prefix is not None: + base_asset_path = os.path.join(base_asset_path, model_prefix) + asset_path = os.path.join(base_asset_path, model_name) + + if os.path.exists(asset_path): + shutil.rmtree(asset_path) + + os.makedirs(asset_path) + + return asset_path diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/fixture_template.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/fixture_template.xml new file mode 100644 index 0000000..6fece82 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/fixture_template.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/install_vhacd.sh b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/install_vhacd.sh new file mode 100644 index 0000000..f4bc7c6 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/install_vhacd.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# +# Install V-HACD 4.0. +# Adapted from https://github.com/kevinzakka/obj2mjcf/blob/main/install_vhacd.sh + +# Check that cmake is installed. +t=`which cmake` +if [ -z "$t" ]; then + echo "You need cmake to install V-HACD." 1>&2 + exit 1 +fi + +# Clone and build executable. +# TODO(kevin): Peg to a 4.0 release when available, see #113. +git clone https://github.com/kmammou/v-hacd.git +cd v-hacd/app +cmake CMakeLists.txt +cmake --build . + +# Add executable to /usr/local/bin. +sudo ln -s "$PWD/TestVHACD" /usr/local/bin/TestVHACD diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/log_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/log_utils.py new file mode 100644 index 0000000..31e05dd --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/log_utils.py @@ -0,0 +1,46 @@ +import json +import numpy as np +from termcolor import colored +from copy import deepcopy +import os +import sys +import textwrap + + +class NumpyEncoder(json.JSONEncoder): + """ + Special json encoder for numpy types + From https://stackoverflow.com/a/49677241 + """ + + def default(self, obj): + if isinstance(obj, np.integer): + return int(obj) + elif isinstance(obj, np.floating): + return float(obj) + elif isinstance(obj, np.ndarray): + return obj.tolist() + return json.JSONEncoder.default(self, obj) + + +def maybe_log_info(message, log=False, color="yellow", indent=False): + if log: + if indent: + message = textwrap.indent(message, " " * 5) + print(colored(message, color)) + + +def save_meta(args, asset_path): + # get args + meta = vars(args) + + # get relative path of running script + script_path = os.path.abspath(sys.argv[0]) + path_list = script_path.split("/") + # idx = len(path_list) - 1 - path_list[::-1].index("robosuite_model_zoo") + # meta["script"] = "/".join(path_list[idx:]) + meta["cmd"] = "python " + " ".join(sys.argv) + + # save meta data + with open(os.path.join(asset_path, "meta.json"), "w") as outfile: + json.dump(meta, outfile, indent=4) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_gen_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_gen_utils.py new file mode 100644 index 0000000..f802c05 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_gen_utils.py @@ -0,0 +1,800 @@ +import numpy as np +import trimesh +import os +import robosuite +import xml.etree.ElementTree as ET +import shutil +import json +from termcolor import colored +from copy import deepcopy +from pathlib import Path + +# import robosuite_model_zoo +from robosuite.utils.transform_utils import mat2quat +import robocasa.utils.model_zoo.log_utils as LogUtils +import robocasa.utils.model_zoo.mtl_utils as MtlUtils + +from robosuite.utils.mjcf_utils import array_to_string, string_to_array + + +def parse_model_info( + model_path, + model_name, + coll_model_path, + asset_path, + obj_path=None, + verbose=False, + center=False, + prescale=False, + rot=None, + transform=None, +): + """ + Extract visual and collision geoms from + main model file and collision model file + """ + + LogUtils.maybe_log_info("Parsing input models...", log=verbose) + + geoms, textures, materials, transform = parse_model( + model_path, + model_name, + asset_path, + obj_path=obj_path, + center=center, + prescale=prescale, + rot=rot, + transform=transform, + # visual_only=(coll_model_path is None), + visual_only=True, + verbose=verbose, + ) + + coll_geoms, bb_info = parse_coll_model( + coll_model_path, + asset_path, + verbose=verbose, + transform=transform, + ) + + if coll_geoms is not None: + geoms = geoms + coll_geoms + + geoms_to_print = deepcopy(geoms) + for g in geoms_to_print: + if "rot_orig" in g: + del g["rot_orig"] + LogUtils.maybe_log_info( + "parsed geoms:\n" + json.dumps(geoms_to_print, indent=4, cls=LogUtils.NumpyEncoder), + log=verbose, + indent=True, + ) + + model_info = dict( + geoms=geoms, + textures=textures, + materials=materials, + bb=bb_info, + ) + + return model_info + + +def parse_model( + model_path, + model_name, + asset_path, + obj_path=None, + visual_only=False, + center=False, + prescale=False, + rot=None, + transform=None, + verbose=False, +): + """ + Extract meshes from main model file + """ + + LogUtils.maybe_log_info( + "parsing (main) model: {}".format(model_path), + log=verbose, + indent=True, + ) + + # load the model + if obj_path is None: + obj_files = [ + os.path.join(model_path, name) + for name in os.listdir(model_path) + if name.endswith(".obj") + ] + obj_file = obj_files[0] + else: + obj_file = obj_path + + geoms, textures, materials = [], [], [] + vis_path = os.path.join(asset_path, "visual") + coll_path = os.path.join(asset_path, "collision") + + if not os.path.isdir(vis_path): + os.makedirs(vis_path) + if not os.path.isdir(coll_path): + os.makedirs(coll_path) + + resolver = trimesh.resolvers.FilePathResolver(os.path.dirname(obj_file)) + model = trimesh.load( + obj_file, + resolver=resolver, + split_object=True, + # group_material=True, + process=False, + maintain_order=False, + ) + + # If transform is passed as an argument, center and prescale must be False. Rotation is applied afterward regardless. + if transform is not None: + assert center is False and prescale is False + + if center: + mat_t = np.eye((4)) + center = (model.bounds[0] + model.bounds[1]) / 2 + mat_t[:3, 3] = -center + transform = mat_t + + if prescale: + mat_s = np.eye((4)) + scale_factor = 1 / np.max(model.bounds[1] - model.bounds[0]) + mat_s *= scale_factor + + if transform is None: + transform = mat_s + else: + transform = np.matmul(mat_s, transform) + + rot = rot or [] + for r in rot: + mat_R = np.eye(4) + + if r in ["x", "x90"]: + mat_R[:3, :3] = [[1, 0, 0], [0, 0, -1], [0, 1, 0]] + elif r in ["x180"]: + mat_R[:3, :3] = [[1, 0, 0], [0, -1, 0], [0, 0, -1]] + elif r in ["x270"]: + mat_R[:3, :3] = [[1, 0, 0], [0, 0, 1], [0, -1, 0]] + + elif r in ["y", "y90"]: + mat_R[:3, :3] = [[0, 0, 1], [0, 1, 0], [-1, 0, 0]] + elif r in ["y180"]: + mat_R[:3, :3] = [[-1, 0, 0], [0, 1, 0], [0, 0, -1]] + elif r in ["y270"]: + mat_R[:3, :3] = [[0, 0, -1], [0, 1, 0], [1, 0, 0]] + + elif r in ["z", "z90"]: + mat_R[:3, :3] = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] + elif r in ["z180"]: + mat_R[:3, :3] = [[-1, 0, 0], [0, -1, 0], [0, 0, 1]] + elif r in ["z270"]: + mat_R[:3, :3] = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]] + elif r in ["z60"]: + mat_R[:3, :3] = [ + [0.5000000, -0.8660254, 0.0000000], + [0.8660254, 0.5000000, 0.0000000], + [0, 0, 1], + ] + + else: + raise ValueError("Invalid choice of rotation {}".format(r)) + + if transform is None: + transform = mat_R + else: + transform = np.matmul(mat_R, transform) + + if transform is not None: + if isinstance(model, trimesh.base.Trimesh): + model.apply_transform(transform) + else: + for k in model.geometry: + model.geometry[k].apply_transform(transform) + + # debugging + # model.show() + # exit() + + mtls = MtlUtils.get_mtls(obj_file, work_dir=vis_path) + + """ + save mtl info. this section adapted from obj2mjf (credit: Kevin Zakka) + """ + for mtl in mtls: + if mtl.map_Kd is not None: + # Create the texture asset. + texture_path = Path(mtl.map_Kd) + texture = dict( + type="2d", + name=texture_path.stem, + file=os.path.join("visual", texture_path.name), + ) + # Reference the texture asset in a material asset. + material = dict( + name=mtl.name, + texture=texture_path.stem, + specular=mtl.mjcf_specular(), + shininess=mtl.mjcf_shininess(), + ) + + textures.append(texture) + materials.append(material) + else: + material = dict( + name=mtl.name, + specular=mtl.mjcf_specular(), + shininess=mtl.mjcf_shininess(), + rgba=mtl.mjcf_rgba(), + ) + materials.append(material) + + submodels = [] + + if isinstance(model, trimesh.base.Trimesh): + submodels = [(materials[-1]["name"], model)] + else: + submodels = list(model.geometry.items()) + + for sm_i, (sm_name, sm) in enumerate(submodels): + # ch_mesh = sm.convex_hull + # bb_mesh = sm.bounding_box_oriented + + # set visual geom path, copy to visual folder + # model_file = os.path.basename(model_path) + obj_name = os.path.basename(obj_file)[:-4] + vis_geom_path = os.path.join(vis_path, "{}_{}.obj".format(obj_name, sm_i)) + # shutil.copyfile( + # obj_file, + # vis_geom_path, + # ) + sm.export(vis_geom_path) + + vis_geom = dict( + obj_type="visual", + geom_type="mesh", + path=vis_geom_path, + name="{}_{}_vis".format(obj_name, sm_i), + material=sm_name, + # **get_bb_info(sm.bounding_box_oriented) + ) + geoms.append(vis_geom) + + if visual_only: + continue + + ch_mesh = sm.convex_hull + # bb_mesh = ch_mesh.bounding_box_oriented + + # obj_name = os.path.basename(obj_file)[:-3] + coll_geom_path = os.path.join(coll_path, "{}_{}.obj".format(obj_name, sm_i)) + coll_geom = dict( + obj_type="collision", + geom_type="mesh", + # path=os.path.join(coll_path, "{}_ch.stl".format(model_name)), + path=coll_geom_path, + name="{}_{}_coll".format(obj_name, sm_i), + **get_bb_info(ch_mesh.bounding_box_oriented), + ) + geoms.append(coll_geom) + ch_mesh.export(coll_geom["path"]) + + return geoms, textures, materials, transform + + +def parse_coll_model( + coll_model_path, + asset_path, + verbose=False, + transform=None, +): + """ + Extract list of primitive geoms from collision model file + """ + + if coll_model_path is None: + return None, None + + LogUtils.maybe_log_info( + "parsing collision model: {}".format(coll_model_path), + log=verbose, + indent=True, + ) + + # load the model + obj_files = [ + os.path.join(coll_model_path, name) + for name in os.listdir(coll_model_path) + if name.endswith(".obj") + ] + + geoms = [] + coll_path = os.path.join(asset_path, "collision") + + scene = trimesh.scene.scene.Scene() + + for obj_file in obj_files: + resolver = trimesh.resolvers.FilePathResolver(os.path.dirname(obj_file)) + model = trimesh.load(obj_file, resolver=resolver) + + if transform is not None: + model.apply_transform(transform) + model.export(obj_file) + + # ch_mesh = model.convex_hull + # bb_mesh = ch_mesh.bounding_box_oriented + + scene.add_geometry(model) + + if not os.path.isdir(coll_path): + os.makedirs(coll_path) + + # set visual geom path, copy to visual folder + # model_file = os.path.basename(model_path) + obj_name = os.path.basename(obj_file)[:-3] + coll_geom_path = os.path.join(coll_path, os.path.basename(obj_file)) + # shutil.copyfile( + # obj_file, + # coll_geom_path, + # ) + + # obj_name = os.path.basename(obj_file)[:-3] + coll_geom = dict( + obj_type="collision", + geom_type="mesh", + # path=os.path.join(coll_path, "{}_ch.stl".format(model_name)), + path=coll_geom_path, + name="{}_coll".format(obj_name), + # **get_bb_info(bb_mesh) + ) + geoms.append(coll_geom) + + bb_info = get_bb_info(scene.bounding_box) + + # f = open(coll_model_path, "r") + # lines = f.readlines() + # f.close() + + # geoms = [] + + # start_inds = [i for (i, line) in enumerate(lines) if line.startswith("o ")] + + # num_coll_meshes = 0 + # for start, end in zip(start_inds, start_inds[1:] + [len(lines)]): + # obj_lines = lines[start:end] + + # # filter out lines corresponding to faces + # obj_lines_filtered = [line for line in obj_lines if not line.startswith("f ")] + + # geom = dict() + + # tmp = tempfile.NamedTemporaryFile(suffix=".obj") + # with open(tmp.name, "w") as f: + # [f.write(line) for line in obj_lines_filtered] + + # mesh = trimesh.load(tmp.name) + + # bb_mesh = mesh.bounding_box_oriented + + # geom.update( + # obj_name=obj_lines[0], + # obj_type="collision", + # **get_bb_info(bb_mesh) + # ) + + # obj_type_substr = obj_lines_filtered[0][2:] + # if obj_type_substr.startswith("Cube"): + # geom_type = "box" + # elif obj_type_substr.startswith("Cylinder"): + # geom_type = "cylinder" + + # """ + # post-process size and orientation of cylinder + # """ + + # pos = geom["pos"] + # quat = geom["quat"] + # size = geom["size"] + + # T = bb_mesh.primitive.transform + + # trans_mesh = mesh.copy() + # trans_mesh.apply_transform(np.linalg.inv(T)) + + # V = np.array(trans_mesh.vertices) + # n = len(V) # number of vertices + # th = 1e-5 # threshold for checking vertices + + # upright_axis = None + # for idx in [0, 1, 2]: + # V = np.take(V, indices=np.argsort(V, axis=0)[:,idx], axis=0) + + # if np.std(V[:n//2,idx]) < th and np.std(V[-n//2:,idx]) < th: + # upright_axis = idx + + # assert upright_axis is not None + # circle_axes = np.delete(np.arange(3), upright_axis) + # assert np.abs(size[circle_axes][1] - size[circle_axes][0]) < 0.01, ( + # "Detected elliptic cylinder! " + # "Mujoco only accepts circular cylinders." + # ) + + # if upright_axis == 0: + # geom["quat"] = mat2quat( + # np.linalg.inv( + # np.matmul( + # np.array([ + # [0, 0, 1], + # [0, 1, 0], + # [-1, 0, 0], + # ]), + # np.linalg.inv(geom["rot_orig"]) + # ) + # ) + # )[[3,0,1,2]] + # elif upright_axis == 1: + # raise NotImplementedError + + # geom["size"] = [np.mean(size[circle_axes]), size[upright_axis]] + + # elif obj_type_substr.startswith("Sphere"): + # geom_type = "ellipsoid" + # else: + # raise ValueError( + # "Detected non-primitive object in collision model:", + # obj_lines_filtered[0] + # ) + + # geom["geom_type"] = geom_type + + # geoms.append(geom) + + return geoms, bb_info + + +def get_geom_element(model_name, geom_info, sc=1.0, show_if_coll_geom=False): + """ + Generate xml element for mjcf geom + """ + + if geom_info.get("hide", False): + return None + + elem = ET.Element("geom") + + elem.attrib["solimp"] = "0.998 0.998 0.001" + elem.attrib["solref"] = "0.001 1" + elem.attrib["density"] = "100" + elem.attrib["friction"] = "0.95 0.3 0.1" + + if geom_info["geom_type"] == "mesh": + elem.attrib["type"] = "mesh" + elem.attrib["mesh"] = geom_info["name"] + else: + raise NotImplementedError + elem.attrib["type"] = geom_info["geom_type"] + elem.attrib["pos"] = " ".join(["{:.5f}".format(v * sc) for v in geom_info["pos"]]) + elem.attrib["quat"] = " ".join(["{:.5f}".format(v) for v in geom_info["quat"]]) + size = geom_info["size"] + elem.attrib["size"] = " ".join(["{:.5f}".format(v * sc / 2) for v in size]) + + if geom_info["obj_type"] == "visual": + elem.attrib["conaffinity"] = "0" + elem.attrib["contype"] = "0" + elem.attrib["group"] = "1" + elem.attrib["material"] = geom_info.get("material", model_name) + elif geom_info["obj_type"] == "collision": + elem.attrib["group"] = "0" + # elem.attrib["condim"] = "4" + + if show_if_coll_geom: + elem.attrib["rgba"] = "0.8 0.8 0.8 0.3" + else: + elem.attrib["rgba"] = "0.8 0.8 0.8 0.0" + + return elem + + +def get_texture_element(texture_info): + elem = ET.Element("texture") + + for k, v in texture_info.items(): + elem.attrib[k] = str(v) + + return elem + + +def get_material_element(material_info): + elem = ET.Element("material") + + for k, v in material_info.items(): + elem.attrib[k] = str(v) + + return elem + + +def get_mesh_element(geom_info, sc=1.0): + """ + Generate xml element for mjcf mesh asset + """ + assert geom_info["geom_type"] == "mesh" + + if geom_info.get("hide", False): + return None + + elem = ET.Element("mesh") + + elem.attrib["file"] = "/".join(geom_info["path"].split("/")[-2:]) + elem.attrib["name"] = geom_info["name"] + elem.attrib["scale"] = "{sc} {sc} {sc}".format(sc=sc) + + return elem + + +def generate_mjcf( + asset_path, + model_name, + model_info, + sc=1.0, + texture_path=None, + hide_vis_geoms=False, + show_coll_geoms=False, + show_sites=False, + verbose=False, +): + """ + Generate mjcf model for object + """ + + LogUtils.maybe_log_info("Generating MJCF...", log=verbose) + + geoms = model_info["geoms"] + textures = model_info["textures"] + materials = model_info["materials"] + bb_info = model_info["bb"] + + ### for now, assume mtl doesn't contain texture #### + # texture_from_mtl = False + # if texture_path is None: + # # get the texture used in the mtl file + # mtl_path = model_path[:-3] + 'mtl' + # texture_dict = {} + # with open(mtl_path, "r") as f: + # for i, line in enumerate(f.readlines()): + # if len(line) > 1: + # key, value = line.split(" ", 1) + # texture_dict[key] = value.strip() + # + # if "map_Kd" in texture_dict: + # texture_path = texture_dict["map_Kd"] + # texture_from_mtl = True + + # load template xml + tree = ET.parse( + os.path.join(os.path.dirname(os.path.realpath(__file__)), "object_template.xml") + ) + root = tree.getroot() + + # set model name + root.attrib["model"] = model_name + + # add mesh assets + asset = root.find("asset") + for geom in geoms: + if geom["geom_type"] == "mesh": + mesh_elem = get_mesh_element(geom, sc=sc) + if mesh_elem is not None: + asset.append(mesh_elem) + + # update default texture and material assets + texture = asset.find("texture") + texture.attrib["name"] = texture.attrib["name"].replace("template", model_name) + if texture_path is not None: + texture.attrib["file"] = "/".join(texture_path.split("/")[-1:]) + else: + texture.attrib["file"] = os.path.join( + os.path.dirname(robosuite.__file__), "models/assets/textures/ceramic.png" + ) + material = asset.find("material") + for k in ["name", "texture"]: + material.attrib[k] = material.attrib[k].replace("template", model_name) + + # add textures + for texture in textures: + texture_elem = get_texture_element(texture) + if texture_elem is not None: + asset.append(texture_elem) + + # add materials + for material in materials: + material_elem = get_material_element(material) + if material_elem is not None: + asset.append(material_elem) + + # add geoms + worldbody = root.find("worldbody") + body = worldbody.find("body").find("body") + for geom in geoms: + if geom["obj_type"] == "visual" and hide_vis_geoms: + continue + + geom_elem = get_geom_element( + model_name, + geom, + sc=sc, + show_if_coll_geom=show_coll_geoms, + ) + if geom_elem is not None: + body.append(geom_elem) + + sites = worldbody.find("body").findall("site") + + if show_sites: + object_body = worldbody.find("body").find("body") + vis_site_strings = [ + """""", + """""", + """""", + ] + for s in vis_site_strings: + object_body.append(ET.fromstring(s)) + + # for debugging + vis_sites = object_body.findall("site") + sites = sites + vis_sites + + if bb_info is not None: + for site in sites: + if site.get("name") == "horizontal_radius_site": + pos = bb_info["pos"].copy() + pos[0] += bb_info["size"][0] / 2 + pos[1] += bb_info["size"][1] / 2 + pos = np.array([pos[0], pos[1], pos[2]]) + site.set("pos", array_to_string(pos * sc)) + elif site.get("name") == "bottom_site": + pos = bb_info["pos"].copy() + pos[2] -= bb_info["size"][2] / 2 + pos = np.array([pos[0], pos[1], pos[2]]) + site.set("pos", array_to_string(pos * sc)) + elif site.get("name") == "top_site": + pos = bb_info["pos"].copy() + pos[2] += bb_info["size"][2] / 2 + pos = np.array([pos[0], pos[1], pos[2]]) + site.set("pos", array_to_string(pos * sc)) + + # save xml for new model + mjcf_path = os.path.join(asset_path, "model.xml") + tree.write(mjcf_path, encoding="utf-8") + LogUtils.maybe_log_info("Wrote MJCF to: {}".format(mjcf_path), log=verbose, indent=True) + + # LogUtils.maybe_log_info( + # "\nTest generated MJCF with robosuite:\npython {base_path}/scripts/object_play.py --device spacemouse --mjcf_path {mjcf_path}".format( + # base_path=os.path.dirname(robosuite_model_zoo.__file__), + # mjcf_path=mjcf_path, + # ), + # log=True, + # color="green" + # ) + + return mjcf_path + + +def get_file_name_and_type(path): + """ + Extract name of object model file and file format + """ + split_name = os.path.basename(path).split(".") + assert len(split_name) == 2 + return split_name[0], split_name[1] + + +def get_bb_info(bb_mesh): + """ + Extract size, position, and orientation (quat) from bounding box mesh + """ + ext = np.array(bb_mesh.primitive.extents) + transform = np.array(bb_mesh.primitive.transform) + + center = transform[:-1, 3] + rot = transform[:3, :3] + quat = mat2quat(rot)[[3, 0, 1, 2]] + + return dict( + pos=center, + quat=quat, + size=ext, + rot_orig=rot, + ) + + +def decompose_convex( + filename, + work_dir, + # V-HACD params + max_output_convex_hulls=32, + voxel_resolution=100000, + volume_error_percent=1.0, + max_hull_vert_count=64, +): + """ + Adapted from https://github.com/kevinzakka/obj2mjcf/blob/main/obj2mjcf/_cli.py + """ + + _VHACD_EXECUTABLE = shutil.which("TestVHACD") + _VHACD_OUTPUTS = ["decomp.obj", "decomp.stl"] + + obj_file = filename.resolve() + + import tempfile + import subprocess + from pathlib import Path + import enum + + with tempfile.TemporaryDirectory() as tmpdirname: + prev_dir = os.getcwd() + os.chdir(tmpdirname) + + # Copy the obj file to the temporary directory. + shutil.copy(obj_file, tmpdirname) + + # Call V-HACD, suppressing output. + ret = subprocess.run( + [ + f"{_VHACD_EXECUTABLE}", + obj_file.name, + "-o", + "obj", + "-h", + f"{max_output_convex_hulls}", # max_output_convex_hulls + "-r", + f"{voxel_resolution}", # voxel_resolution + "-e", + f"{volume_error_percent}", # volume_error_percent + "-d", + f"{14}", # max_recursion_depth + "-s", + f"{int(not False)}", # disable_shrink_wrap + # "-f", + # f"{enum.auto().name.lower()}", # fill_mode.name.lower() + "-v", + f"{max_hull_vert_count}", # max_hull_vert_count + "-a", + f"{int(not False)}", # disable_async + "-l", + f"{2}", # min_edge_length + "-p", + f"{int(False)}", # split_hull + ], + # stdout=subprocess.DEVNULL, + # stderr=subprocess.STDOUT, + check=True, + ) + # if ret.returncode != 0: + # logging.error(f"V-HACD failed on {filename}") + # return False + + # Remove the original obj file and the V-HACD output files. + for name in _VHACD_OUTPUTS + [obj_file.name]: + file_to_delete = Path(tmpdirname) / name + if file_to_delete.exists(): + file_to_delete.unlink() + + os.chdir(prev_dir) + + # Get list of sorted collisions. + collisions = list(Path(tmpdirname).glob("*.obj")) + collisions.sort(key=lambda x: x.stem) + + os.makedirs(work_dir.resolve(), exist_ok=True) + + for i, filename in enumerate(collisions): + savename = str(work_dir / f"{obj_file.stem}_collision_{i}.obj") + shutil.move(str(filename), savename) + + return True diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_obj.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_obj.py new file mode 100644 index 0000000..12faf38 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mjcf_obj.py @@ -0,0 +1,251 @@ +import os +import time +import numpy as np +import tempfile +import random +import string +import xml.etree.ElementTree as ET + +import robosuite + +from robosuite.models.objects import MujocoXMLObject +from robosuite.utils.mjcf_utils import array_to_string, string_to_array + +import robosuite.utils.transform_utils as T + +import robosuite_model_zoo + + +def postprocess_model_xml(xml_str): + """ + New version of postprocess model xml that only replaces robosuite file paths if necessary (otherwise + there is an error with the "max" operation), and also replaces robosuite-model-zoo file paths + if necessary. + """ + + path = os.path.split(robosuite.__file__)[0] + path_split = path.split("/") + + # replace mesh and texture file paths + tree = ET.fromstring(xml_str) + root = tree + asset = root.find("asset") + meshes = asset.findall("mesh") + textures = asset.findall("texture") + all_elements = meshes + textures + + for elem in all_elements: + old_path = elem.get("file") + if old_path is None: + continue + + old_path_split = old_path.split("/") + # maybe replace all paths to robosuite assets + check_lst = [loc for loc, val in enumerate(old_path_split) if val == "robosuite"] + if len(check_lst) > 0: + ind = max(check_lst) # last occurrence index + new_path_split = path_split + old_path_split[ind + 1 :] + new_path = "/".join(new_path_split) + elem.set("file", new_path) + + # maybe replace all paths to robosuite model zoo assets + check_lst = [ + loc for loc, val in enumerate(old_path_split) if val == "robosuite-model-zoo-dev" + ] + if len(check_lst) > 0: + ind = max(check_lst) # last occurrence index + new_path_split = ( + os.path.split(robosuite_model_zoo.__file__)[0].split("/")[:-1] + + old_path_split[ind + 1 :] + ) + new_path = "/".join(new_path_split) + elem.set("file", new_path) + + return ET.tostring(root, encoding="utf8").decode("utf8") + + +class MJCFObject(MujocoXMLObject): + """ + Blender object with support for changing the scaling + """ + + def __init__( + self, + name, + mjcf_path, + scale=1.0, + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=100, + friction=(0.95, 0.3, 0.1), + margin=None, + rgba=None, + priority=None, + ): + # get scale in x, y, z + if isinstance(scale, float): + scale = [scale, scale, scale] + elif isinstance(scale, tuple) or isinstance(scale, list): + assert len(scale) == 3 + scale = tuple(scale) + else: + raise Exception("got invalid scale: {}".format(scale)) + scale = np.array(scale) + + self.solimp = solimp + self.solref = solref + self.density = density + self.friction = friction + self.margin = margin + + self.priority = priority + + self.rgba = rgba + + # read default xml + xml_path = mjcf_path + folder = os.path.dirname(xml_path) + tree = ET.parse(xml_path) + root = tree.getroot() + + # modify mesh scales + asset = root.find("asset") + meshes = asset.findall("mesh") + for mesh in meshes: + # if a scale already exists, multiply the scales + scale_to_set = scale + existing_scale = mesh.get("scale") + if existing_scale is not None: + scale_to_set = string_to_array(existing_scale) * scale + mesh.set("scale", array_to_string(scale_to_set)) + + # modify sites for collision (assumes we can just scale up the locations - may or may not work) + for n in ["bottom_site", "top_site", "horizontal_radius_site"]: + site = root.find("worldbody/body/site[@name='{}']".format(n)) + pos = string_to_array(site.get("pos")) + pos = scale * pos + site.set("pos", array_to_string(pos)) + + """ + # disable collision geoms + worldbody = root.find("worldbody") + from robosuite.utils.mjcf_utils import find_elements + geoms = find_elements( + root=worldbody, + tags="geom", + attribs={"group": "0"}, + return_first=False + ) + for g in geoms: + # g.set("group", "1") + g.set("mass", "0.0001") + g.set("conaffinity", "2") + g.set("contype", "2") + """ + + # write modified xml (and make sure to postprocess any paths just in case) + xml_str = ET.tostring(root, encoding="utf8").decode("utf8") + xml_str = postprocess_model_xml(xml_str) + time_str = str(time.time()).replace(".", "_") + new_xml_path = os.path.join(folder, "{}_{}.xml".format(time_str, os.getpid())) + f = open(new_xml_path, "w") + f.write(xml_str) + f.close() + # print(f"Write to {new_xml_path}") + + # initialize object with new xml we wrote + super().__init__( + # xml_path_completion("objects/{}.xml".format(obj_name)), + fname=new_xml_path, + name=name, + joints=[dict(type="free", damping="0.0005")], + # joints=None, + obj_type="all", + duplicate_collision_geoms=False, + ) + + # clean up xml - we don't need it anymore + if os.path.exists(new_xml_path): + os.remove(new_xml_path) + + def _get_geoms(self, root, _parent=None): + """ + Helper function to recursively search through element tree starting at @root and returns + a list of (parent, child) tuples where the child is a geom element + + Args: + root (ET.Element): Root of xml element tree to start recursively searching through + _parent (ET.Element): Parent of the root element tree. Should not be used externally; only set + during the recursive call + + Returns: + list: array of (parent, child) tuples where the child element is a geom type + """ + geom_pairs = super(MJCFObject, self)._get_geoms(root=root, _parent=_parent) + + # modify geoms according to the attributes + for i, (parent, element) in enumerate(geom_pairs): + element.set("solref", array_to_string(self.solref)) + element.set("solimp", array_to_string(self.solimp)) + element.set("density", str(self.density)) + element.set("friction", array_to_string(self.friction)) + if self.margin is not None: + element.set("margin", str(self.margin)) + + if (self.rgba is not None) and (element.get("group") == "1"): + element.set("rgba", array_to_string(self.rgba)) + + if self.priority is not None: + # set high priorit + element.set("priority", str(self.priority)) + + return geom_pairs + + @property + def horizontal_radius(self): + horizontal_radius_site = self.worldbody.find( + "./body/site[@name='{}horizontal_radius_site']".format(self.naming_prefix) + ) + site_values = string_to_array(horizontal_radius_site.get("pos")) + return np.linalg.norm(site_values[0:2]) + + def get_bbox_points(self, trans=None, rot=None): + """ + Get the full 8 bounding box points of the object + rot: a rotation matrix + """ + bbox_offsets = [] + + bottom_offset = self.bottom_offset + top_offset = self.top_offset + horizontal_radius_site = self.worldbody.find( + "./body/site[@name='{}horizontal_radius_site']".format(self.naming_prefix) + ) + horiz_radius = string_to_array(horizontal_radius_site.get("pos"))[:2] + + center = np.mean([bottom_offset, top_offset], axis=0) + half_size = [horiz_radius[0], horiz_radius[1], top_offset[2] - center[2]] + + # center = np.array([0, 0, 0]) + # half_size = np.array([0.01, 0.01, 0.01]) + + bbox_offsets = [ + center + half_size * np.array([-1, -1, -1]), # p0 + center + half_size * np.array([1, -1, -1]), # px + center + half_size * np.array([-1, 1, -1]), # py + center + half_size * np.array([-1, -1, 1]), # pz + center + half_size * np.array([1, 1, 1]), + center + half_size * np.array([-1, 1, 1]), + center + half_size * np.array([1, -1, 1]), + center + half_size * np.array([1, 1, -1]), + ] + + if trans is None: + trans = np.array([0, 0, 0]) + if rot is not None: + rot = T.quat2mat(rot) + else: + rot = np.eye(3) + + points = [(np.matmul(rot, p) + trans) for p in bbox_offsets] + return points diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mtl_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mtl_utils.py new file mode 100644 index 0000000..d517604 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/mtl_utils.py @@ -0,0 +1,198 @@ +""" +Much of this code is directly copied from obj2mjcf: +https://github.com/kevinzakka/obj2mjcf/blob/main/obj2mjcf/_cli.py + +Credit: Kevin Zakka +""" + +from dataclasses import dataclass +from typing import Optional, Sequence +from PIL import Image +from pathlib import Path + +import os +import shutil + +# MTL fields relevant to MuJoCo. +_MTL_FIELDS = ( + # Ambient, diffuse and specular colors. + "Ka", + "Kd", + "Ks", + # d or Tr are used for the rgba transparency. + "d", + "Tr", + # Shininess. + "Ns", + # References a texture file. + "map_Kd", +) + +# Character used to denote a comment in an MTL file. +_MTL_COMMENT_CHAR = "#" + + +@dataclass +class Material: + name: str + Ka: Optional[str] = None + Kd: Optional[str] = None + Ks: Optional[str] = None + d: Optional[str] = None + Tr: Optional[str] = None + Ns: Optional[str] = None + map_Kd: Optional[str] = None + + @staticmethod + def from_string(lines: Sequence[str]) -> "Material": + """Construct a Material object from a string.""" + attrs = {"name": lines[0].split(" ")[1].strip()} + for line in lines[1:]: + for attr in _MTL_FIELDS: + if line.startswith(attr): + elems = line.split(" ")[1:] + elems = [elem for elem in elems if elem != ""] + attrs[attr] = " ".join(elems) + break + return Material(**attrs) + + def mjcf_rgba(self) -> str: + Kd = self.Kd or "1.0 1.0 1.0" + if self.d is not None: # alpha + alpha = self.d + elif self.Tr is not None: # 1 - alpha + alpha = str(1.0 - float(self.Tr)) + else: + alpha = "1.0" + # alpha = "1.0" + # TODO(kevin): Figure out how to use Ka for computing rgba. + return f"{Kd} {alpha}" + + def mjcf_shininess(self) -> str: + if self.Ns is not None: + # Normalize Ns value to [0, 1]. Ns values normally range from 0 to 1000. + Ns = float(self.Ns) / 1_000 + else: + Ns = 0.5 + return f"{Ns}" + + def mjcf_specular(self) -> str: + if self.Ks is not None: + # Take the average of the specular RGB values. + Ks = sum(list(map(float, self.Ks.split(" ")))) / 3 + else: + Ks = 0.5 + return f"{Ks}" + + +def get_mtls(filename, work_dir): + filename = Path(filename) + work_dir = Path(work_dir) + + process_mtl = False + with open(filename, "r") as f: + for line in f.readlines(): + if line.startswith("mtllib"): # Deals with commented out lines. + process_mtl = True + name = line.split()[1] + break + + sub_mtls: List[List[str]] = [] + mtls: List[Material] = [] + if process_mtl: + # Make sure the MTL file exists. The MTL filepath is relative to the OBJ's. + mtl_filename = filename.parent / name + if not mtl_filename.exists(): + raise RuntimeError( + f"The MTL file {mtl_filename.resolve()} referenced in the OBJ file " + f"{filename} does not exist" + ) + # logging.info(f"Found MTL file: {mtl_filename}") + + # Parse the MTL file into separate materials. + with open(mtl_filename, "r") as f: + lines = f.readlines() + # Remove comments. + lines = [line for line in lines if not line.startswith(_MTL_COMMENT_CHAR)] + # Remove empty lines. + lines = [line for line in lines if line.strip()] + # Remove trailing whitespace. + lines = [line.strip() for line in lines] + # Split at each new material definition. + for line in lines: + if line.startswith("newmtl"): + sub_mtls.append([]) + sub_mtls[-1].append(line) + for sub_mtl in sub_mtls: + mtls.append(Material.from_string(sub_mtl)) + + # Process each material. + for mtl in mtls: + # logging.info(f"Found material: {mtl.name}") + if mtl.map_Kd is not None: + texture_path = Path(mtl.map_Kd) + texture_name = texture_path.name + src_filename = filename.parent / texture_path + if not src_filename.exists(): + raise RuntimeError( + f"The texture file {src_filename} referenced in the MTL file " + f"{mtl.name} does not exist" + ) + # We want a flat directory structure in work_dir. + dst_filename = work_dir / texture_name + shutil.copy(src_filename, dst_filename) + # MuJoCo only supports PNG textures ¯\_(ツ)_/¯. + if texture_path.suffix.lower() in [".jpg", ".jpeg"]: + image = Image.open(dst_filename) + os.remove(dst_filename) + dst_filename = (work_dir / texture_path.stem).with_suffix(".png") + image.save(dst_filename) + texture_name = dst_filename.name + mtl.map_Kd = texture_name + resize_texture(dst_filename, 1.0) + # logging.info("Done processing MTL file") + + return mtls + + +def resize_texture(filename: Path, resize_percent) -> None: + """Resize a texture to a percentage of its original size.""" + if resize_percent == 1.0: + return + image = Image.open(filename) + new_width = int(image.size[0] * resize_percent) + new_height = int(image.size[1] * resize_percent) + logging.info(f"Resizing {filename} to {new_width}x{new_height}") + image = image.resize((new_width, new_height), Image.LANCZOS) + image.save(filename) + + +def get_image_paths(mtl_path): + image_paths = [] + + # Parse the MTL file into separate materials. + with open(mtl_path, "r") as f: + lines = f.readlines() + # Remove comments. + lines = [line for line in lines if not line.startswith(_MTL_COMMENT_CHAR)] + # Remove empty lines. + lines = [line for line in lines if line.strip()] + # Remove trailing whitespace. + lines = [line.strip() for line in lines] + # Split at each new material definition. + + sub_mtls = [] + + for line in lines: + if line.startswith("newmtl"): + sub_mtls.append([]) + sub_mtls[-1].append(line) + for sub_mtl in sub_mtls: + # print(sub_mtl) + + path = Material.from_string(sub_mtl).map_Kd + + if path is not None: + image_paths.append(path) + + return image_paths diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_play_env.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_play_env.py new file mode 100644 index 0000000..22ed17c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_play_env.py @@ -0,0 +1,250 @@ +import numpy as np + +from robosuite.environments.manipulation.single_arm_env import SingleArmEnv +from robosuite.models.arenas import TableArena +from robosuite.models.tasks import ManipulationTask +from robosuite.utils.placement_samplers import UniformRandomSampler + +from robocasa.utils.model_zoo.mjcf_obj import MJCFObject +from robosuite.models.objects import BoxObject + +import robosuite.utils.transform_utils as T + + +class ObjectPlayEnv(SingleArmEnv): + def __init__( + self, + robots, + obj_mjcf_path, + obj_scale=1.0, + env_configuration="default", + controller_configs=None, + gripper_types="default", + initialization_noise="default", + table_full_size=(0.8, 0.8, 0.05), + table_friction=(1.0, 5e-3, 1e-4), + use_camera_obs=True, + use_object_obs=True, + reward_scale=1.0, + reward_shaping=False, + placement_initializer=None, + has_renderer=False, + has_offscreen_renderer=True, + render_camera="frontview", + render_collision_mesh=False, + render_visual_mesh=True, + render_gpu_device_id=-1, + control_freq=20, + horizon=1000, + ignore_done=False, + hard_reset=True, + camera_names="agentview", + camera_heights=256, + camera_widths=256, + camera_depths=False, + x_range=(-0.03, 0.03), + y_range=(-0.03, 0.03), + rotation=None, + num_objects=1, + ): + # settings for table top + self.table_full_size = table_full_size + self.table_friction = table_friction + self.table_offset = np.array((0, 0, 0.8)) + + # reward configuration + self.reward_scale = reward_scale + self.reward_shaping = reward_shaping + + # whether to use ground-truth object states + self.use_object_obs = use_object_obs + + # object placement initializer + self.placement_initializer = placement_initializer + + self._obj_mjcf_path = obj_mjcf_path + self._obj_scale = obj_scale + self._x_range = x_range + self._y_range = y_range + self._rotation = rotation + self._num_objects = num_objects + + self._cam_configs = dict( + agentview=dict( + pos=[0, -0.5, 1.35], + quat=T.convert_quat( + np.array([0.3826834, 0, 0, 0.9238795]), + to="wxyz", + ), + ), + ) + + super().__init__( + robots=robots, + env_configuration=env_configuration, + controller_configs=controller_configs, + mount_types="default", + gripper_types=gripper_types, + initialization_noise=initialization_noise, + use_camera_obs=use_camera_obs, + has_renderer=has_renderer, + has_offscreen_renderer=has_offscreen_renderer, + render_camera=render_camera, + render_collision_mesh=render_collision_mesh, + render_visual_mesh=render_visual_mesh, + render_gpu_device_id=render_gpu_device_id, + control_freq=control_freq, + horizon=horizon, + ignore_done=ignore_done, + hard_reset=hard_reset, + camera_names=camera_names, + camera_heights=camera_heights, + camera_widths=camera_widths, + camera_depths=camera_depths, + ) + + def reward(self, action=None): + reward = 0.0 + + return reward + + def _load_model(self): + """ + Loads an xml model, puts it in self.model + """ + super()._load_model() + + # Adjust base pose accordingly + # xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0]) + # self.robots[0].robot_model.set_base_xpos(xpos) + ### logic copied from kitchen.py to rotate robot around ### + for robot, rotation, offset in zip(self.robots, (-np.pi / 2,), (0,)): + xpos = robot.robot_model.base_xpos_offset["table"](self.table_full_size[0]) + rot = np.array((0, 0, rotation)) + xpos = T.euler2mat(rot) @ np.array(xpos) + xpos += np.array((0, offset, 0)) + robot.robot_model.set_base_xpos(xpos) + robot.robot_model.set_base_ori(rot) + + # load model for table top workspace + self.mujoco_arena = TableArena( + table_full_size=self.table_full_size, + table_friction=self.table_friction, + table_offset=self.table_offset, + ) + + # Arena always gets set to zero origin + self.mujoco_arena.set_origin([0, 0, 0]) + + self.set_cameras() + + objects = [] + for obj_num in range(self._num_objects): + if self._obj_mjcf_path == "cube": + # debugging case using primitive geom + obj = BoxObject( + name="cube_{}".format(obj_num), + size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015], + size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018]) + rgba=[1, 0, 0, 1], + ) + else: + obj = MJCFObject( + name="MCJFObj_{}".format(obj_num), + mjcf_path=self._obj_mjcf_path, + scale=self._obj_scale, + ) + + objects.append(obj) + + # Create placement initializer + if self.placement_initializer is not None: + self.placement_initializer.reset() + self.placement_initializer.add_objects(objects) + else: + self.placement_initializer = UniformRandomSampler( + name="ObjectSampler", + mujoco_objects=objects, + x_range=self._x_range, + y_range=self._y_range, + rotation=self._rotation, + ensure_object_boundary_in_range=False, + ensure_valid_placement=True, + reference_pos=self.table_offset, + z_offset=0.01, + # # for debugging + # x_range=(0, 0), + # y_range=(0, 0), + # rotation=(0, 0), + ) + + # task includes arena, robot, and objects of interest + self.model = ManipulationTask( + mujoco_arena=self.mujoco_arena, + mujoco_robots=[robot.robot_model for robot in self.robots], + mujoco_objects=objects, + ) + + # def _setup_references(self): + # """ + # Sets up references to important components. A reference is typically an + # index or a list of indices that point to the corresponding elements + # in a flatten array, which is how MuJoCo stores physical simulation data. + # """ + # super()._setup_references() + + # # Additional object references from this env + # self.obj_body_id = self.sim.model.body_name2id(self.mjcf_obj.root_body) + + def set_cameras(self): + """ + copied from environments/manipulation/kitchen.py + """ + for cam_name, cam_cfg in self._cam_configs.items(): + if cam_cfg.get("parent_body", None) is not None: + continue + + self.mujoco_arena.set_camera( + camera_name=cam_name, pos=cam_cfg["pos"], quat=cam_cfg["quat"] + ) + + def _reset_internal(self): + """ + Resets simulation internal configurations. + """ + super()._reset_internal() + + # Reset all object positions using initializer sampler if we're not directly loading from an xml + if not self.deterministic_reset: + + # Sample from the placement initializer for all objects + object_placements = self.placement_initializer.sample() + + # Loop through all objects and reset their positions + for obj_pos, obj_quat, obj in object_placements.values(): + # self.sim.model.body_pos[self.sim.model.body_name2id(obj.root_body)] = obj_pos + # self.sim.model.body_quat[self.sim.model.body_name2id(obj.root_body)] = obj_quat + + self.sim.data.set_joint_qpos( + obj.joints[0], + np.concatenate([np.array(obj_pos), np.array(obj_quat)]), + ) + + # def visualize(self, vis_settings): + # """ + # In addition to super call, visualize gripper site proportional to the distance to the cube. + # + # Args: + # vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific + # component should be visualized. Should have "grippers" keyword as well as any other relevant + # options specified. + # """ + # # Run superclass method first + # super().visualize(vis_settings=vis_settings) + # + # # Color the gripper visualization site according to its distance to the cube + # if vis_settings["grippers"]: + # self._visualize_gripper_to_target(gripper=self.robots[0].gripper, target=self.cube) + + def _check_success(self): + return False diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_template.xml b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_template.xml new file mode 100644 index 0000000..986e66f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/object_template.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/parser_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/parser_utils.py new file mode 100644 index 0000000..83ca6b4 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/model_zoo/parser_utils.py @@ -0,0 +1,61 @@ +import argparse + + +def get_base_parser(): + parser = argparse.ArgumentParser() + parser.add_argument( + "--base_asset_path", + type=str, + help="(optional) base asset path where model files are stored. " + "Defaults to robosuite_model_zoo/assets_private folder.", + ) + parser.add_argument( + "--model_folder", + type=str, + help="(optional) folder to store model (relative to base asset path). " "Defaults to None.", + ) + parser.add_argument( + "--model_name", + type=str, + help="(optional) name of saved mjcf model. " "Defaults to name from --model_path.", + ) + parser.add_argument( + "--scale", + type=float, + default=1.0, + help="1d scaling of input model", + ) + parser.add_argument( + "--show_sites", + action="store_true", + help="show model bounds sites", + ) + parser.add_argument( + "--verbose", + action="store_true", + help="print detailed information for debugging", + ) + + ### V-HACD settings ### + parser.add_argument( + "--vhacd_max_output_convex_hulls", + type=float, + default=32, + ) + parser.add_argument( + "--vhacd_voxel_resolution", + type=float, + default=100000, + ) + parser.add_argument( + "--vhacd_volume_error_percent", + type=float, + default=1.0, + ) + parser.add_argument( + "--vhacd_max_hull_vert_count", + type=float, + default=64, + ) + + return parser diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/object_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/object_utils.py new file mode 100644 index 0000000..0ef5bef --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/object_utils.py @@ -0,0 +1,621 @@ +import numpy as np +import mujoco as mj +import robosuite.utils.transform_utils as T +from robosuite.utils.mjcf_utils import array_to_string + +from robocasa.models.objects.objects import MJCFObject + + +def obj_inside_of(env, obj_name, fixture_id, partial_check=False): + """ + whether an object (another mujoco object) is inside of fixture. applies for most fixtures + """ + from robocasa.models.fixtures import Fixture + + obj = env.objects[obj_name] + fixture = env.get_fixture(fixture_id) + assert isinstance(obj, MJCFObject) + assert isinstance(fixture, Fixture) + + # step 1: calculate fxiture points + fixtr_p0, fixtr_px, fixtr_py, fixtr_pz = fixture.get_int_sites(relative=False) + u = fixtr_px - fixtr_p0 + v = fixtr_py - fixtr_p0 + w = fixtr_pz - fixtr_p0 + + # get the position and quaternion of object + obj_pos = np.array(env.sim.data.body_xpos[env.obj_body_id[obj.name]]) + obj_quat = T.convert_quat(env.sim.data.body_xquat[env.obj_body_id[obj.name]], to="xyzw") + + if partial_check: + obj_points_to_check = [obj_pos] + th = 0.0 + else: + # calculate 8 boundary points of object + obj_points_to_check = obj.get_bbox_points(trans=obj_pos, rot=obj_quat) + # threshold to mitigate false negatives: even if the bounding box point is out of bounds, + th = 0.05 + + inside_of = True + for obj_p in obj_points_to_check: + check1 = np.dot(u, fixtr_p0) - th <= np.dot(u, obj_p) <= np.dot(u, fixtr_px) + th + check2 = np.dot(v, fixtr_p0) - th <= np.dot(v, obj_p) <= np.dot(v, fixtr_py) + th + check3 = np.dot(w, fixtr_p0) - th <= np.dot(w, obj_p) <= np.dot(w, fixtr_pz) + th + + if not (check1 and check2 and check3): + inside_of = False + break + + return inside_of + + +# used for cabinets, cabinet panels, counters, etc. +def set_geom_dimensions(sizes, positions, geoms, rotated=False): + """ + set the dimensions of geoms in a fixture + + Args: + sizes (dict): dictionary of sizes for each side + + positions (dict): dictionary of positions for each side + + geoms (dict): dictionary of geoms for each side + + rotated (bool): whether the fixture is rotated. Fixture may be rotated to make texture appear uniform + due to mujoco texture conventions + """ + if rotated: + # rotation trick to make texture appear uniform + # see .xml file + for side in sizes.keys(): + # not the best detection method, TODO: check euler + if "door" in side or "trim" in side: + sizes[side] = [sizes[side][i] for i in [0, 2, 1]] + + # set sizes and positions of all geoms + for side in positions.keys(): + for geom in geoms[side]: + if geom is None: + raise ValueError("Did not find geom:", side) + geom.set("pos", array_to_string(positions[side])) + geom.set("size", array_to_string(sizes[side])) + + +def get_rel_transform(fixture_A, fixture_B): + """ + Gets fixture_B's position and rotation relative to fixture_A's frame + """ + A_trans = np.array(fixture_A.pos) + B_trans = np.array(fixture_B.pos) + + A_rot = np.array([0, 0, fixture_A.rot]) + B_rot = np.array([0, 0, fixture_B.rot]) + + A_mat = T.euler2mat(A_rot) + B_mat = T.euler2mat(B_rot) + + T_WA = np.vstack((np.hstack((A_mat, A_trans[:, None])), [0, 0, 0, 1])) + T_WB = np.vstack((np.hstack((B_mat, B_trans[:, None])), [0, 0, 0, 1])) + + T_AB = np.matmul(np.linalg.inv(T_WA), T_WB) + + return T_AB[:3, 3], T_AB[:3, :3] + + +def compute_rel_transform(A_pos, A_mat, B_pos, B_mat): + """ + Gets B's position and rotation relative to A's frame + """ + T_WA = np.vstack((np.hstack((A_mat, A_pos[:, None])), [0, 0, 0, 1])) + T_WB = np.vstack((np.hstack((B_mat, B_pos[:, None])), [0, 0, 0, 1])) + + T_AB = np.matmul(np.linalg.inv(T_WA), T_WB) + + return T_AB[:3, 3], T_AB[:3, :3] + + +def get_fixture_to_point_rel_offset(fixture, point): + """ + get offset relative to fixture's frame, given a global point + """ + global_offset = point - fixture.pos + T_WF = T.euler2mat([0, 0, fixture.rot]) + rel_offset = np.matmul(np.linalg.inv(T_WF), global_offset) + return rel_offset + + +def get_pos_after_rel_offset(fixture, offset): + """ + get global position of a fixture, after applying offset relative to center of fixture + """ + fixture_rot = np.array([0, 0, fixture.rot]) + fixture_mat = T.euler2mat(fixture_rot) + + return fixture.pos + np.dot(fixture_mat, offset) + + +def project_point_to_line(P, A, B): + """ + logic copied from here: https://stackoverflow.com/a/61342198 + """ + AP = P - A + AB = B - A + result = A + np.dot(AP, AB) / np.dot(AB, AB) * AB + + return result + + +def point_in_fixture(point, fixture, only_2d=False): + """ + check if point is inside of the exterior bounding boxes of the fixture + + Args: + point (np.array): point to check + + fixture (Fixture): fixture object + + only_2d (bool): whether to check only in 2D + """ + p0, px, py, pz = fixture.get_ext_sites(relative=False) + th = 0.00 + u = px - p0 + v = py - p0 + w = pz - p0 + check1 = np.dot(u, p0) - th <= np.dot(u, point) <= np.dot(u, px) + th + check2 = np.dot(v, p0) - th <= np.dot(v, point) <= np.dot(v, py) + th + check3 = np.dot(w, p0) - th <= np.dot(w, point) <= np.dot(w, pz) + th + + if only_2d: + return check1 and check2 + else: + return check1 and check2 and check3 + + +def obj_in_region( + obj, + obj_pos, + obj_quat, + p0, + px, + py, + pz=None, +): + """ + check if object is in the region defined by the points. + Uses either the objects bounding box or the object's horizontal radius + """ + from robocasa.models.fixtures import Fixture + + if isinstance(obj, MJCFObject) or isinstance(obj, Fixture): + obj_points = obj.get_bbox_points(trans=obj_pos, rot=obj_quat) + else: + radius = obj.horizontal_radius + obj_points = obj_pos + np.array( + [ + [radius, 0, 0], + [-radius, 0, 0], + [0, radius, 0], + [0, -radius, 0], + ] + ) + + u = px - p0 + v = py - p0 + w = pz - p0 if pz is not None else None + + for point in obj_points: + check1 = np.dot(u, p0) <= np.dot(u, point) <= np.dot(u, px) + check2 = np.dot(v, p0) <= np.dot(v, point) <= np.dot(v, py) + + if not check1 or not check2: + return False + + if w is not None: + check3 = np.dot(w, p0) <= np.dot(w, point) <= np.dot(w, pz) + if not check3: + return False + + return True + + +def obj_in_region_with_keypoints( + obj, obj_pos, obj_quat, region_points, min_num_points=3, region_type="box" +): + """ + check if object is in the region defined by the keypoints. Only min_num_points points need to be in the region to return True. + Keypoints include the points of the object's bounding box and the center of the object. + region can be a box or a cylinder or a sphere + TODO: support plane region + """ + from robocasa.models.fixtures import Fixture + + if isinstance(obj, MJCFObject) or isinstance(obj, Fixture): + obj_points = obj.get_bbox_points(trans=obj_pos, rot=obj_quat) + else: + radius = obj.horizontal_radius + obj_points = obj_pos + np.array( + [ + [radius, 0, 0], + [-radius, 0, 0], + [0, radius, 0], + [0, -radius, 0], + ] + ) + + # center points of object for each plane + obj_u = obj_points[1] - obj_points[0] + obj_v = obj_points[2] - obj_points[0] + obj_w = obj_points[3] - obj_points[0] + centers = [] + + # Front and back XY planes + center_xy_front = obj_points[0] + 0.5 * obj_u + 0.5 * obj_v + center_xy_back = center_xy_front + obj_w + centers.extend([center_xy_front, center_xy_back]) + + if obj_w is not None: + # Left and right XZ planes + center_xz_left = obj_points[0] + 0.5 * obj_u + 0.5 * obj_w + center_xz_right = center_xz_left + obj_v + centers.extend([center_xz_left, center_xz_right]) + + # Top and bottom YZ planes + center_yz_top = obj_points[0] + 0.5 * obj_v + 0.5 * obj_w + center_yz_bottom = center_yz_top + obj_u + centers.extend([center_yz_top, center_yz_bottom]) + + center_point = np.mean(obj_points, axis=0) + centers.append(center_point) + obj_points = np.concatenate([obj_points, centers]) + + # Erode the bounding box by pulling points 10% closer to the center + # Avoids false negatives when object is near the edge of the region + obj_points = obj_points * 0.8 + center_point * 0.2 + + if region_type == "box": + if len(region_points) == 4: + p0, px, py, pz = region_points + elif len(region_points) == 3: + p0, px, py = region_points + pz = None + else: + raise ValueError(f"Invalid number of region points: {len(region_points)}") + return obj_in_box_region(obj_points, p0, px, py, pz, min_num_points) + elif region_type == "cylinder": + p0, axis_vector, radius = region_points + return obj_in_cylinder_region(obj_points, p0, axis_vector, radius, min_num_points) + elif region_type == "sphere": + p0, radius = region_points + return obj_in_sphere_region(obj_points, p0, radius, min_num_points) + + +def fixture_pairwise_dist(f1, f2): + """ + Gets the distance between two fixtures by finding the minimum distance between their exterior bounding box points + """ + f1_points = f1.get_ext_sites(all_points=True, relative=False) + f2_points = f2.get_ext_sites(all_points=True, relative=False) + + all_dists = [np.linalg.norm(p1 - p2) for p1 in f1_points for p2 in f2_points] + return np.min(all_dists) + + +def objs_intersect( + obj, + obj_pos, + obj_quat, + other_obj, + other_obj_pos, + other_obj_quat, +): + """ + check if two objects intersect + """ + from robocasa.models.fixtures import Fixture + + bbox_check = (isinstance(obj, MJCFObject) or isinstance(obj, Fixture)) and ( + isinstance(other_obj, MJCFObject) or isinstance(other_obj, Fixture) + ) + if bbox_check: + obj_points = obj.get_bbox_points(trans=obj_pos, rot=obj_quat) + other_obj_points = other_obj.get_bbox_points(trans=other_obj_pos, rot=other_obj_quat) + + face_normals = [ + obj_points[1] - obj_points[0], + obj_points[2] - obj_points[0], + obj_points[3] - obj_points[0], + other_obj_points[1] - other_obj_points[0], + other_obj_points[2] - other_obj_points[0], + other_obj_points[3] - other_obj_points[0], + ] + + intersect = True + + # noramlize length of normals + for normal in face_normals: + normal = np.array(normal) / np.linalg.norm(normal) + + obj_projs = [np.dot(p, normal) for p in obj_points] + other_obj_projs = [np.dot(p, normal) for p in other_obj_points] + + # see if gap detected + if np.min(other_obj_projs) > np.max(obj_projs) or np.min(obj_projs) > np.max( + other_obj_projs + ): + intersect = False + break + else: + """ + old code from placement_samplers.py + """ + obj_x, obj_y, obj_z = obj_pos + other_obj_x, other_obj_y, other_obj_z = other_obj_pos + xy_collision = ( + np.linalg.norm((obj_x - other_obj_x, obj_y - other_obj_y)) + <= other_obj.horizontal_radius + obj.horizontal_radius + ) + if obj_z > other_obj_z: + z_collision = obj_z - other_obj_z <= other_obj.top_offset[-1] - obj.bottom_offset[-1] + else: + z_collision = other_obj_z - obj_z <= obj.top_offset[-1] - other_obj.bottom_offset[-1] + + if xy_collision and z_collision: + intersect = True + else: + intersect = False + + return intersect + + +def normalize_joint_value(raw, joint_min, joint_max): + """ + normalize raw value to be between 0 and 1 + """ + return (raw - joint_min) / (joint_max - joint_min) + + +def get_highest_spawn_region(env, receptacle): + """ + Get the highest spawn region of a receptacle. + """ + return max( + receptacle.spawns, + key=lambda x: env.sim.data.geom_xpos[env.sim.model.geom_name2id(x.get("name"))][2], + ) + + +def calculate_spawn_region(env, spawn): + """ + Calculates the corner points of the spawn region. + + Parameters: + env: The simulation/environment object. + spawn: The spawn geom. + + Returns: + For box: (p0, px, py, pz) + For cylinder: (p0, axis_vector, radius) + For sphere: (center, radius) + """ + # Get spawn parameters from the simulation. + spawn_name = spawn.get("name") + spawn_id = env.sim.model.geom_name2id(spawn_name) + + spawn_pos = env.sim.data.get_geom_xpos(spawn_name) + spawn_xmat = env.sim.data.get_geom_xmat(spawn_name) + spawn_size = env.sim.model.geom_size[spawn_id] + spawn_type = env.sim.model.geom_type[spawn_id] + assert spawn_type in [ + mj.mjtGeom.mjGEOM_BOX, + mj.mjtGeom.mjGEOM_CYLINDER, + mj.mjtGeom.mjGEOM_SPHERE, + ], "Spawn region only supports box, cylinder and sphere geometries." + + if spawn_type == mj.mjtGeom.mjGEOM_BOX: + # Box geometry - return corners as before + p0 = ( + spawn_pos + - spawn_size[0] * spawn_xmat[:, 0] + - spawn_size[1] * spawn_xmat[:, 1] + - spawn_size[2] * spawn_xmat[:, 2] + ) + px = p0 + 2 * spawn_size[0] * spawn_xmat[:, 0] + py = p0 + 2 * spawn_size[1] * spawn_xmat[:, 1] + pz = p0 + 2 * spawn_size[2] * spawn_xmat[:, 2] + return p0, px, py, pz + elif spawn_type == mj.mjtGeom.mjGEOM_CYLINDER: + # Cylinder geometry - return (base_center, axis_vector, radius) + height = spawn_size[1] * 2 + radius = spawn_size[0] + axis_vector = height * spawn_xmat[:, 2] + return spawn_pos, axis_vector, radius + elif spawn_type == mj.mjtGeom.mjGEOM_SPHERE: + # Sphere geometry - return (center, radius) + radius = spawn_size[0] + return spawn_pos, radius + else: + raise ValueError(f"Invalid spawn type: {spawn_type}") + + +def check_obj_in_receptacle( + env, obj_name, receptacle_name, th=None, spawn_check=True, spawn_regions=None +): + """ + check if object is in receptacle object based on spawn regions or threshold + """ + obj = env.objects[obj_name] + recep = env.objects[receptacle_name] + obj_pos = np.array(env.sim.data.body_xpos[env.obj_body_id[obj_name]]) + obj_quat = T.convert_quat( + np.array(env.sim.data.body_xquat[env.obj_body_id[obj_name]]), to="xyzw" + ) + recep_pos = np.array(env.sim.data.body_xpos[env.obj_body_id[receptacle_name]]) + + if len(recep.spawns) > 0 and spawn_check: + if spawn_regions is None: + spawn_regions = recep.spawns + + for spawn in spawn_regions: + region_points = calculate_spawn_region(env, spawn) + + if obj_in_region_with_keypoints( + obj, + obj_pos, + obj_quat, + region_points, + min_num_points=2, + region_type=spawn.get("type"), + ): + return True + return False + else: + if th is None: + th = recep.horizontal_radius * 0.7 + obj_in_recep = ( + env.check_contact(obj, recep) and np.linalg.norm(obj_pos[:2] - recep_pos[:2]) < th + ) + return obj_in_recep + + +def check_obj_fixture_contact(env, obj_name, fixture_name): + """ + check if object is in contact with fixture + """ + obj = env.objects[obj_name] + fixture = env.get_fixture(fixture_name) + return env.check_contact(obj, fixture) + + +def gripper_obj_far(env, obj_name="obj", th=0.25): + """ + check if gripper is far from object based on distance defined by threshold + """ + return gripper_obj_far_by_side(env=env, obj_name=obj_name, gripper_side="right", th=th)[1] + + +def any_gripper_obj_far(env, obj_name="obj", th=0.25): + """ + check if all grippers are far from object based on distance defined by threshold + """ + left_is_far = gripper_obj_far_by_side(env=env, obj_name=obj_name, gripper_side="left", th=th)[1] + right_is_far = gripper_obj_far_by_side(env=env, obj_name=obj_name, gripper_side="right", th=th)[ + 1 + ] + return left_is_far and right_is_far + + +def gripper_obj_far_by_side(env, obj_name: str, gripper_side: str, th: float = 0.25): + """ + check if gripper is far from object based on distance defined by threshold + """ + obj_pos = env.sim.data.body_xpos[env.obj_body_id[obj_name]] + gripper_site_pos = env.sim.data.site_xpos[env.robots[0].eef_site_id[gripper_side]] + dist = np.linalg.norm(gripper_site_pos - obj_pos) + return dist, dist > th + + +def obj_cos(env, obj_name="obj", ref=(0, 0, 1)): + def cos(u, v): + return np.dot(u, v) / max(np.linalg.norm(u) * np.linalg.norm(v), 1e-10) + + obj_id = env.obj_body_id[obj_name] + obj_quat = T.convert_quat(np.array(env.sim.data.body_xquat[obj_id]), to="xyzw") + obj_mat = T.quat2mat(obj_quat) + + return cos(obj_mat[:, 2], np.array(ref)) + + +def check_obj_upright(env, obj_name, threshold=0.8, symmetric: bool = False): + """ + Check if an object is upright based on its z-axis orientation + """ + obj_id = env.obj_body_id[obj_name] + obj_quat = T.convert_quat(np.array(env.sim.data.body_xquat[obj_id]), to="xyzw") + obj_mat = T.quat2mat(obj_quat) + z_alignment = obj_mat[:, 2][2] + z_alignment = abs(z_alignment) if symmetric else z_alignment + return z_alignment > threshold + + +def obj_in_box_region(obj_points, p0, px, py, pz, min_num_points): + """ + Check if enough object points are inside a box region defined by corner points. + + Args: + obj_points (np.array): Points to check + p0 (np.array): Origin corner point + px (np.array): X-axis corner point + py (np.array): Y-axis corner point + pz (np.array): Z-axis corner point (optional) + min_num_points (int): Minimum number of points that need to be in region + + Returns: + bool: True if enough points are inside the region + """ + u = px - p0 + v = py - p0 + w = pz - p0 if pz is not None else None + + num_points_in_region = 0 + for point in obj_points: + check1 = np.dot(u, p0) <= np.dot(u, point) <= np.dot(u, px) + check2 = np.dot(v, p0) <= np.dot(v, point) <= np.dot(v, py) + + if w is not None: + check3 = np.dot(w, p0) <= np.dot(w, point) <= np.dot(w, pz) + if check1 and check2 and check3: + num_points_in_region += 1 + else: + if check1 and check2: + num_points_in_region += 1 + + return num_points_in_region >= min_num_points + + +def obj_in_cylinder_region(obj_points, p0, axis_vector, radius, min_num_points): + """ + Check if enough object points are inside a cylinder region defined by base point, height vector and radius + + Args: + obj_points (np.array): Points to check + p0 (np.array): Base center point of cylinder + axis_vector (np.array): Vector from base center to top center, defines cylinder axis and height + radius (float): Radius of cylinder + min_num_points (int): Minimum number of points that need to be in region + """ + num_points_in_region = 0 + # Normalize height vector to get axis direction + axis = axis_vector / np.linalg.norm(axis_vector) + height_magnitude = np.linalg.norm(axis_vector) + + for point in obj_points: + # Project point onto cylinder axis + point_rel = point - p0 + height_proj = np.dot(point_rel, axis) + + # Check height bounds + if 0 <= height_proj <= height_magnitude: + # Check radius by removing height component + radius_vec = point_rel - height_proj * axis + if np.linalg.norm(radius_vec) <= radius: + num_points_in_region += 1 + + return num_points_in_region >= min_num_points + + +def obj_in_sphere_region(obj_points, p0, radius, min_num_points): + """ + Check if enough object points are inside a sphere region defined by center and radius + + Args: + obj_points (np.array): Points to check + p0 (np.array): Center point of sphere + radius (float): Radius of sphere + min_num_points (int): Minimum number of points that need to be in region + """ + num_points_in_region = 0 + for point in obj_points: + if np.linalg.norm(point - p0) <= radius: + num_points_in_region += 1 + + return num_points_in_region >= min_num_points diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/placement_samplers.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/placement_samplers.py new file mode 100644 index 0000000..df9651c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/placement_samplers.py @@ -0,0 +1,711 @@ +import collections +from copy import copy + +import numpy as np + +from robocasa.models.objects.objects import MJCFObject +from robosuite.models.objects import MujocoObject +from robosuite.utils import RandomizationError +from robosuite.utils.transform_utils import ( + convert_quat, + euler2mat, + mat2quat, + quat_multiply, + rotate_2d_point, +) + +from robocasa.utils.object_utils import ( + obj_in_region, + objs_intersect, + obj_in_region_with_keypoints, +) + + +class ObjectPositionSampler: + """ + Base class of object placement sampler. + + Args: + name (str): Name of this sampler. + + mujoco_objects (None or MujocoObject or list of MujocoObject): single model or list of MJCF object models + + ensure_object_boundary_in_range (bool): If True, will ensure that the object is enclosed within a given boundary + (should be implemented by subclass) + + ensure_valid_placement (bool): If True, will check for correct (valid) object placements + + reference_pos (3-array): global (x,y,z) position relative to which sampling will occur + + z_offset (float): Add a small z-offset to placements. This is useful for fixed objects + that do not move (i.e. no free joint) to place them above the table. + """ + + def __init__( + self, + name, + mujoco_objects=None, + ensure_object_boundary_in_range=True, + ensure_valid_placement=True, + reference_pos=(0, 0, 0), + reference_rot=0, + z_offset=0.0, + rng=None, + ): + if rng is None: + rng = np.random.default_rng() + self.rng = rng + + # Setup attributes + self.name = name + if mujoco_objects is None: + self.mujoco_objects = [] + else: + # Shallow copy the list so we don't modify the inputted list but still keep the object references + self.mujoco_objects = ( + [mujoco_objects] + if isinstance(mujoco_objects, MujocoObject) + else copy(mujoco_objects) + ) + self.ensure_object_boundary_in_range = ensure_object_boundary_in_range + self.ensure_valid_placement = ensure_valid_placement + self.reference_pos = reference_pos + self.reference_rot = reference_rot + self.z_offset = z_offset + + def add_objects(self, mujoco_objects): + """ + Add additional objects to this sampler. Checks to make sure there's no identical objects already stored. + + Args: + mujoco_objects (MujocoObject or list of MujocoObject): single model or list of MJCF object models + """ + mujoco_objects = ( + [mujoco_objects] if isinstance(mujoco_objects, MujocoObject) else mujoco_objects + ) + for obj in mujoco_objects: + assert obj not in self.mujoco_objects, "Object '{}' already in sampler!".format( + obj.name + ) + self.mujoco_objects.append(obj) + + def reset(self): + """ + Resets this sampler. Removes all mujoco objects from this sampler. + """ + self.mujoco_objects = [] + + def sample(self, fixtures=None, reference=None, on_top=True): + """ + Uniformly sample on a surface (not necessarily table surface). + + Args: + fixtures (dict): dictionary of current object placements in the scene as well as any other relevant + obstacles that should not be in contact with newly sampled objects. Used to make sure newly + generated placements are valid. Should be object names mapped to (pos, quat, MujocoObject) + + reference (str or 3-tuple or None): if provided, sample relative placement. Can either be a string, which + corresponds to an existing object found in @fixtures, or a direct (x,y,z) value. If None, will sample + relative to this sampler's `'reference_pos'` value. + + on_top (bool): if True, sample placement on top of the reference object. + + Return: + dict: dictionary of all object placements, mapping object_names to (pos, quat, obj), including the + placements specified in @fixtures. Note quat is in (w,x,y,z) form + """ + raise NotImplementedError + + @property + def sides_combinations(self): + return { + "left": ["front_left", "back_left"], + "right": ["front_right", "back_right"], + "front": ["front_left", "front_right"], + "back": ["back_left", "back_right"], + "all": ["front_left", "front_right", "back_left", "back_right"], + } + + @property + def valid_sides(self): + return set( + [ + "left", + "right", + "front", + "back", + "all", + "front_left", + "front_right", + "back_left", + "back_right", + ] + ) + + +class UniformRandomSampler(ObjectPositionSampler): + """ + Places all objects within the table uniformly random. + + Args: + name (str): Name of this sampler. + + mujoco_objects (None or MujocoObject or list of MujocoObject): single model or list of MJCF object models + + x_range (2-array of float): Specify the (min, max) relative x_range used to uniformly place objects + + y_range (2-array of float): Specify the (min, max) relative y_range used to uniformly place objects + + rotation (None or float or Iterable): + :`None`: Add uniform random random rotation + :`Iterable (a,b)`: Uniformly randomize rotation angle between a and b (in radians) + :`value`: Add fixed angle rotation + + rotation_axis (str): Can be 'x', 'y', or 'z'. Axis about which to apply the requested rotation + + ensure_object_boundary_in_range (bool): + :`True`: The center of object is at position: + [uniform(min x_range + radius, max x_range - radius)], [uniform(min x_range + radius, max x_range - radius)] + :`False`: + [uniform(min x_range, max x_range)], [uniform(min x_range, max x_range)] + + ensure_valid_placement (bool): If True, will check for correct (valid) object placements + + reference_pos (3-array): global (x,y,z) position relative to which sampling will occur + + z_offset (float): Add a small z-offset to placements. This is useful for fixed objects + that do not move (i.e. no free joint) to place them above the table. + + num_attempts (int): Number of attempts to sample an object before giving up. + """ + + def __init__( + self, + name, + mujoco_objects=None, + x_range=(0, 0), + y_range=(0, 0), + rotation=None, + rotation_axis="z", + ensure_object_boundary_in_range=True, + ensure_valid_placement=True, + ensure_object_in_ref_region=False, + ensure_object_out_of_ref_region=False, + reference_pos=(0, 0, 0), + reference_rot=0, + z_offset=0.0, + rng=None, + side="all", + num_attempts=5000, + ): + """Uniformly sample the position of the object. + + Args: + ensure_object_in_ref_region (bool, optional): + If True, we need to ensure the object be placed within the reference region. Not need to be fully in the region, but at least the center of the object. + ensure_object_out_of_ref_region (bool, optional): + If True, we need to ensure the object be placed outside the reference region. Need to be fully outside the region. + """ + self.x_range = x_range + self.y_range = y_range + self.rotation = rotation + self.rotation_axis = rotation_axis + self.num_attempts = num_attempts + if side not in self.valid_sides: + raise ValueError("Invalid value for side, must be one of:", self.valid_sides) + + super().__init__( + name=name, + mujoco_objects=mujoco_objects, + ensure_object_boundary_in_range=ensure_object_boundary_in_range, + ensure_valid_placement=ensure_valid_placement, + reference_pos=reference_pos, + reference_rot=reference_rot, + z_offset=z_offset, + rng=rng, + ) + self.ensure_object_in_ref_region = ensure_object_in_ref_region + self.ensure_object_out_of_ref_region = ensure_object_out_of_ref_region + + def _sample_x(self): + """ + Samples the x location for a given object + + Returns: + float: sampled x position + """ + minimum, maximum = self.x_range + return self.rng.uniform(high=maximum, low=minimum) + + def _sample_y(self): + """ + Samples the y location for a given object + + Returns: + float: sampled y position + """ + minimum, maximum = self.y_range + return self.rng.uniform(high=maximum, low=minimum) + + def _sample_quat(self): + """ + Samples the orientation for a given object + + Returns: + np.array: sampled object quaternion in (w,x,y,z) form + + Raises: + ValueError: [Invalid rotation axis] + """ + if self.rotation is None: + rot_angle = self.rng.uniform(high=2 * np.pi, low=0) + elif isinstance(self.rotation, collections.abc.Iterable): + if isinstance(self.rotation[0], collections.abc.Iterable): + rotation = self.rng.choice(self.rotation) + else: + rotation = self.rotation + rot_angle = self.rng.uniform(high=max(rotation), low=min(rotation)) + else: + rot_angle = self.rotation + + # Return angle based on axis requested + if self.rotation_axis == "x": + return np.array([np.cos(rot_angle / 2), np.sin(rot_angle / 2), 0, 0]) + elif self.rotation_axis == "y": + return np.array([np.cos(rot_angle / 2), 0, np.sin(rot_angle / 2), 0]) + elif self.rotation_axis == "z": + return np.array([np.cos(rot_angle / 2), 0, 0, np.sin(rot_angle / 2)]) + else: + # Invalid axis specified, raise error + raise ValueError( + "Invalid rotation axis specified. Must be 'x', 'y', or 'z'. Got: {}".format( + self.rotation_axis + ) + ) + + def sample(self, placed_objects=None, reference=None, neg_reference=None, on_top=True): + """ + Uniformly sample relative to this sampler's reference_pos or @reference (if specified). + + Args: + placed_objects (dict): dictionary of current object placements in the scene as well as any other relevant + obstacles that should not be in contact with newly sampled objects. Used to make sure newly + generated placements are valid. Should be object names mapped to (pos, quat, MujocoObject) + + reference (str, tuple, or None, optional): Defines the reference for relative placement. It can be an object + name found in `fixtures`, a tuple specifying an object and its site ID, or a direct (x, y, z) coordinate. + If not provided, placement is sampled relative to this sampler's `reference_pos`. + + on_top (bool): if True, sample placement on top of the reference object. This corresponds to a sampled + z-offset of the current sampled object's bottom_offset + the reference object's top_offset + (if specified) + + Return: + dict: dictionary of all object placements, mapping object_names to (pos, quat, obj), including the + placements specified in @fixtures. Note quat is in (w,x,y,z) form + + Raises: + RandomizationError: [Cannot place all objects] + AssertionError: [Reference object name does not exist, invalid inputs] + """ + # Standardize inputs + placed_objects = {} if placed_objects is None else copy(placed_objects) + spawn_ref_obj = None + + if reference is None: + base_offset = self.reference_pos + elif type(reference) is str: + assert ( + reference in placed_objects + ), "Invalid reference received. Current options are: {}, requested: {}".format( + placed_objects.keys(), reference + ) + ref_pos, _, ref_obj = placed_objects[reference] + base_offset = np.array(ref_pos) + if on_top: + base_offset += np.array((0, 0, ref_obj.top_offset[-1])) + # Handle shelf object references + # If the reference is provided as a tuple (object_key: str, spawn_id: int), + # then we treat it as a reference to a specific shelf level + elif type(reference) is tuple and len(reference) == 2: + (reference, spawn_id) = reference + assert ( + reference in placed_objects + ), "Invalid reference received. Current options are: {}, requested: {}".format( + placed_objects.keys(), reference + ) + ref_pos, _, ref_obj = placed_objects[reference] + assert isinstance( + ref_obj, MJCFObject + ), "Invalid reference received. Should be of type MJCFObject" + if spawn_id == -1: + spawn_id, site = ref_obj.get_random_spawn(self.rng, exclude_disabled=True) + else: + site = ref_obj.spawns[spawn_id] + ref_obj.set_spawn_active(spawn_id, False) + + base_offset = np.array(ref_pos) + base_offset += ref_obj.get_spawn_bottom_offset(site) + spawn_ref_obj = ref_obj + else: + base_offset = np.array(reference) + assert ( + base_offset.shape[0] == 3 + ), "Invalid reference received. Should be (x,y,z) 3-tuple, but got: {}".format( + base_offset + ) + + # Sample pos and quat for all objects assigned to this sampler + for obj in self.mujoco_objects: + # First make sure the currently sampled object hasn't already been sampled + assert obj.name not in placed_objects, "Object '{}' has already been sampled!".format( + obj.name + ) + + success = False + + # get reference rotation + ref_quat = convert_quat(mat2quat(euler2mat([0, 0, self.reference_rot])), to="wxyz") + + ### get boundary points ### + region_points = np.array( + [ + [self.x_range[0], self.y_range[0], 0], + [self.x_range[1], self.y_range[0], 0], + [self.x_range[0], self.y_range[1], 0], + ] + ) + for i in range(len(region_points)): + region_points[i][0:2] = rotate_2d_point( + region_points[i][0:2], rot=self.reference_rot + ) + region_points += base_offset + + for i in range(self.num_attempts): + # sample object coordinates + relative_x = self._sample_x() + relative_y = self._sample_y() + + # apply rotation + object_x, object_y = rotate_2d_point( + [relative_x, relative_y], rot=self.reference_rot + ) + + object_x = object_x + base_offset[0] + object_y = object_y + base_offset[1] + object_z = self.z_offset + base_offset[2] + if on_top: + object_z -= obj.bottom_offset[-1] + + # random rotation + quat = self._sample_quat() + # multiply this quat by the object's initial rotation if it has the attribute specified + if hasattr(obj, "init_quat"): + quat = quat_multiply(quat, obj.init_quat) + quat = convert_quat( + quat_multiply( + convert_quat(ref_quat, to="xyzw"), + convert_quat(quat, to="xyzw"), + ), + to="wxyz", + ) + + location_valid = True + + # ensure object placed fully in region + if self.ensure_object_boundary_in_range and not obj_in_region( + obj, + obj_pos=[object_x, object_y, object_z], + obj_quat=convert_quat(quat, to="xyzw"), + p0=region_points[0], + px=region_points[1], + py=region_points[2], + ): + location_valid = False + continue + + # objects cannot overlap + if self.ensure_valid_placement: + for (x, y, z), other_quat, other_obj in placed_objects.values(): + if spawn_ref_obj and other_obj == spawn_ref_obj: + if not objs_intersect( + obj=obj, + obj_pos=[object_x, object_y, object_z], + obj_quat=convert_quat(quat, to="xyzw"), + other_obj=other_obj, + other_obj_pos=[x, y, z], + other_obj_quat=convert_quat(other_quat, to="xyzw"), + ): + location_valid = False + break + else: + if objs_intersect( + obj=obj, + obj_pos=[object_x, object_y, object_z], + obj_quat=convert_quat(quat, to="xyzw"), + other_obj=other_obj, + other_obj_pos=[x, y, z], + other_obj_quat=convert_quat(other_quat, to="xyzw"), + ): + location_valid = False + break + + # ensure at least one point of the object should be in the region + if self.ensure_object_in_ref_region and reference is not None: + _ref_pos, _ref_quat, _ref_obj = placed_objects[reference] + if not obj_in_region_with_keypoints( + obj=obj, + obj_pos=[object_x, object_y, object_z], + obj_quat=convert_quat(quat, to="xyzw"), + region_points=_ref_obj.get_bbox_points(trans=_ref_pos, rot=_ref_quat)[:3], + min_num_points=3, + ): + location_valid = False + break + + if self.ensure_object_out_of_ref_region and neg_reference is not None: + for nf in neg_reference: + assert ( + nf in placed_objects + ), "Invalid negative reference received. Current options are: {}, requested: {}".format( + placed_objects.keys(), nf + ) + _ref_pos, _ref_quat, _ref_obj = placed_objects[nf] + if obj_in_region( + obj=obj, + obj_pos=[object_x, object_y, object_z], + obj_quat=convert_quat(quat, to="xyzw"), + p0=_ref_obj.get_bbox_points(trans=_ref_pos, rot=_ref_quat)[0], + px=_ref_obj.get_bbox_points(trans=_ref_pos, rot=_ref_quat)[1], + py=_ref_obj.get_bbox_points(trans=_ref_pos, rot=_ref_quat)[2], + ): + location_valid = False + break + + if location_valid: + # location is valid, put the object down + pos = (object_x, object_y, object_z) + placed_objects[obj.name] = (pos, quat, obj) + success = True + break + + if not success: + raise RandomizationError(f"Cannot place object: {obj.name}") + + return placed_objects + + +class SequentialCompositeSampler(ObjectPositionSampler): + """ + Samples position for each object sequentially. Allows chaining + multiple placement initializers together - so that object locations can + be sampled on top of other objects or relative to other object placements. + + Args: + name (str): Name of this sampler. + """ + + def __init__(self, name, rng=None): + # Samplers / args will be filled in later + self.samplers = collections.OrderedDict() + self.sample_args = collections.OrderedDict() + self.sampler_optional = collections.OrderedDict() + + super().__init__(name=name, rng=rng) + + def append_sampler(self, sampler, sample_args=None, optional=False): + """ + Adds a new placement initializer with corresponding @sampler and arguments + + Args: + sampler (ObjectPositionSampler): sampler to add + sample_args (None or dict): If specified, should be additional arguments to pass to @sampler's sample() + call. Should map corresponding sampler's arguments to values (excluding @fixtures argument) + optional (bool): If True, do not raise an error if the sampler fails to place all objects. This parameter + is for handling optional clutter generation. When True, clutter objects that can't be placed due to + space constraints are silently skipped. + + Raises: + AssertionError: [Object name in samplers] + """ + # Verify that all added mujoco objects haven't already been added, and add to this sampler's objects dict + for obj in sampler.mujoco_objects: + assert ( + obj not in self.mujoco_objects + ), f"Object '{obj.name}' already has sampler associated with it!" + self.mujoco_objects.append(obj) + self.samplers[sampler.name] = sampler + self.sample_args[sampler.name] = sample_args + self.sampler_optional[sampler.name] = optional + + def hide(self, mujoco_objects): + """ + Helper method to remove an object from the workspace. + + Args: + mujoco_objects (MujocoObject or list of MujocoObject): Object(s) to hide + """ + sampler = UniformRandomSampler( + name="HideSampler", + mujoco_objects=mujoco_objects, + x_range=[-10, -20], + y_range=[-10, -20], + rotation=[0, 0], + rotation_axis="z", + z_offset=10, + ensure_object_boundary_in_range=False, + ensure_valid_placement=False, + rng=self.rng, + ) + self.append_sampler(sampler=sampler) + + def add_objects(self, mujoco_objects): + """ + Override super method to make sure user doesn't call this (all objects should implicitly belong to sub-samplers) + """ + raise AttributeError("add_objects() should not be called for SequentialCompsiteSamplers!") + + def add_objects_to_sampler(self, sampler_name, mujoco_objects): + """ + Adds specified @mujoco_objects to sub-sampler with specified @sampler_name. + + Args: + sampler_name (str): Existing sub-sampler name + mujoco_objects (MujocoObject or list of MujocoObject): Object(s) to add + """ + # First verify that all mujoco objects haven't already been added, and add to this sampler's objects dict + mujoco_objects = ( + [mujoco_objects] if isinstance(mujoco_objects, MujocoObject) else mujoco_objects + ) + for obj in mujoco_objects: + assert ( + obj not in self.mujoco_objects + ), f"Object '{obj.name}' already has sampler associated with it!" + self.mujoco_objects.append(obj) + # Make sure sampler_name exists + assert ( + sampler_name in self.samplers.keys() + ), "Invalid sub-sampler specified, valid options are: {}, " "requested: {}".format( + self.samplers.keys(), sampler_name + ) + # Add the mujoco objects to the requested sub-sampler + self.samplers[sampler_name].add_objects(mujoco_objects) + + def reset(self): + """ + Resets this sampler. In addition to base method, iterates over all sub-samplers and resets them + """ + super().reset() + for sampler in self.samplers.values(): + sampler.reset() + + def sample(self, placed_objects=None, reference=None, on_top=True): + """ + Sample from each placement initializer sequentially, in the order + that they were appended. + + Args: + placed_objects (dict): dictionary of current object placements in the scene as well as any other relevant + obstacles that should not be in contact with newly sampled objects. Used to make sure newly + generated placements are valid. Should be object names mapped to (pos, quat, MujocoObject) + + reference (str or 3-tuple or None): if provided, sample relative placement. This will override each + sampler's @reference argument if not already specified. Can either be a string, which + corresponds to an existing object found in @fixtures, or a direct (x,y,z) value. If None, will sample + relative to this sampler's `'reference_pos'` value. + + on_top (bool): if True, sample placement on top of the reference object. This will override each + sampler's @on_top argument if not already specified. This corresponds to a sampled + z-offset of the current sampled object's bottom_offset + the reference object's top_offset + (if specified) + + Return: + dict: dictionary of all object placements, mapping object_names to (pos, quat, obj), including the + placements specified in @fixtures. Note quat is in (w,x,y,z) form + + Raises: + RandomizationError: [Cannot place all objects] + """ + # Standardize inputs + placed_objects = {} if placed_objects is None else copy(placed_objects) + + # Iterate through all samplers to sample + for sampler, s_args in zip(self.samplers.values(), self.sample_args.values()): + # Pre-process sampler args + if s_args is None: + s_args = {} + for arg_name, arg in zip(("reference", "on_top"), (reference, on_top)): + if arg_name not in s_args: + s_args[arg_name] = arg + # Run sampler + try: + new_placements = sampler.sample( + placed_objects=placed_objects or s_args.pop("placed_objects", {}), + **s_args, + ) + except RandomizationError: + if self.sampler_optional[sampler.name]: + continue + else: + raise + # Update placements + placed_objects.update(new_placements) + + # only return placements for newly placed objects + sampled_obj_names = [ + obj.name for sampler in self.samplers.values() for obj in sampler.mujoco_objects + ] + return {k: v for (k, v) in placed_objects.items() if k in sampled_obj_names} + + +class MultiRegionSampler(ObjectPositionSampler): + def __init__( + self, + name, + regions, + side="all", + mujoco_objects=None, + rotation=None, + rotation_axis="z", + ensure_object_boundary_in_range=True, + ensure_valid_placement=True, + ensure_object_in_ref_region=False, + rng=None, + z_offset=0.0, + ): + if len(regions) != 4: + raise ValueError("Exactly four sites (one for each quadrant) must be provided.") + if side not in self.valid_sides: + raise ValueError("Invalid value for side, must be one of:", self.valid_sides) + + # initialize sides and regions + if side in self.sides_combinations: + self.sides = self.sides_combinations[side] + else: + self.sides = [side] + self.regions = regions + + # create a list of uniform samplers (one for each site) + self.samplers = list() + for s in self.sides: + site = self.regions[s] + sampler = UniformRandomSampler( + name=name, + mujoco_objects=mujoco_objects, + reference_pos=site["pos"], + x_range=site["x_range"], + y_range=site["y_range"], + rotation=rotation, + rotation_axis=rotation_axis, + ensure_object_boundary_in_range=ensure_object_boundary_in_range, + ensure_valid_placement=ensure_valid_placement, + ensure_object_in_ref_region=ensure_object_in_ref_region, + z_offset=z_offset, + rng=rng, + ) + self.samplers.append(sampler) + + def sample(self, fixtures=None, reference=None, on_top=True): + # randomly picks a sampler and calls its sample function + sampler = self.rng.choice(self.samplers) + return sampler.sample(fixtures=fixtures, reference=reference, on_top=on_top) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/postprocess_xml_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/postprocess_xml_utils.py new file mode 100644 index 0000000..ce535f7 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/postprocess_xml_utils.py @@ -0,0 +1,78 @@ +import h5py +import json +import argparse +import os +from shutil import copyfile +import robosuite +import xml.etree.ElementTree as ET +from tqdm import tqdm +import numpy as np + + +def get_eef_panda_site(prefix): + """ + Helper function for new element added in robosuite v1.5 + """ + + eef_panda_site = ( + """ """ + ) + + tree = ET.ElementTree(ET.fromstring(eef_panda_site)) + root = tree.getroot() + for elem in root.iter(): + elem.attrib["name"] = "{}_{}".format(prefix, elem.get("name")) + + return tree + + +def update_gripper_name(gripper_name: str): + tokens = gripper_name.split("_") + tokens.insert(1, "right") + return "_".join(tokens) + + +def convert_xml(xml_str): + """ + Postprocess xml string generated by robosuite to be compatible with robosuite v1.5 + This script should not the xml string if it was already generated using robosuite v1.5 + Args: + xml_str (str): xml string to process (from robosuite v1.4.1) + """ + + root = ET.fromstring(xml_str) + worldbody = root.find("worldbody") + bodies_to_process = [] + + for body in worldbody.iter("body"): + body_name = body.get("name") + if "link0" in body_name: + if body not in bodies_to_process: + bodies_to_process.append(body) + + # add elements introduced in v1.5 + for body in bodies_to_process: + prefix = body.get("name").split("_")[0] + eef_tree = get_eef_panda_site(prefix) + body.append(eef_tree.getroot()) + + assets = root.find("asset") + + # change path from mounts to bases + for asset in assets.iter("mesh"): + path = asset.get("file") + assert "robosuite/models/assets" in path or "robocasa/models/assets" in path, print(path) + if "mounts" in path: + assert not "robot" in path + new_path = path.replace( + "robosuite/models/assets/mounts", "robosuite/models/assets/bases" + ) + asset.set("file", new_path) + + # update naming prefixes + for elem in root.iter(): + for attr, value in elem.attrib.items(): + if value.startswith("mount"): + elem.set(attr, value.replace("mount", "fixed_base", 1)) + elif value.startswith("gripper"): + elem.set(attr, update_gripper_name(value)) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/configs.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/configs.py new file mode 100644 index 0000000..fe17da5 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/configs.py @@ -0,0 +1,126 @@ +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from typing import TYPE_CHECKING, Any, Optional, Union + +import numpy as np + +from robocasa.utils.scene.success_criteria import SuccessCriteria + +if TYPE_CHECKING: + from robocasa.utils.scene.scene import SceneObject + + +@dataclass(frozen=True) +class ReferenceConfig: + obj: Optional[SceneObject] + spawn_id: Optional[int] = None + on_top: bool = True + + def to_dict(self) -> Optional[dict[str, Any]]: + if self.obj is None: + return None + if self.spawn_id is None: + return {"reference": self.obj.cfg.name, "on_top": self.on_top} + else: + return {"reference": (self.obj.cfg.name, self.spawn_id)} + + +@dataclass(frozen=True) +class SamplingConfig: + x_range: np.ndarray = np.zeros(2) + y_range: np.ndarray = np.zeros(2) + rotation: np.ndarray = np.zeros(2) + reference_pos: np.ndarray = np.zeros(3) + z_offset: float = 0 + reference: Optional[ReferenceConfig] = None + + @property + def sampling_args(self) -> Optional[dict]: + if self.reference is None: + return None + return self.reference.to_dict() + + +@dataclass(frozen=True) +class ObjectConfig: + mjcf_path: str + name: str + static: bool = False + scale: float = 1.0 + density: int = 100 + friction: tuple[float, float, float] = (1, 1, 1) + margin: Optional[float] = None + rgba: Optional[tuple[float, float, float, float]] = None + sampler_config: SamplingConfig = SamplingConfig() + + +@dataclass(frozen=True) +class SceneConfig: + objects: list[SceneObject] + success: SuccessCriteria + instruction: str + + +class SceneHandedness(Enum): + UNIVERSAL = "UNIVERSAL" + LEFT = "LEFT" + RIGHT = "RIGHT" + + +@dataclass(frozen=True) +class SceneScaleConfig: + planar_scale: Union[float, tuple[float, float]] = 1.0 + vertical_scale: float = 1.0 + handedness: SceneHandedness = SceneHandedness.UNIVERSAL + handedness_axis: int = 1 + + def get_ref_pos(self, pos: np.ndarray) -> np.ndarray: + pos = pos.astype(float, copy=True) # ensure float + pos[self.handedness_axis] *= float(self._handedness_factor) + pos[:2] = pos[:2] * np.array(self._scale, dtype=float) + pos[2] *= float(self.vertical_scale) + return pos + + def get_rotation(self, rot: Optional[np.ndarray]) -> Optional[np.ndarray]: + if rot is None: + return None + rot = rot.astype(float, copy=True) + rot *= float(self._handedness_factor) + return rot + + def get_ranges(self, x_range: np.ndarray, y_range: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + x_range = x_range.astype(float, copy=True) + y_range = y_range.astype(float, copy=True) + + if self.handedness_axis == 0: + x_range *= float(self._handedness_factor) + x_range.sort() + elif self.handedness_axis == 1: + y_range *= float(self._handedness_factor) + y_range.sort() + return x_range, y_range + + def get_z_offset(self, z_offset: float) -> float: + return float(z_offset) * float(self.vertical_scale) + + @property + def _handedness_factor(self) -> float: + if self.handedness == "LEFT": # replace with enum comparison + return -1.0 + else: + return 1.0 + + @property + def _scale(self) -> tuple[float, float]: + if isinstance(self.planar_scale, tuple) and len(self.planar_scale) == 2: + return float(self.planar_scale[0]), float(self.planar_scale[1]) + elif isinstance(self.planar_scale, (float, int)): + scale = float(self.planar_scale) + return scale, scale + else: + raise ValueError( + f"Invalid type for 'planar_scale': expected float or tuple[float, float], " + f"but got {type(self.planar_scale).__name__}" + ) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/scene.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/scene.py new file mode 100644 index 0000000..f59e05a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/scene.py @@ -0,0 +1,146 @@ +from graphlib import CycleError, TopologicalSorter +from pathlib import Path + +import numpy as np +from robosuite.environments import MujocoEnv +from robosuite.utils import transform_utils as T +from robosuite.utils.mjcf_utils import xml_path_completion + +import robocasa +from robocasa.models.objects.objects import MJCFObject +from robocasa.utils.placement_samplers import ( + ObjectPositionSampler, + SequentialCompositeSampler, + UniformRandomSampler, +) +from robocasa.utils.scene.configs import ( + ObjectConfig, + SceneConfig, + SceneScaleConfig, +) + + +class SceneObject: + def __init__(self, cfg: ObjectConfig): + self.cfg = cfg + self._mujoco_object = self._get_object() + + @property + def mj_obj(self) -> MJCFObject: + return self._mujoco_object + + def get_sampler_with_args( + self, env: MujocoEnv, scene_scale: SceneScaleConfig + ) -> tuple[ObjectPositionSampler, dict]: + return ( + self._get_sampler(self._mujoco_object, env, scene_scale), + self.cfg.sampler_config.sampling_args, + ) + + def _get_object(self) -> MJCFObject: + abs_mjcf_path = ( + self.cfg.mjcf_path + if Path(self.cfg.mjcf_path).is_absolute() + else xml_path_completion(self.cfg.mjcf_path, root=robocasa.models.assets_root) + ) + return MJCFObject( + name=self.cfg.name, + mjcf_path=abs_mjcf_path, + scale=self.cfg.scale, + solimp=(0.998, 0.998, 0.001), + solref=(0.001, 1), + density=self.cfg.density, + friction=self.cfg.friction, + margin=self.cfg.margin, + static=self.cfg.static, + rgba=self.cfg.rgba, + ) + + def _get_sampler( + self, obj: MJCFObject, env: MujocoEnv, scene_scale: SceneScaleConfig + ) -> ObjectPositionSampler: + x_range, y_range = scene_scale.get_ranges( + self.cfg.sampler_config.x_range, self.cfg.sampler_config.y_range + ) + z_offset = scene_scale.get_z_offset(self.cfg.sampler_config.z_offset) + rotation = scene_scale.get_rotation(self.cfg.sampler_config.rotation) + reference_pos = scene_scale.get_ref_pos(self.cfg.sampler_config.reference_pos) + return UniformRandomSampler( + name=f"{self.cfg.name}_sampler", + mujoco_objects=obj, + x_range=x_range, + y_range=y_range, + rotation=rotation, + rng=env.rng, + rotation_axis="z", + ensure_object_boundary_in_range=False, + ensure_valid_placement=False, + reference_pos=reference_pos, + z_offset=z_offset, + ) + + +class Scene: + def __init__(self, env: MujocoEnv, config: SceneConfig, scene_scale: SceneScaleConfig): + self._env = env + self._config = config + self._scene_scale = scene_scale + self._scene_objects: dict[MJCFObject, SceneObject] = {} + for scene_obj in self._config.objects: + self._scene_objects[scene_obj.mj_obj] = scene_obj + self._scene_sampler = self._get_sampler() + + @property + def mujoco_objects(self) -> list[MJCFObject]: + return list(self._scene_objects.keys()) + + @property + def instruction(self) -> str: + return self._config.instruction + + def reset(self) -> None: + sim = self._env.sim + object_placements = self._scene_sampler.sample() + for obj_pos, obj_quat, obj in object_placements.values(): + scene_obj = self._scene_objects[obj] + if scene_obj.cfg.static: + body_id = sim.model.body_name2id(obj.root_body) + sim.model.body_pos[body_id] = obj_pos + sim.model.body_quat[body_id] = obj_quat + obj.set_pos(obj_pos) + obj.set_euler(T.mat2euler(T.quat2mat(T.convert_quat(obj_quat, "xyzw")))) + else: + joint_name = next(j for j in obj.joints if np.size(sim.data.get_joint_qpos(j)) == 7) + qpos = np.concatenate([np.array(obj_pos), np.array(obj_quat)]) + # Set qpos0 to support robust mujoco environment reset + start_i, end_i = sim.model.get_joint_qpos_addr(joint_name) + sim.data.model.qpos0[start_i:end_i] = qpos + # Set qpos + sim.data.set_joint_qpos( + joint_name, + qpos, + ) + + def success(self) -> bool: + return self._config.success.is_true(self._env) + + def _get_sampler(self) -> SequentialCompositeSampler: + sorted_objects = self._sort_scene_objects(list(self._scene_objects.values())) + + scene_sampler = SequentialCompositeSampler(name="SceneSampler", rng=self._env.rng) + for obj in sorted_objects: + sampler, args = obj.get_sampler_with_args(self._env, self._scene_scale) + scene_sampler.append_sampler(sampler=sampler, sample_args=args) + + return scene_sampler + + @staticmethod + def _sort_scene_objects(scene_objects: list[SceneObject]) -> list[SceneObject]: + ts = TopologicalSorter() + for obj in scene_objects: + ref = obj.cfg.sampler_config.reference.obj if obj.cfg.sampler_config.reference else None + ts.add(obj, *([ref] if ref else [])) + try: + return list(ts.static_order()) + except CycleError: + raise ValueError("Circular reference detected") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/success_criteria.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/success_criteria.py new file mode 100644 index 0000000..55c48ba --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/scene/success_criteria.py @@ -0,0 +1,157 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING, Optional + +import numpy as np +from robosuite.environments import MujocoEnv +from robosuite.environments.manipulation.manipulation_env import ManipulationEnv + +from robocasa.utils import object_utils as OU + +if TYPE_CHECKING: + from robocasa.utils.scene.scene import SceneObject + + +class SuccessCriteria(ABC): + @abstractmethod + def is_true(self, env: MujocoEnv) -> bool: + raise NotImplementedError + + +class IsUpright(SuccessCriteria): + def __init__(self, obj: SceneObject, threshold=0.8, symmetric: bool = False): + self._obj = obj + self._threshold = threshold + self._symmetric = symmetric + + def is_true(self, env: MujocoEnv) -> bool: + return OU.check_obj_upright( + env, + obj_name=self._obj.mj_obj.name, + threshold=self._threshold, + symmetric=self._symmetric, + ) + + +class IsClose(SuccessCriteria): + def __init__( + self, + obj_a: SceneObject, + obj_b: SceneObject, + max_distance: float, + use_xy_only: bool = False, + ): + self._obj_a = obj_a + self._obj_b = obj_b + self._max_distance = max_distance + self._use_xy_only = use_xy_only + + def is_true(self, env: MujocoEnv) -> bool: + pos_a = env.sim.data.body_xpos[env.obj_body_id[self._obj_a.mj_obj.name]] + pos_b = env.sim.data.body_xpos[env.obj_body_id[self._obj_b.mj_obj.name]] + if self._use_xy_only: + pos_a = pos_a[:2] + pos_b = pos_b[:2] + return np.linalg.norm(pos_a - pos_b) <= self._max_distance + + +class IsInContact(SuccessCriteria): + def __init__(self, obj_a: SceneObject, obj_b: SceneObject): + self._obj_a = obj_a + self._obj_b = obj_b + + def is_true(self, env: MujocoEnv) -> bool: + return env.check_contact(self._obj_a.mj_obj, self._obj_b.mj_obj) + + +class IsGripperFar(SuccessCriteria): + def __init__(self, obj: SceneObject, threshold: float = 0.4): + self._threshold = threshold + self._obj = obj + + def is_true(self, env: MujocoEnv) -> bool: + return OU.any_gripper_obj_far(env, obj_name=self._obj.mj_obj.name, th=self._threshold) + + +class IsGrasped(SuccessCriteria): + _GRIPPERS = {"left", "right"} + + def __init__(self, obj: SceneObject, gripper: Optional[str] = None): + self._obj = obj + self._grippers = {gripper} if gripper in self._GRIPPERS else self._GRIPPERS + + def is_true(self, env: MujocoEnv) -> bool: + assert isinstance(env, ManipulationEnv), "Expected a ManipulationEnv instance." + robot = env.robots[0] + return any(env._check_grasp(robot.gripper[g], self._obj.mj_obj) for g in self._grippers) + + +class IsRobotInRange(SuccessCriteria): + def __init__(self, target: SceneObject, threshold: float, planar: bool): + self._target = target + self._threshold = threshold + self._planar = planar + + def is_true(self, env: MujocoEnv) -> bool: + robot = env.robots[0] + robot_pos = env.sim.data.get_body_xpos(robot.robot_model.root_body) + obj_pos = env.sim.data.body_xpos[env.obj_body_id[self._target.mj_obj.name]] + if self._planar: + robot_pos = robot_pos[:2] + obj_pos = obj_pos[:2] + return np.linalg.norm(robot_pos - obj_pos) <= self._threshold + + +class IsPositionInRange(SuccessCriteria): + def __init__( + self, + target: SceneObject, + axis_index: int, + min_val: Optional[float] = None, + max_val: Optional[float] = None, + ): + self._target = target + self._axis = axis_index + self._min = min_val or float("-inf") + self._max = max_val or float("inf") + + def is_true(self, env: MujocoEnv) -> bool: + pos = env.sim.data.body_xpos[env.obj_body_id[self._target.mj_obj.name]] + return self._min <= pos[self._axis] <= self._max + + +class IsJointQposInRange(SuccessCriteria): + def __init__(self, obj: SceneObject, joint_id: int, min_val: float, max_val: float): + self._obj = obj + self._joint_id = joint_id + self._min = min_val + self._max = max_val + + def is_true(self, env: MujocoEnv) -> bool: + qpos = env.sim.data.get_joint_qpos(self._obj.mj_obj.joints[self._joint_id]) + return self._min <= qpos <= self._max + + +class NotCriteria(SuccessCriteria): + def __init__(self, criteria: SuccessCriteria): + self.criteria = criteria + + def is_true(self, env: MujocoEnv) -> bool: + return not self.criteria.is_true(env) + + +class AllCriteria(SuccessCriteria): + def __init__(self, *criteria: SuccessCriteria): + self.criteria = criteria + + def is_true(self, env: MujocoEnv) -> bool: + return all(c.is_true(env) for c in self.criteria) + + +class AnyCriteria(SuccessCriteria): + def __init__(self, *criteria: SuccessCriteria): + self.criteria = criteria + + def is_true(self, env: MujocoEnv) -> bool: + return any(c.is_true(env) for c in self.criteria) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/texture_swap.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/texture_swap.py new file mode 100644 index 0000000..4d9044a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/texture_swap.py @@ -0,0 +1,670 @@ +import inspect +import os +import random +from copy import deepcopy +from pathlib import Path + +import numpy as np +from lxml import etree as ET +from robosuite.utils.mjcf_utils import find_elements + +import robocasa + +TEXTURES_DIR = Path(inspect.getfile(robocasa)).parent / "models" / "assets" / "generative_textures" + +CABINET_TEX_NAMES = [ + "flat copy 26.png", + "196415a6-3c0f-4a1f-a644-fc54a6801223.png", + "2a310e7f-8005-4dba-8dea-edb1484e1e28.png", + "flat copy 37.png", + "1fb8c016-b655-48ab-ad53-d6238a745065.png", + "f80067c5-9967-4607-880f-1c4002d400cb.png", + "00846c84-f900-4bdd-897f-3928e7d87dce.png", + "flat copy 38.png", + "54c4d59e-c0b1-4608-a953-568e9ae13409.png", + "flat copy 2.png", + "flat copy 23.png", + "flat copy 16.png", + "251695ed-d413-48fe-817e-c3e02b3ee117.png", + "18338c06-57fc-4882-ad44-b5f3b3846806.png", + "flat copy 40.png", + "36a1e6cb-98a1-4ada-a14e-8c04f135c5ee.png", + "f06bca10-a7ce-4a4a-92f6-973d7400fe8a.png", + "flat copy 41.png", + "397c17e0-e91b-4fa6-98d9-97e6d2f009ed.png", + "532a9660-8c7a-488b-8930-9134a9479afa.png", + "flat copy 33.png", + "d0299fe2-026b-4727-85e1-cb5de303e315.png", + "68a933bc-df0c-4891-bd41-2c05e4fde504.png", + "6698bb80-52f7-4e5b-ad6a-4a8122504f9a.png", + "99f0113d-8cb4-40b6-9670-b4be4c1fc475.png", + "flat copy 32.png", + "flat copy 34.png", + "c1ad279c-9093-469e-aa20-b73af0fc8db4.png", + "518ddc39-55c2-4b18-8bf6-e9c4f0806760.png", + "flat copy 8.png", + "860cab10-089c-4d99-ae89-3e4f87df5d9c.png", + "f29d41e9-d285-47e4-ad29-f04423533802.png", + "flat copy 13.png", + "flat copy 12.png", + "e6420ee4-14f0-404f-9cd7-abc9a69da621.png", + "flat copy 4.png", + "de7f86d5-56c9-4331-a9aa-655687dc354b.png", + "flat copy 35.png", + "flat.png", + "b6abcb20-8b90-42d3-9175-ba6b4d53a7a9.png", + "d29a6530-28d6-4dbe-bce5-9014d613ae5e.png", + "992f49b2-3f7c-4c54-a4d6-75af4693353b.png", + "flat copy 27.png", + "fd195b15-b98c-47c9-b608-de4157f6ceba.png", + "flat copy 10.png", + "flat copy 15.png", + "af993127-4ec6-4fa3-9a27-018af985a4a0.png", + "8de5fd51-b7b5-4014-a892-f0498c35fec2.png", + "flat copy 3.png", + "flat copy 11.png", + "flat copy 30.png", + "edc5b189-60a3-4aa0-a68d-cc35009f3648.png", + "flat copy 20.png", + "flat copy 19.png", + "9fe8882d-3aa9-4d2b-8de3-d9d220ea526c.png", + "flat copy 7.png", + "flat copy 46.png", + "flat copy 9.png", + "230e3f37-02d7-4be9-b6a8-1e4fb3acd368.png", + "flat copy 45.png", + "7f426b18-12ee-492b-9c69-dfa8ccbc42cc.png", + "flat copy 49.png", + "flat copy 22.png", + "7fe6e2dc-615f-432e-92d4-0a4f17a9ac49.png", + "flat copy 47.png", + "flat copy 42.png", + "34c9cf7b-5b5d-4c81-bb95-6d5743d25a18.png", + "aae9ec57-7df1-4af7-8e40-62962b90cce4.png", + "flat copy 36.png", + "flat copy 25.png", + "5a34b72d-3b7e-4a9a-9a0f-f73f2230fe28.png", + "e2e0160d-9809-48d2-b018-416ee0254fc0.png", + "a6bb714e-0f35-46d6-b7e8-3c18232a4801.png", + "a86a130e-04a3-44b0-99b7-44ce0a63cc2b.png", + "01f783e0-38de-497d-8ebe-c8d8a47e520a.png", + "flat copy 44.png", + "e0e2d63e-900b-4ecb-8df9-28115a58c3a2.png", + "flat copy 50.png", + "flat copy 5.png", + "flat copy 18.png", + "1aac54cf-8bf3-428c-93cd-1d3c258ff6b4.png", + "flat copy 14.png", + "11717454-440d-4b32-85c3-bf0013c66c61.png", + "67d50012-cba6-4525-8e00-b71124861f7e.png", + "25436e21-3140-4875-89a9-d14c5ce20f31.png", + "e4a28073-2252-4603-8c4b-75527a8c5ae9.png", + "flat copy 17.png", + "flat copy 48.png", + "flat copy 6.png", + "28d31de4-53f8-434a-ac57-4d5d27f5ab2f.png", + "flat copy.png", + "flat copy 39.png", + "flat copy 43.png", + "flat copy 24.png", + "6a13b80d-83c7-4fe1-8412-4fc07eefaa35.png", + "43a60b06-97b9-4119-b416-c7fc6783aa81.png", + "flat copy 21.png", + "flat copy 28.png", + "flat copy 31.png", + "flat copy 29.png", +] + +COUNTER_TOP_TEX_NAMES = [ + "5065776e-1fbc-4a70-a786-9a2ec6ee5aa0.png", + "e7f151aa-2354-48d0-94e8-071462f16ca7.png", + "b6618923-8455-44c3-8ae6-b766f34267cc.png", + "71d72043-3d97-4ea6-82f0-3ccbb53e0534.png", + "deb1e565-5c29-4285-aef8-f904c2b41dfb.png", + "ecf23b20-3db2-476a-bc06-442754ba5e76.png", + "39be4639-f28b-4bfa-8c1e-4b37e40395be.png", + "8bafdd4c-a44c-4bc0-8978-e7bc0abcedb4.png", + "3b958a28-0807-484a-92e7-f17b4c6dcbeb.png", + "ecd3c3fb-c4f2-4021-86d8-665e92b2735e.png", + "014ac251-0435-41c2-9dee-c664ee268327.png", + "0171bbba-6293-4117-b0ad-bc4b98d1c20e.png", + "0bd64a61-e098-4d06-ba3f-0a12269d3584.png", + "d5db5c5c-779b-4de0-ac66-31856239d488.png", + "9c47a3ac-2e56-47c1-86d4-006f98e21987.png", + "133b4eaf-bb93-4655-844c-a02278dfc032.png", + "58bcfa8c-6e11-494f-8fc5-d5a32ac7b3de.png", + "276de88a-374b-4dde-ac45-c52d651044b3.png", + "f52f1aaa-c061-42c0-8bdd-d2edb4af8736.png", + "1ccd1303-2e23-44c4-bb21-0dd474b0d899.png", + "761d55a2-5858-4d02-ab13-0bb272a67eed.png", + "c9d714cd-b00b-4d96-b6a6-cdc9e01c1b71.png", + "06da7e0e-c12c-4da7-a56b-9a3c82d87809.png", + "e5d1c208-34ad-49a7-8151-c0d55ecff952.png", + "9ccbd07a-887c-48c8-a806-0fac93606bb0.png", + "3d48ef42-02d9-41ed-bef6-f5c0dd47cdfa.png", + "9e1fd142-1d1c-4a82-a961-681db31b343f.png", + "5259cbe7-d956-4a0a-99d1-d7c7d391488f.png", + "94bdb3c5-faa2-4274-9337-ca5b9fabce88.png", + "e572f3d8-9ef2-4cd2-83c1-8acb766dbc4b.png", + "956782b3-ca4b-49e8-8939-6c50e87b9609.png", + "258270ae-3af4-4644-a031-176efac2c3f9.png", + "1fa52c2f-7474-4aa9-9ff9-0e411a78daac.png", + "f1299a54-59e5-42ea-9f53-e9f11fd6ca3c.png", + "6a6a86a2-bbba-422a-9620-5b90adaaf7a3.png", + "771c03d6-f0c8-4e3e-b734-0a07fff03c22.png", + "8c6ed656-6d1c-4707-82a2-cbff87a589f7.png", + "6a4f994f-293f-4a8a-8f8d-ac99f8e58adf.png", + "da90fb4d-3dfb-4a17-8ecc-95e82ab69b32.png", + "1563c9ee-30ab-44cc-b21f-fab5ad8df99d.png", + "932f5612-9058-4923-820d-894d630190cb.png", + "5e2958ab-c27d-41c8-8167-de9053f79038.png", + "2408cfed-ad91-43e2-9295-fdafafb76c8d.png", + "8549466d-50fe-4cb9-9001-e93532e7938c.png", + "00e4b598-2dd0-4ff9-b0a4-46e223a9296e.png", + "0666bf9a-4625-4132-bd7f-0079dace8c8c.png", + "ceb18b7d-1dcf-4aa2-adae-0e0aae1c5eeb.png", + "a4903060-7bfd-4bd5-b8d6-b916d172a82b.png", + "f5f4cf81-db71-42c7-b889-c25c145132c6.png", + "bf277536-38a6-41cd-a278-bd68e25816fd.png", + "a1cfe2ab-12cd-4d43-9ffc-77d48d0493f3.png", + "1c72f7db-374f-47db-8424-9526d4e7fa7a.png", + "3dc423ca-b351-429f-9842-1bd17e3ce360.png", + "2a8aea9d-218c-4601-8e8d-b98eb93fa461.png", + "451b6b8b-5d86-44ad-853b-e99153ba48e1.png", + "25d4c91a-c831-4249-b78e-a4b25d0b1ea1.png", + "b4f0036f-f2e7-4167-bbc4-708ee783052f.png", + "db04923e-96ac-41db-a16c-901f10a41bf0.png", + "65e832aa-974a-4ca8-9dd8-e90a96d75b6c.png", + "f66bf802-8273-4df7-a58a-faa8ba6ed46f.png", + "827976d2-dbab-47a3-905f-602e945a6b30.png", + "124bd263-2b8a-4057-9211-b36869047fd2.png", + "f69e2027-7163-4a53-8361-6050e5679242.png", + "d366b33d-335e-4a76-91f5-a5ce14266c89.png", + "adcb6ab3-7ecf-47f9-a11e-6a505ad23d98.png", + "2cea6a36-7053-4808-be22-fc5cc76b5531.png", + "ac3491a2-e817-4f5e-8453-ce34cb78a8a9.png", + "a875270b-fd82-409d-9bed-1d567657ff8e.png", + "f29cfb8b-d640-48a4-ae5e-02395ed7c6db.png", + "8c165058-abec-4ea9-9990-19b7eff4df4f.png", + "1c6ea15c-1cad-4a0c-838a-5eee8f99cfc9.png", + "897d7c04-f1bd-4bb1-bffb-bfada23a5b58.png", + "1b23c19f-7e79-4b96-a744-42615a72d722.png", + "f921b606-5363-41c2-aa93-dc8ec1475fda.png", + "efca4079-dd9f-4d54-8943-216d6c02e70a.png", + "040ff758-026f-4313-b390-ec86d3690f9d.png", + "0104c474-6473-4e05-b789-6bdb59f55da7.png", + "f73ed4a7-c1b1-4660-9eba-00f2ba25977b.png", + "0b184f66-ba4f-4fcd-9556-bea9f58dc9c6.png", + "7f638e9b-10e8-448c-a2f9-a75ae414dcfa.png", + "e563fd4b-656f-45f1-b803-df0d6dcb8bd3.png", + "0ed8ccf8-33e5-45a2-9200-4dc1a5565494.png", + "1ab263d2-ea24-4fe7-9383-f227aadc8c47.png", + "561d2d94-ec96-4246-974a-d9abc6f2297a.png", + "9af6ae6e-a842-4baa-a01e-cec57045c367.png", + "1c6fc0f3-d4ef-423b-9bba-d4a5d59c05df.png", + "76a72b39-24ec-4044-aad8-49f381939063.png", + "08ca0b3f-17e9-476e-9c17-ff81616c6bd7.png", + "9b637968-8173-44da-8f41-3943c4d182f9.png", + "725eb22a-2c4b-44b0-8aaf-bef282232601.png", + "3e81b87f-10ee-4f74-8ba8-1b724df383fb.png", + "fc9c7e5a-7abb-4ff6-84f2-9fce43bc200f.png", + "a8f33495-cd01-4b66-9c7e-3ba5edec6565.png", + "8585099d-cf7f-4c1c-8f36-937939dba42e.png", + "fe8afe85-5153-48cb-80e0-88de2a577f0c.png", + "46c59505-7dec-48f2-a0d1-c36ac213890e.png", + "95b636b0-8afb-4430-8f6a-1f4e0f4eb808.png", + "5ab4d96a-5401-45d8-977c-b3fef4e55ca9.png", + "8c8dd1cd-6c84-4fa8-b51b-ac736fda7a6c.png", + "76c7b19f-b375-48db-a1df-d79485a86d94.png", +] + +FLOOR_TEX_NAMES = [ + "8e4ea598-7170-4d24-ad12-42d45ef17a3b.png", + "9d7e7eac-9d81-4b04-8a07-ec35f9e7b58d.png", + "83c7c8bf-9277-4f59-88bd-1a4f428262ef.png", + "28359684-bb12-4b99-86b2-8c0998ee8fec.png", + "8ebc0bbe-1656-465e-b31e-766d1aceaec2.png", + "ada4e736-880d-4372-b148-218dc114c7fe.png", + "8a12dcf7-fc25-4341-9ea0-e583f1cc3d71.png", + "28f34ad5-b4f1-4b81-99cc-b04ed28d653b.png", + "b5350f4e-9521-4af1-a03f-a39e5d8b1796.png", + "14b4a5c3-69e0-425c-b8c5-ce495ab87409.png", + "022ec5c1-324e-4217-9f03-92b0e2f9c74f.png", + "9f345d0f-a768-4def-bebf-b818cf0c68dc.png", + "49de13f1-9488-4893-8398-48b0a5f27453.png", + "a938167f-6cb2-4e87-94f5-185eac06ff21.png", + "2aa2a926-fe72-4221-bd30-16e0a0cc9324.png", + "1671701f-60d2-4c71-95cc-8bdbb9f57ff9.png", + "8b24233c-7658-4d52-809d-a6fc71f37554.png", + "9582e8cc-89f5-464f-b183-2f21b8f0f267.png", + "8184ce85-7db2-4b23-908b-1170cc6830cf.png", + "32a18f41-8e7c-4a9d-b5ac-a7ba19a9ed35.png", + "56c9fb44-a8ee-469d-85d4-6d419f9dcba3.png", + "7ff039e9-7563-4ef0-b2ec-ce3d0d259a99.png", + "dbdd0778-8700-4967-bb52-75bbfa7eb29c.png", + "50578da0-60d8-4e12-800e-3df2fbf1bd5a.png", + "cedd99bd-8f8f-4d60-98e8-75e0af9b7e4f.png", + "b6c12637-3bd8-41d4-a787-3c2c271f4221.png", + "22c6be8e-a027-47dc-b2a1-3284cf5baad0.png", + "ed301cb6-fc33-4a6a-a2fd-4ce27a1f5bac.png", + "fc6bffaf-b9fd-4d51-a795-cb95d280dcab.png", + "310661bb-e74a-4b55-b999-ae1e2855990f.png", + "c327867a-02ea-4629-8059-181d3f341dc2.png", + "67a6f678-5f4b-4999-88fd-d0587299eb90.png", + "c65c9338-2903-4c6a-8015-dc606d24e659.png", + "e584ce4a-ff4d-461f-989e-5e4d21da42de.png", + "8776902b-74c1-4a54-94f2-320b4e89b5df.png", + "b588fa60-819c-4ae7-84bb-8f3de009ee1b.png", + "bdddfbd0-acc5-4e73-becb-c91f69986201.png", + "1f13ee23-d3f8-4fe4-b24d-97c4dece35c5.png", + "447fc81b-73c3-4502-8de7-9db654e62902.png", + "f78f9be1-385c-43ed-a89b-a4f11d45695f.png", + "c82d1a06-f78d-4e79-9b3e-23c4756d555d.png", + "573d34d2-a045-455c-b525-34a8571808eb.png", + "07f74135-07ab-4e29-bb00-4782f15ab59f.png", + "bec253e6-b2f0-48f2-afba-9207388a7937.png", + "0139f959-a4a8-4504-b6d2-f9351afd5941.png", + "8373a547-a792-438e-b941-f01ca1f0fb5d.png", + "b37ff2ac-f755-42f1-bdd4-a3a0f30fbfc6.png", + "f068688f-4b20-4d2f-b33a-9732b5050f72.png", + "c707d14d-3089-4a3e-a5b2-1088eda621bc.png", + "81ea9eea-29ea-4516-9e0d-e7ed0df98f5d.png", + "9aa36664-4ac6-4a50-b0e0-05c96f52c58e.png", + "c85db92d-b5a4-4fc8-aabd-9d4c82f3f444.png", + "df431f5c-699e-47ff-9161-b6e264a82163.png", + "4d8ea3e7-4a0b-4015-802c-cbbd0b12ba2e.png", + "3a34566d-b31f-4d3d-a86a-8038fde33b8c.png", + "83c6fd2f-caa2-4d53-b4d2-e19a7f12e6b4.png", + "ef044eea-9ebe-475e-bc05-d4e9d9255e39.png", + "02b897ed-2381-403a-aef1-095096993039.png", + "bb9ea1a0-7bfb-4aa2-85ee-0dbdb5001fa3.png", + "2afa8673-57cc-4953-b178-b57eedd9ad0a.png", + "2066e385-7798-47b3-89cc-1eaad15c75c4.png", + "45639cb9-7ac7-4d8c-a8a1-719d3a53d531.png", + "8f7fabcb-6875-4437-9d9c-27a2552c10dd.png", + "ce8ccc5f-20b9-4d06-aee6-fe85a6c02eaa.png", + "11fa953e-c11d-4dad-8e51-47cc6dc67aeb.png", + "8f51e06a-7bd0-4d2f-9a20-7821a975559a.png", + "1ef4cfa4-cf52-4efd-9fb3-6698650ee239.png", + "4d974231-cd17-4da5-ad9a-7faf83c2d683.png", + "cadaada3-e8f1-4bc7-af71-342215edbbb1.png", + "fb9b36a4-d87a-4c7f-a06c-a1e94869b96b.png", + "5daabcbd-0ce4-4dba-9342-c74d3da4fadf.png", + "612b00d2-819f-46ac-a599-38672184c842.png", + "0b5b5a9a-1435-4dfe-b6f4-19f4dbae1e8e.png", + "11f6a3bb-c36c-45c2-a429-501ac0cabc02.png", + "cf96c697-41f0-4d3c-a1d4-17d0d82719d0.png", + "81d9e9f2-8ff4-4d45-b1dd-d3afd3c62fb0.png", + "82036c07-5321-47c8-974c-27d58e526ba1.png", + "e793023f-7e5f-4211-9724-9022d3c8f920.png", + "0c01c256-4779-4093-a0b9-3347c31ef53f.png", + "9db099e2-56cb-4bec-becc-eb69c298866e.png", + "05f75518-7b70-4098-8983-aca459f33ddb.png", + "7c935010-badd-4fa4-8089-4fc9728dfe96.png", + "f9d057b1-5843-4b68-a58b-9fe87291a72f.png", + "9354be03-5a87-4ce8-85bc-8b0157abeb1c.png", + "9bfe963b-6d17-4704-8a9f-c2de48491b70.png", + "3b70d093-e264-4550-b5e5-f750ba577b78.png", + "67837bbb-05ae-4665-90e5-572130b7fc86.png", + "d339c422-36d0-40f3-8393-24076abb279c.png", + "5ccc395f-efd5-44af-b5f9-885789e26661.png", + "6d58d55a-34cc-455f-8b4c-004fabef6e11.png", + "5729f1d0-0796-4d82-99f7-76a171266fae.png", + "1e6da165-23cb-4426-91cf-450b3ac74e49.png", + "86ae5f3b-1a9c-4895-87eb-b526b403f332.png", + "4e53b13b-317a-484c-9b8b-7417453e45ff.png", + "8ddb08c1-facb-4fb1-841b-bde4e47fe584.png", + "41c3dbec-db54-4417-bb76-f51a4349943e.png", + "a2881fcf-1af3-46e9-87b0-c198fad703d1.png", + "808ec0fb-fcf8-4629-9296-38530521a95e.png", + "7c411449-a3c6-48e4-ab30-d08bca5938d6.png", + "0df28fee-6cef-48ae-9e8e-c0f0f3482412.png", +] + +WALL_TEX_NAMES = [ + "plain copy 49.png", + "18a6abbd-8ca1-43d9-9a0a-8424a7d1e016.png", + "plain copy 2.png", + "0882d081-d8be-49c0-aad6-3f5eff92265b.png", + "plain copy 39.png", + "7a19bf00-4f43-4e39-8fe6-977df7c6d147.png", + "plain copy 8.png", + "9b6f9830-a89d-4a42-965f-581dd0a12617.png", + "bcf9bf9b-ac75-408e-ab2e-516391c39d8e.png", + "plain copy 51.png", + "845424a1-4603-47d7-892c-426c2d427f0d.png", + "6e961263-5c90-41f9-87d3-b9e132c9583a.png", + "c72ea705-e088-4665-9683-6a8685758aef.png", + "plain copy 22.png", + "plain copy 44.png", + "plain copy 10.png", + "df17f0f3-68bb-46e2-ae9f-86cf84f48b04.png", + "plain copy 53.png", + "0906d773-7c22-4173-9edc-efc6e2da88e0.png", + "8995f82f-2435-4395-8ecd-a00824edd923.png", + "0adf5feb-6a2f-4417-811a-b7d70d42fb34.png", + "plain copy 5.png", + "536410ee-0460-43b0-9e8e-799e96439a0d.png", + "plain copy 11.png", + "plain copy 19.png", + "plain copy 17.png", + "plain copy.png", + "plain copy 31.png", + "Lance_Z._white_square_tiles_texture_image_ikea_high_quality_rea_f41b5937-af3e-4df8-b19d-ae4fa942f741.png", + "f6353155-1a10-41a1-be1d-149368938757.png", + "b1a7c75f-6b03-4b10-99cd-3299d7b6beb5.png", + "plain copy 52.png", + "7844f4b6-7d34-46b4-8ea4-19ab954ca58f.png", + "0dd40625-0cf8-4523-92cd-063c96108a23.png", + "plain copy 40.png", + "plain copy 21.png", + "plain copy 36.png", + "plain copy 37.png", + "plain copy 41.png", + "plain copy 23.png", + "1b8b8a44-ce4e-4156-978b-ad66bf4a6f45.png", + "plain copy 46.png", + "d208d65f-4d99-4963-8140-d54180844c8c.png", + "0aa2c331-8025-4417-a157-91467143c85b.png", + "d4775c13-15ea-496d-b5fc-19d2a9cab128.png", + "11760e4b-5f86-46cb-82ae-c87cc6f434d1.png", + "plain copy 3.png", + "plain copy 30.png", + "plain copy 9.png", + "0af727bd-14b4-4f7d-9fee-19f7f001ad34.png", + "67d1229f-7689-4dd6-ab2b-d003e0cd3cb7.png", + "plain copy 33.png", + "plain copy 4.png", + "2149f068-ca27-40ed-b53d-07e53210e678.png", + "plain copy 38.png", + "6b215002-fd31-4f1a-859a-d5d98a2a0c52.png", + "4fe97e2e-ae88-4dc9-8321-3529a2108508.png", + "Lance_Z._white_square_tiles_ikea_high_quality_realistic_ec4bd31e-d76d-4512-8b0d-a86dd8dd6aad.png", + "plain copy 6.png", + "plain copy 13.png", + "0d7afefe-58c8-4161-bf56-0974c7e806a2.png", + "plain copy 28.png", + "314f4070-0b2f-4433-98e7-dbdf18a8bff2.png", + "plain copy 18.png", + "dae6863d-b653-4998-8239-c6a6bb24362c.png", + "497cbc0b-a80a-42b3-8f5e-3bc97c145255.png", + "plain copy 20.png", + "4874a0d3-f5f2-46ea-a1d3-824b2f56f8ce.png", + "plain copy 50.png", + "plain copy 12.png", + "7c7c9950-576a-45fd-b3d1-74cca30f6fe7.png", + "8474c563-65b7-408b-976f-b19b38a44f90.png", + "Lance_Z._white_square_tiles_texture_image_ikea_high_quality_rea_ce2dcd92-29d7-4176-b7fa-79d91dab133b.png", + "738a1240-97f3-4fa6-97df-0ecc4b7aac26.png", + "plain copy 32.png", + "plain copy 29.png", + "a4046f8f-a2d6-4ef4-aa39-2ec541cbd246.png", + "plain copy 25.png", + "cbf05cdc-5d1c-472b-9891-0e4802553359.png", + "plain copy 16.png", + "plain copy 47.png", + "4bb0ad30-b60a-44a6-a259-058fab19b422.png", + "77abb443-a1b8-4b02-96de-1a7bc68d4f3f.png", + "plain copy 27.png", + "plain copy 24.png", + "plain copy 43.png", + "35ded0fe-73a7-4e82-8c01-32f79f5214a3.png", + "4b9d5219-38a0-4303-8837-29c4c51eed54.png", + "8949ad7c-2b89-444c-a758-b513f25937e1.png", + "plain copy 48.png", + "plain copy 45.png", + "plain copy 35.png", + "plain copy 26.png", + "plain copy 42.png", + "f7c0d850-4b56-484f-85ce-e9c4bd01ec22.png", + "9ec0b130-598a-4471-99ee-f68ee49a4a9f.png", + "b1dcb5d2-434e-4bd1-9329-eb1c2a6c039d.png", + "cf0d9763-3596-48de-af34-5f3647d812df.png", + "plain copy 14.png", + "fda88519-7a09-466d-84a2-19b216562dcb.png", +] + + +def get_random_textures(rng, frac=1.0): + """ + This function returns a dictionary of random textures for cabinet, counter top, floor, and wall. + + Args: + rng (np.random.Generator): Random number generator used for texture selection + + frac (float): Fraction of textures to select from the list of available textures + Default: 1.0 (select from all textures) + + Returns: + textures (dict): Dictionary of texture paths + """ + + end_ind = int(frac * 100) + ind = rng.integers(0, end_ind) + + textures = dict( + cab_tex=os.path.join(TEXTURES_DIR, "cabinet", CABINET_TEX_NAMES[ind]), + counter_tex=os.path.join(TEXTURES_DIR, "counter", COUNTER_TOP_TEX_NAMES[ind]), + floor_tex=os.path.join(TEXTURES_DIR, "floor", FLOOR_TEX_NAMES[ind]), + wall_tex=os.path.join(TEXTURES_DIR, "wall", WALL_TEX_NAMES[ind]), + ) + + return textures + + +def replace_counter_top_texture(rng, initial_state: str, new_counter_top_texture_file: str = None): + """ + This function replaces the counter top textures during playback. + + Args: + rng (np.random.Generator): Random number generator used for texture selection + + initial_state (str): Initial env XML string + + new_counter_top_texture_file (str): New texture file for counter top: i.e "marble/dark_marble.png" + If None (default), will replace with a random texture from marble directory + """ + + root = ET.fromstring(initial_state) + asset = root.find("asset") + + if new_counter_top_texture_file is None: + new_counter_top_texture_file = get_random_textures(rng)["counter_tex"] + else: + new_counter_top_texture_file = os.path.join(TEXTURES_DIR, new_counter_top_texture_file) + + # step 1: find the name of texture that will be replaced + counter_tex_name = None + for mat in asset.findall("material"): + name = mat.get("name") + if "counter_top" in name: + counter_tex_name = mat.get("texture") + break + assert counter_tex_name is not None + + # step 2: find and replace texture element + CTOP_TEX_NAME = "counter_top_replacement_texture" + for tex in asset.findall("texture"): + name = tex.get("name") + if name == counter_tex_name: + tex.set("name", CTOP_TEX_NAME) + tex.set("file", str(new_counter_top_texture_file)) + + # step 3: reference new textures in materials + for mat in asset.findall("material"): + name = mat.get("name") + if "counter_top" in name: + mat.set("texture", CTOP_TEX_NAME) + + return ET.tostring(root).decode("utf-8") + + +def replace_cab_textures(rng, initial_state: str, new_cab_texture_file: str = None): + """ + This function replaces the cabinet and counter base textures during playback. + + Args: + rng (np.random.Generator): Random number generator used for texture selection + + initial_state (str): Initial env XML string + + new_cab_texture_file (str): New texture file for counter base and cabinets: i.e "cabinet/..." + If None (default), will replace with a random texture from flat or wood directories + """ + + root = ET.fromstring(initial_state) + asset = root.find("asset") + + if new_cab_texture_file is None: + new_cab_texture_file = get_random_textures(rng)["cab_tex"] + else: + new_cab_texture_file = os.path.join(TEXTURES_DIR, new_cab_texture_file) + + CAB_TEX_NAME_2D = "cab_replacement_texture_2d" + tex_2d = find_elements( + asset, tags="texture", attribs={"name": CAB_TEX_NAME_2D}, return_first=True + ) + if tex_2d is not None: + tex_2d.set("file", str(new_cab_texture_file)) + else: + tex_2d = ET.Element( + "texture", type="2d", name=CAB_TEX_NAME_2D, file=str(new_cab_texture_file) + ) + asset.append(tex_2d) + + CAB_TEX_NAME_CUBE = "cab_replacement_texture_cube" + tex_cube = find_elements( + asset, tags="texture", attribs={"name": CAB_TEX_NAME_CUBE}, return_first=True + ) + if tex_cube is not None: + tex_cube.set("file", str(new_cab_texture_file)) + else: + tex_cube = ET.Element( + "texture", + type="cube", + name=CAB_TEX_NAME_CUBE, + file=str(new_cab_texture_file), + ) + asset.append(tex_cube) + + for mat in asset.findall("material"): + name = mat.get("name") + if "counter_base" in name: + mat.set("texture", CAB_TEX_NAME_CUBE) + elif "housing" in name: + mat.set("texture", CAB_TEX_NAME_CUBE) + elif ( + "stack" in name + or "cab" in name + or "shelves" in name + or "bottom" in name + or ("top" in name and "counter" not in name and "stove" not in name) + ): + if "handle" in name or "transparent" in name: + continue + elif "door" in name: + mat.set("texture", CAB_TEX_NAME_2D) + elif "shelves" in name: + mat.set("texture", CAB_TEX_NAME_2D) + else: + mat.set("texture", CAB_TEX_NAME_CUBE) + + return ET.tostring(root).decode("utf-8") + + +def replace_floor_texture(rng, initial_state: str, new_floor_texture_file: str = None): + """ + This function replaces the counter top textures during playback. + + Args: + rng (np.random.Generator): Random number generator used for texture selection + + initial_state (str): Initial env XML string + + new_floor_texture_file (str): New texture file for counter top: i.e "wood/dark_wood_planks_2.png" + If None (default), will replace with a random texture from wood directory + """ + + root = ET.fromstring(initial_state) + asset = root.find("asset") + + if new_floor_texture_file is None: + new_floor_texture_file = get_random_textures(rng)["floor_tex"] + else: + new_floor_texture_file = os.path.join(TEXTURES_DIR, new_floor_texture_file) + + # step 1: find the name of texture that will be replaced + floor_tex_name = None + for mat in asset.findall("material"): + name = mat.get("name") + if "floor" in name and "backing" not in name: + floor_tex_name = mat.get("texture") + break + assert floor_tex_name is not None + + # step 2: find and replace texture element + FLOOR_TEX_NAME = "floor_replacement_texture" + for tex in asset.findall("texture"): + name = tex.get("name") + if name == floor_tex_name: + tex.set("name", FLOOR_TEX_NAME) + tex.set("file", str(new_floor_texture_file)) + tex.set("type", "2d") + + # step 3: reference new textures in materials + for mat in asset.findall("material"): + name = mat.get("name") + if "floor" in name and "backing" not in name: + mat.set("texture", FLOOR_TEX_NAME) + mat.set("texrepeat", "2 2") + + return ET.tostring(root).decode("utf-8") + + +def replace_wall_texture(rng, initial_state: str, new_wall_texture_file: str = None): + """ + This function replaces the counter top textures during playback. + + Args: + rng (np.random.Generator): Random number generator used for texture selection + + initial_state (str): Initial env XML string + + new_wall_texture_file (str): New texture file for counter top: i.e "wood/dark_wood_planks_2.png" + If None (default), will replace with a random texture from wood directory + """ + + root = ET.fromstring(initial_state) + asset = root.find("asset") + + if new_wall_texture_file is None: + new_wall_texture_file = get_random_textures(rng)["wall_tex"] + else: + new_wall_texture_file = os.path.join(TEXTURES_DIR, new_wall_texture_file) + + # step 1: find the name of texture that will be replaced + wall_tex_name = None + for mat in asset.findall("material"): + name = mat.get("name") + if "wall" in name and "floor" not in name and "backing" not in name: + wall_tex_name = mat.get("texture") + break + assert wall_tex_name is not None + + # step 2: add new texture element + WALL_TEX_NAME = "wall_replacement_texture" + for tex in asset.findall("texture"): + name = tex.get("name") + if name == wall_tex_name: + tex.set("name", WALL_TEX_NAME) + tex.set("file", str(new_wall_texture_file)) + tex.set("type", "2d") + + # step 3: reference new textures in materials + for mat in asset.findall("material"): + name = mat.get("name") + if "wall" in name and "floor" not in name and "backing" not in name: + mat.set("texture", WALL_TEX_NAME) + mat.set("texrepeat", "3 3") + + return ET.tostring(root).decode("utf-8") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/transform_utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/transform_utils.py new file mode 100644 index 0000000..2dc6892 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/transform_utils.py @@ -0,0 +1,56 @@ +from robosuite.utils.transform_utils import * + + +def unmake_pose(pose): + """ + Split homogenous pose matrices back into translation vectors and rotation matrices. + + Args: + pose (np.array): batch of pose matrices with last 2 dimensions of (4, 4) + + Returns: + pos (np.array): batch of position vectors with last dimension of 3 + rot (np.array): batch of rotation matrices with last 2 dimensions of (3, 3) + """ + return pose[..., :3, 3], pose[..., :3, :3] + + +def standardize_quat(quaternions): + """ + Convert a unit quaternion to a standard form: one in which the real + part is non negative. + + Args: + quaternions: Quaternions with real part last (e.g. xyzw), + as tensor of shape (..., 4). + + Returns: + Standardized quaternions as tensor of shape (..., 4). + """ + # if isinstance(quaternions, torch.Tensor): + # return torch.where(quaternions[..., 3:4] < 0, -quaternions, quaternions) + return np.where(quaternions[..., 3:4] < 0, -quaternions, quaternions) + + +def pose_in_world_to_pose_in_ref(pos_in_world, rot_in_world, ref_pos, ref_rot): + """ + Takes a pose in world frame and a reference pose (in world frame) and + transforms the pose to be with respect to the reference frame. + """ + + # Let O be the frame of the item of interest, R be the reference frame, + # and W be the world frame. Then, + # + # T^O_R = (T^R_W)^-1 T^O_W + pose_in_world = make_pose(pos_in_world, rot_in_world) + ref_pose = make_pose(ref_pos, ref_rot) + world_in_ref = pose_inv(ref_pose) + return np.matmul(world_in_ref, pose_in_world) + + +def extract_top_down_angle(rotation_matrix): + """ + Gets top-down angle about z-axis for a given 3D rotation matrix, assuming + that the rotation matrix corresponds to a simple rotation about z-axis. + """ + return np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0]) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/README.md b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/README.md new file mode 100644 index 0000000..770a519 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/README.md @@ -0,0 +1,30 @@ +# MuJoCo USD Integration + +USD is file format originally introduced by Pixar to compose 3D scenes in virtual worlds. We implement the ability to export MuJoCo models and trajectories as USD files for visualization with external renderers such as NVIDIA Omniverse and Blender. + +## Installation + +We assume that users have already installed MuJoCo either directly with pip or from source. For information on how to build MuJoCo from source, please visit this [reference](https://mujoco.readthedocs.io/en/stable/python.html#building-from-source). We also assume Python>=3.8. + +Using the USD bridge functionality requires the installation of additional libraries. Specifically, we utilize the `open3d` and `usd-core` packages. These can be installed directly using pip. + +```bash +pip install open3d +pip install usd-core +``` + +## Getting Started + +We provide a demo script to get started with generating USD files. This demo script is located at `python/mujoco/usd/demo.py`. In this script, we load a the `cards.xml` model and step through it for a given number of steps. At each step, we make a call to the USD exporter to store information for any state changes including the positions and orientations of geoms in the scene. + +## TODOs and Known Bugs + +TODOs: + +- Include tendons and contact forces in USD file +- Include references to third party materials for NVIDIA Omniverse in USD file +- Including custom defined matrials for NVIDIA Omniverse +- Bridge for modeling environments and assets from third party modeling engines to MuJoCo + +Known issues 🐛: +- Texture mapping for non-UV objects not consistent with MuJoCo renderer when ported to new renderers \ No newline at end of file diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/__init__.py new file mode 100644 index 0000000..981ddee --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/__init__.py @@ -0,0 +1,4 @@ +from .component import * +from .exporter import * +from .shapes import * +from .utils import * diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/component.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/component.py new file mode 100644 index 0000000..9c0f90a --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/component.py @@ -0,0 +1,429 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +from typing import List, Optional, Tuple + +import mujoco +import numpy as np + +# TODO: b/288149332 - Remove once USD Python Binding works well with pytype. +# pytype: disable=module-attr +# from open3d import open3d as o3d +# import open3d as o3d +from pxr import Gf, Sdf, Usd, UsdGeom, UsdLux, UsdShade, Vt + +import robocasa.utils.usd.shapes as shapes_component +import robocasa.utils.usd.utils as utils_component + + +class USDMesh: + def __init__( + self, + stage: Usd.Stage, + model: mujoco.MjModel, + geom: mujoco.MjvGeom, + obj_name: str, + dataid: int, + rgba: np.ndarray = np.array([1, 1, 1, 1]), + texture_file: Optional[str] = None, + ): + self.stage = stage + self.model = model + self.geom = geom + self.obj_name = obj_name + self.rgba = rgba + self.dataid = dataid + self.texture_file = texture_file + + xform_path = f"/World/Mesh_Xform_{obj_name}" + mesh_path = f"{xform_path}/Mesh_{obj_name}" + self.usd_xform = UsdGeom.Xform.Define(stage, xform_path) + self.usd_mesh = UsdGeom.Mesh.Define(stage, mesh_path) + self.usd_prim = stage.GetPrimAtPath(mesh_path) + + # setting mesh structure properties + mesh_vert, mesh_face, mesh_facenum = self._get_mesh_geometry() + self.usd_mesh.GetPointsAttr().Set(mesh_vert) + self.usd_mesh.GetFaceVertexCountsAttr().Set([3 for _ in range(mesh_facenum)]) + self.usd_mesh.GetFaceVertexIndicesAttr().Set(mesh_face) + + # setting mesh uv properties + mesh_texcoord, mesh_facetexcoord = self._get_uv_geometry() + self.texcoords = UsdGeom.PrimvarsAPI(self.usd_mesh).CreatePrimvar( + "UVMap", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.faceVarying + ) + self.texcoords.Set(mesh_texcoord) + self.texcoords.SetIndices(Vt.IntArray(mesh_facetexcoord.tolist())) + + self._attach_material() + + # defining ops required by update function + self.transform_op = self.usd_xform.AddTransformOp() + + def get_facetexcoord_ranges(self, nmesh, arr): + facetexcoords_ranges = [0] + running_sum = 0 + for i in range(nmesh): + running_sum += arr[i] * 3 + facetexcoords_ranges.append(running_sum) + return facetexcoords_ranges + + def _get_uv_geometry(self): + mesh_texcoord_adr_from = self.model.mesh_texcoordadr[self.dataid] + mesh_texcoord_adr_to = ( + self.model.mesh_texcoordadr[self.dataid + 1] + if self.dataid < self.model.nmesh - 1 + else len(self.model.mesh_texcoord) + ) + mesh_texcoord = self.model.mesh_texcoord[mesh_texcoord_adr_from:mesh_texcoord_adr_to] + + mesh_facetexcoord_ranges = self.get_facetexcoord_ranges( + self.model.nmesh, self.model.mesh_facenum + ) + + mesh_facetexcoord = self.model.mesh_facetexcoord.flatten() + mesh_facetexcoord = mesh_facetexcoord[ + mesh_facetexcoord_ranges[self.dataid] : mesh_facetexcoord_ranges[self.dataid + 1] + ] + + mesh_facetexcoord[mesh_facetexcoord == len(mesh_texcoord)] = 0 + + return mesh_texcoord, mesh_facetexcoord + + def _get_mesh_geometry(self): + mesh_vert_adr_from = self.model.mesh_vertadr[self.dataid] + mesh_vert_adr_to = ( + self.model.mesh_vertadr[self.dataid + 1] + if self.dataid < self.model.nmesh - 1 + else len(self.model.mesh_vert) + ) + mesh_vert = self.model.mesh_vert[mesh_vert_adr_from:mesh_vert_adr_to] + + mesh_face_adr_from = self.model.mesh_faceadr[self.dataid] + mesh_face_adr_to = ( + self.model.mesh_faceadr[self.dataid + 1] + if self.dataid < self.model.nmesh - 1 + else len(self.model.mesh_face) + ) + mesh_face = self.model.mesh_face[mesh_face_adr_from:mesh_face_adr_to] + + mesh_facenum = self.model.mesh_facenum[self.dataid] + + return mesh_vert, mesh_face, mesh_facenum + + def _attach_material(self): + mtl_path = Sdf.Path(f"/World/_materials/Material_{self.obj_name}") + mtl = UsdShade.Material.Define(self.stage, mtl_path) + + if self.texture_file: + bsdf_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Principled_BSDF")) + image_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Image_Texture")) + uvmap_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("uvmap")) + + # setting the bsdf shader attributes + bsdf_shader.CreateIdAttr("UsdPreviewSurface") + bsdf_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource( + image_shader.ConnectableAPI(), "rgb" + ) + bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(self.rgba[-1])) + bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(self.geom.shininess) + bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set( + 1.0 - self.geom.shininess + ) + + mtl.CreateSurfaceOutput().ConnectToSource(bsdf_shader.ConnectableAPI(), "surface") + + self.usd_mesh.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI) + UsdShade.MaterialBindingAPI(self.usd_mesh).Bind(mtl) + + # setting the image texture attributes + image_shader.CreateIdAttr("UsdUVTexture") + image_shader.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(self.texture_file) + image_shader.CreateInput("sourceColorSpace", Sdf.ValueTypeNames.Token).Set("sRGB") + image_shader.CreateInput("wrapS", Sdf.ValueTypeNames.Token).Set("repeat") + image_shader.CreateInput("wrapT", Sdf.ValueTypeNames.Token).Set("repeat") + image_shader.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource( + uvmap_shader.ConnectableAPI(), "result" + ) + image_shader.CreateOutput("rgb", Sdf.ValueTypeNames.Float3) + + # setting uvmap shader attributes + uvmap_shader.CreateIdAttr("UsdPrimvarReader_float2") + uvmap_shader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("UVMap") + uvmap_shader.CreateOutput("results", Sdf.ValueTypeNames.Float2) + else: + bsdf_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Principled_BSDF")) + + # settings the bsdf shader attributes + bsdf_shader.CreateIdAttr("UsdPreviewSurface") + + bsdf_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set( + tuple(self.rgba[0:3]) + ) + bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(self.rgba[-1])) + bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(self.geom.shininess) + bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set( + 1.0 - self.geom.shininess + ) + + mtl.CreateSurfaceOutput().ConnectToSource(bsdf_shader.ConnectableAPI(), "surface") + + self.usd_mesh.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI) + UsdShade.MaterialBindingAPI(self.usd_mesh).Bind(mtl) + + def update(self, pos: np.ndarray, mat: np.ndarray, visible: bool, frame: int): + transformation_mat = utils_component.create_transform_matrix( + rotation_matrix=mat, translation_vector=pos + ).T + self.transform_op.Set(Gf.Matrix4d(transformation_mat.tolist()), frame) + self.update_visibility(visible, frame) + + def update_visibility(self, visible: bool, frame: int): + if visible: + self.usd_prim.GetAttribute("visibility").Set("inherited", frame) + else: + self.usd_prim.GetAttribute("visibility").Set("invisible", frame) + + +class USDPrimitiveMesh: + def __init__( + self, + mesh_config: List[dict], + stage: Usd.Stage, + geom: mujoco.MjvGeom, + obj_name: str, + rgba: np.ndarray = np.array([1, 1, 1, 1]), + texture_file: Optional[str] = None, + ): + self.mesh_config = mesh_config + self.stage = stage + self.geom = geom + self.obj_name = obj_name + self.rgba = rgba + self.texture_file = texture_file + + self.usd_prim = Usd.Prim() + self.usd_mesh = UsdGeom.Mesh() + self.prim_mesh = None + self.transform_op = Gf.Matrix4d(1.0) + + _, self.prim_mesh = shapes_component.mesh_generator(mesh_config) + + xform_path = f"/World/{self.obj_name}_Xform" + mesh_path = f"{xform_path}/{obj_name}" + self.usd_xform = UsdGeom.Xform.Define(stage, xform_path) + self.usd_mesh = UsdGeom.Mesh.Define(stage, mesh_path) + self.usd_prim = stage.GetPrimAtPath(mesh_path) + + self.prim_mesh.translate(-self.prim_mesh.get_center()) + + mesh_vert, mesh_face, mesh_facenum = self._get_mesh_geometry() + self.usd_mesh.GetPointsAttr().Set(mesh_vert) + self.usd_mesh.GetFaceVertexCountsAttr().Set([3 for _ in range(mesh_facenum)]) + self.usd_mesh.GetFaceVertexIndicesAttr().Set(mesh_face) + + # setting mesh uv properties + mesh_texcoord, mesh_facetexcoord = self._get_uv_geometry() + self.texcoords = UsdGeom.PrimvarsAPI(self.usd_mesh).CreatePrimvar( + "UVMap", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.faceVarying + ) + + self.texcoords.Set(mesh_texcoord) + self.texcoords.SetIndices(Vt.IntArray([i for i in range(mesh_facenum * 3)])) + + self._set_refinement_properties() + + # setting attributes for the shape + self._attach_material() + + # defining ops required by update function + self.transform_op = self.usd_xform.AddTransformOp() + + def _set_refinement_properties(self): + self.usd_prim.GetAttribute("subdivisionScheme").Set("none") + + def _get_uv_geometry(self): + + assert self.prim_mesh + + x_scale, y_scale = self.geom.texrepeat + + mesh_texcoord = np.array(self.prim_mesh.triangle_uvs) + mesh_facetexcoord = np.asarray(self.prim_mesh.triangles) + + x_multiplier, y_multiplier = 1, 1 + if self.geom.texuniform: + x_multiplier, y_multiplier = self.geom.size[:2] + + mesh_texcoord[:, 0] *= x_scale * x_multiplier + mesh_texcoord[:, 1] *= y_scale * y_multiplier + + return mesh_texcoord, mesh_facetexcoord.flatten() + + def _get_mesh_geometry(self): + + assert self.prim_mesh + + # get mesh geometry from the open3d mesh model + mesh_vert = np.asarray(self.prim_mesh.vertices) + mesh_face = np.asarray(self.prim_mesh.triangles) + + return mesh_vert, mesh_face, len(mesh_face) + + def _attach_material(self): + mtl_path = Sdf.Path(f"/World/_materials/Material_{self.obj_name}") + mtl = UsdShade.Material.Define(self.stage, mtl_path) + if self.texture_file: + bsdf_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Principled_BSDF")) + image_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Image_Texture")) + uvmap_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("uvmap")) + + # setting the bsdf shader attributes + bsdf_shader.CreateIdAttr("UsdPreviewSurface") + bsdf_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource( + image_shader.ConnectableAPI(), "rgb" + ) + bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(self.rgba[-1])) + bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(self.geom.shininess) + bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set( + 1.0 - self.geom.shininess + ) + + mtl.CreateSurfaceOutput().ConnectToSource(bsdf_shader.ConnectableAPI(), "surface") + + self.usd_mesh.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI) + UsdShade.MaterialBindingAPI(self.usd_mesh).Bind(mtl) + + # setting the image texture attributes + image_shader.CreateIdAttr("UsdUVTexture") + image_shader.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(self.texture_file) + image_shader.CreateInput("sourceColorSpace", Sdf.ValueTypeNames.Token).Set("sRGB") + image_shader.CreateInput("wrapS", Sdf.ValueTypeNames.Token).Set("repeat") + image_shader.CreateInput("wrapT", Sdf.ValueTypeNames.Token).Set("repeat") + image_shader.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource( + uvmap_shader.ConnectableAPI(), "result" + ) + image_shader.CreateOutput("rgb", Sdf.ValueTypeNames.Float3) + + # setting uvmap shader attributes + uvmap_shader.CreateIdAttr("UsdPrimvarReader_float2") + uvmap_shader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("UVMap") + uvmap_shader.CreateOutput("results", Sdf.ValueTypeNames.Float2) + else: + bsdf_shader = UsdShade.Shader.Define(self.stage, mtl_path.AppendPath("Principled_BSDF")) + + # settings the bsdf shader attributes + bsdf_shader.CreateIdAttr("UsdPreviewSurface") + bsdf_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set( + tuple(self.rgba[:3]) + ) + bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(self.rgba[-1])) + bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(self.geom.shininess) + bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set( + 1.0 - self.geom.shininess + ) + + mtl.CreateSurfaceOutput().ConnectToSource(bsdf_shader.ConnectableAPI(), "surface") + + self.usd_mesh.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI) + UsdShade.MaterialBindingAPI(self.usd_mesh).Bind(mtl) + + def update(self, pos: np.ndarray, mat: np.ndarray, visible: bool, frame: int): + transformation_mat = utils_component.create_transform_matrix( + rotation_matrix=mat, translation_vector=pos + ).T + self.transform_op.Set(Gf.Matrix4d(transformation_mat.tolist()), frame) + self.update_visibility(visible, frame) + + def update_visibility(self, visible: bool, frame: int): + if visible: + self.usd_prim.GetAttribute("visibility").Set("inherited", frame) + else: + self.usd_prim.GetAttribute("visibility").Set("invisible", frame) + + +class USDSphereLight: + def __init__(self, stage: Usd.Stage, obj_name: str, radius: Optional[float] = 0.3): + self.stage = stage + + xform_path = f"/World/Light_Xform_{obj_name}" + light_path = f"{xform_path}/Light_{obj_name}" + self.usd_xform = UsdGeom.Xform.Define(stage, xform_path) + self.usd_light = UsdLux.SphereLight.Define(stage, light_path) + self.usd_prim = stage.GetPrimAtPath(light_path) + + # we assume in mujoco that all lights are point lights + self.usd_light.GetRadiusAttr().Set(radius) + self.usd_light.GetTreatAsPointAttr().Set(False) + self.usd_light.GetNormalizeAttr().Set(True) + + # defining ops required by update function + self.translate_op = self.usd_xform.AddTranslateOp() + + def update(self, pos: np.ndarray, intensity: int, color: np.ndarray, frame: int): + self.translate_op.Set(Gf.Vec3d(pos.tolist()), frame) + + if not np.any(pos): + intensity = 0 + + self.usd_light.GetIntensityAttr().Set(intensity) + self.usd_light.GetColorAttr().Set(Gf.Vec3d(color.tolist())) + + +class USDDomeLight: + def __init__(self, stage: Usd.Stage, obj_name: str): + self.stage = stage + + xform_path = f"/World/Light_Xform_{obj_name}" + light_path = f"{xform_path}/Light_{obj_name}" + self.usd_xform = UsdGeom.Xform.Define(stage, xform_path) + self.usd_light = UsdLux.DomeLight.Define(stage, light_path) + self.usd_prim = stage.GetPrimAtPath(light_path) + + # we assume in mujoco that all lights are point lights + self.usd_light.GetNormalizeAttr().Set(True) + + def update(self, intensity: int, color: np.ndarray, frame: int): + self.usd_light.GetIntensityAttr().Set(intensity) + self.usd_light.GetExposureAttr().Set(0.0) + self.usd_light.GetColorAttr().Set(Gf.Vec3d(color.tolist())) + + +class USDCamera: + def __init__(self, stage: Usd.Stage, obj_name: str): + self.stage = stage + + xform_path = f"/World/Camera_Xform_{obj_name}" + camera_path = f"{xform_path}/Camera_{obj_name}" + self.usd_xform = UsdGeom.Xform.Define(stage, xform_path) + self.usd_camera = UsdGeom.Camera.Define(stage, camera_path) + self.usd_prim = stage.GetPrimAtPath(camera_path) + + # defining ops required by update function + self.transform_op = self.usd_xform.AddTransformOp() + + # self.usd_camera.CreateFocalLengthAttr().Set(18.14756) # default in omniverse + self.usd_camera.CreateFocalLengthAttr().Set(12) + self.usd_camera.CreateFocusDistanceAttr().Set(400) + + self.usd_camera.GetHorizontalApertureAttr().Set(12) + + self.usd_camera.GetClippingRangeAttr().Set(Gf.Vec2f(1e-4, 1e6)) + + def update(self, cam_pos: np.ndarray, cam_mat: np.ndarray, frame: int): + + transformation_mat = utils_component.create_transform_matrix( + rotation_matrix=cam_mat, translation_vector=cam_pos + ).T + self.transform_op.Set(Gf.Matrix4d(transformation_mat.tolist()), frame) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/demo.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/demo.py new file mode 100644 index 0000000..629895e --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/demo.py @@ -0,0 +1,58 @@ +import argparse +from pathlib import Path + +import mujoco +from tqdm import tqdm + +from robocasa.utils.usd import exporter + + +def generate_usd_trajectory(args): + + # load a model to mujoco + model_path = args.model_path + m = mujoco.MjModel.from_xml_path(model_path) + d = mujoco.MjData(m) + + # create an instance of the USDExporter + exp = exporter.USDExporter( + model=m, + output_directory_name=Path(args.model_path).stem, + output_directory_root=args.output_directory_root, + camera_names=args.camera_names, + ) + + # step through the model for length steps + for i in tqdm(range(args.length)): + mujoco.mj_step(m, d) + exp.update_scene(d) + + exp.save_scene(filetype=args.export_extension) + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser() + + parser.add_argument("--model_path", type=str, required=True, help="path to mjcf xml model") + + parser.add_argument("--length", type=int, default=100, help="length of trajectory to render") + + parser.add_argument( + "--output_directory_root", + type=str, + default="../usd_trajectories/", + help="location where to create usd files", + ) + + parser.add_argument("--camera_names", type=str, nargs="+", help="cameras to include in usd") + + parser.add_argument( + "--export_extension", + type=str, + default="usd", + help="extension of exported file (can be usd, usda, or usdc)", + ) + + args = parser.parse_args() + generate_usd_trajectory(args) diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter.py new file mode 100644 index 0000000..cca5988 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter.py @@ -0,0 +1,403 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +import os +from typing import List, Optional, Tuple, Union + +import mujoco +import numpy as np +import scipy +import termcolor +import tqdm +from PIL import Image as im +from PIL import ImageOps + +# TODO: b/288149332 - Remove once USD Python Binding works well with pytype. +# pytype: disable=module-attr +from pxr import Sdf, Usd, UsdGeom + +import robocasa.utils.usd.component as component_module +import robocasa.utils.usd.shapes as shapes_module + + +class USDExporter: + def __init__( + self, + model: mujoco.MjModel, + height: int = 480, + width: int = 480, + max_geom: int = 10000, + output_directory_name: str = "mujoco_usdpkg", + output_directory_root: str = "./", + light_intensity: int = 10000, + camera_names: Optional[List[str]] = None, + specialized_materials_file: Optional[str] = None, + verbose: bool = True, + ): + """Initializes a new USD Exporter + + Args: + model: an MjModel instance. + height: image height in pixels. + width: image width in pixels. + max_geom: Optional integer specifying the maximum number of geoms that + can be rendered in the same scene. If None this will be chosen + automatically based on the estimated maximum number of renderable + geoms in the model. + output_directory_name: name of root directory to store outputted frames + and assets generated by the USD renderer. + output_directory_root: path to root directory storing generated frames + and assets by the USD renderer. + verbose: decides whether to print updates. + """ + + buffer_width = model.vis.global_.offwidth + buffer_height = model.vis.global_.offheight + + if width > buffer_width: + raise ValueError( + f""" + Image width {width} > framebuffer width {buffer_width}. Either reduce the image + width or specify a larger offscreen framebuffer in the model XML using the + clause: + + + """.lstrip() + ) + + if height > buffer_height: + raise ValueError( + f""" + Image height {height} > framebuffer height {buffer_height}. Either reduce the + image height or specify a larger offscreen framebuffer in the model XML using + the clause: + + + """.lstrip() + ) + + self.model = model + self.height = height + self.width = width + self.max_geom = max_geom + self.output_directory_name = output_directory_name + self.output_directory_root = output_directory_root + self.light_intensity = light_intensity + self.camera_names = camera_names + self.specialized_materials_file = specialized_materials_file + self.verbose = verbose + + self.frame_count = 0 # maintains how many times we have saved the scene + self.updates = 0 + + self.geom_name2usd = {} + + # initializing rendering requirements + self.renderer = mujoco.Renderer(model, height, width, max_geom) + self._initialize_usd_stage() + self._scene_option = mujoco.MjvOption() # using default scene option + + # initializing output_directories + self._initialize_output_directories() + + # loading required textures for the scene + self._load_textures() + + @property + def usd(self): + return self.stage.GetRootLayer().ExportToString() + + @property + def scene(self): + return self.renderer.scene + + def _initialize_usd_stage(self): + self.stage = Usd.Stage.CreateInMemory() + UsdGeom.SetStageUpAxis(self.stage, UsdGeom.Tokens.z) + self.stage.SetStartTimeCode(0) + # add as user input + self.stage.SetTimeCodesPerSecond(60.0) + + default_prim = UsdGeom.Xform.Define(self.stage, Sdf.Path("/World")).GetPrim() + self.stage.SetDefaultPrim(default_prim) + + def _initialize_output_directories(self): + self.output_directory_path = os.path.join( + self.output_directory_root, self.output_directory_name + ) + if not os.path.exists(self.output_directory_path): + os.makedirs(self.output_directory_path) + + self.frames_directory = os.path.join(self.output_directory_path, "frames") + if not os.path.exists(self.frames_directory): + os.makedirs(self.frames_directory) + + self.assets_directory = os.path.join(self.output_directory_path, "assets") + if not os.path.exists(self.assets_directory): + os.makedirs(self.assets_directory) + + if self.verbose: + print( + termcolor.colored( + "Writing output frames and assets to" f" {self.output_directory_path}", + "green", + ) + ) + + def update_scene( + self, + data: mujoco.MjData, + scene_option: Optional[mujoco.MjvOption] = None, + ): + """Updates the scene with latest sim data + + Args: + data: structure storing current simulation state + scene_option: we use this to determine which geom groups to activate + """ + + self.frame_count += 1 + + scene_option = scene_option or self._scene_option + + # update the mujoco renderer + self.renderer.update_scene(data, scene_option=scene_option) + + # TODO: update scene options + if self.updates == 0: + self._initialize_usd_stage() + + self._load_lights() + self._load_cameras() + + self._update_geoms() + self._update_lights() + self._update_cameras(data, scene_option=scene_option) + + self.updates += 1 + + def _load_textures(self): + # TODO: remove code once added internally to mujoco + data_adr = 0 + self.texture_files = [] + for texture_id in tqdm.tqdm(range(self.model.ntex)): + texture_height = self.model.tex_height[texture_id] + texture_width = self.model.tex_width[texture_id] + pixels = 3 * texture_height * texture_width + img = im.fromarray( + self.model.tex_rgb[data_adr : data_adr + pixels].reshape( + texture_height, texture_width, 3 + ) + ) + img = ImageOps.flip(img) + + texture_file_name = f"texture_{texture_id}.png" + + img.save(os.path.join(self.assets_directory, texture_file_name)) + + relative_path = os.path.relpath(self.assets_directory, self.frames_directory) + img_path = os.path.join( + relative_path, texture_file_name + ) # relative path, TODO: switch back to this + + self.texture_files.append(img_path) + + data_adr += pixels + + if self.verbose: + print( + termcolor.colored( + f"Completed writing {self.model.ntex} textures to" f" {self.assets_directory}", + "green", + ) + ) + + def _load_geom(self, geom: mujoco.MjvGeom): + + geom_name = self._get_geom_name(geom.objtype, geom.objid) + + assert geom_name not in self.geom_name2usd + + texture_file = self.texture_files[geom.texid] if geom.texid != -1 else None + + if geom.type == mujoco.mjtGeom.mjGEOM_MESH: + usd_geom = component_module.USDMesh( + stage=self.stage, + model=self.model, + geom=geom, + obj_name=geom_name, + dataid=self.model.geom_dataid[geom.objid], + rgba=geom.rgba, + texture_file=texture_file, + ) + else: + mesh_config = shapes_module.mesh_config_generator( + name=geom_name, geom_type=geom.type, size=geom.size + ) + usd_geom = component_module.USDPrimitiveMesh( + mesh_config=mesh_config, + stage=self.stage, + geom=geom, + obj_name=geom_name, + rgba=geom.rgba, + texture_file=texture_file, + ) + + self.geom_name2usd[geom_name] = usd_geom + + def _update_geoms(self): + + geom_names = set(self.geom_name2usd.keys()) + + for i in range(self.scene.ngeom): + geom = self.scene.geoms[i] + geom_name = mujoco.mj_id2name(self.model, geom.objtype, geom.objid) + if not geom_name: + geom_name = "None" + geom_name += f"_{geom.objid}" + + # iterate through all geoms in the scene and makes update + for i in range(self.scene.ngeom): + geom = self.scene.geoms[i] + geom_name = self._get_geom_name(geom.objtype, geom.objid) + + if geom_name not in self.geom_name2usd: + self._load_geom(geom) + if self.geom_name2usd[geom_name]: + self.geom_name2usd[geom_name].update_visibility(False, 0) + + if self.geom_name2usd[geom_name]: + self.geom_name2usd[geom_name].update( + pos=geom.pos, + mat=geom.mat, + visible=geom.rgba[3] > 0, + frame=self.updates, + ) + if geom_name in geom_names: + geom_names.remove(geom_name) + + for geom_name in geom_names: + if self.geom_name2usd[geom_name]: + self.geom_name2usd[geom_name].update_visibility(False, self.updates) + + def _load_lights(self): + # initializes an usd light object for every light in the scene + self.usd_lights = [] + for i in range(self.scene.nlight): + light = self.scene.lights[i] + if not np.allclose(light.pos, [0, 0, 0]): + self.usd_lights.append + (component_module.USDSphereLight(stage=self.stage, obj_name=str(i))) + else: + self.usd_lights.append(None) + + def _update_lights(self): + for i in range(self.scene.nlight): + light = self.scene.lights[i] + + if np.allclose(light.pos, [0, 0, 0]): + continue + + if i >= len(self.usd_lights) or self.usd_lights[i] is None: + continue + + self.usd_lights[i].update( + pos=light.pos, + intensity=self.light_intensity, + color=light.diffuse, + frame=self.updates, + ) + + def _load_cameras(self): + self.usd_cameras = [] + if self.camera_names is not None: + for name in self.camera_names: + self.usd_cameras.append(component_module.USDCamera(stage=self.stage, obj_name=name)) + + def _update_cameras( + self, + data: mujoco.MjData, + scene_option: Optional[mujoco.MjvOption] = None, + ): + for i in range(len(self.usd_cameras)): + + camera = self.usd_cameras[i] + camera_name = self.camera_names[i] + + self.renderer.update_scene(data, scene_option=scene_option, camera=camera_name) + + avg_camera = mujoco.mjv_averageCamera(self.scene.camera[0], self.scene.camera[1]) + + forward = avg_camera.forward + up = avg_camera.up + right = np.cross(forward, up) + + R = np.eye(3) + R[:, 0] = right + R[:, 1] = up + R[:, 2] = -forward + + camera.update(cam_pos=avg_camera.pos, cam_mat=R, frame=self.updates) + + def add_light( + self, + pos: List[float], + intensity: int, + objid: int, + radius: Optional[float] = 1.0, + color: Optional[np.ndarray] = np.array([0.3, 0.3, 0.3]), + obj_name: Optional[str] = "light_1", + light_type: Optional[str] = "sphere", + ): + + if light_type == "sphere": + new_light = component_module.USDSphereLight( + stage=self.stage, obj_name=str(objid), radius=radius + ) + + new_light.update(pos=np.array(pos), intensity=intensity, color=color, frame=0) + elif light_type == "dome": + new_light = component_module.USDDomeLight(stage=self.stage, obj_name=str(objid)) + + new_light.update(intensity=intensity, color=color, frame=0) + + def add_camera( + self, + pos: List[float], + rotation_xyz: List[float], + objid: int, + obj_name: Optional[str] = "camera_1", + ): + new_camera = component_module.USDCamera(stage=self.stage, obj_name=str(objid)) + + r = scipy.spatial.transform.Rotation.from_euler("xyz", rotation_xyz, degrees=True) + new_camera.update(cam_pos=np.array(pos), cam_mat=r.as_matrix(), frame=0) + + def save_scene(self, filetype: str = "usd"): + assert filetype in ["usd", "usda", "usdc"] + self.stage.SetEndTimeCode(self.frame_count) + self.stage.Export( + f"{self.output_directory_root}/{self.output_directory_name}/frames/frame_{self.frame_count}_.{filetype}" + ) + if self.verbose: + print( + termcolor.colored(f"Completed writing frame_{self.frame_count}.{filetype}", "green") + ) + + def _get_geom_name(self, objtype, objid): + geom_name = mujoco.mj_id2name(self.model, objtype, objid) + if not geom_name: + geom_name = "None" + geom_name += f"_{objid}" + return geom_name diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter_test.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter_test.py new file mode 100644 index 0000000..40ce805 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/exporter_test.py @@ -0,0 +1,71 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Tests for the MuJoCo USD Exporter.""" + +import logging +import os + +import mujoco +from absl.testing import absltest +from etils import epath + +# Open3D and USD are not fully supported on all MuJoCo architectures. +# pylint: disable=python.style(g-import-not-at-top) +execute_test = True +try: + from pxr import Usd + + from robocasa.utils.usd import exporter as exporter_module +except ImportError: + logging.warning("Skipping test due to missing import") + execute_test = False +# pylint: enable=python.style(g-import-not-at-top) + + +class ExporterTest(absltest.TestCase): + def test_usd_export(self): + if not execute_test: + return + + output_dir = os.getenv("TEST_UNDECLARED_OUTPUTS_DIR") + xml = """ + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + data = mujoco.MjData(model) + exporter = exporter_module.USDExporter( + model, + output_directory_name="mujoco_usdpkg", + output_directory_root=output_dir, + ) + exporter.update_scene(data) + exporter.save_scene("export.usda") + + with open( + os.path.join(output_dir, "mujoco_usdpkg/frames", "frame_1_.export.usda"), + "r", + ) as f: + golden_path = os.path.join(epath.resource_path("mujoco"), "testdata", "usd_golden.usda") + with open(golden_path, "r") as golden_file: + self.assertEqual(f.read(), golden_file.read()) + + +if __name__ == "__main__": + absltest.main() diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/shapes.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/shapes.py new file mode 100644 index 0000000..d0f7d98 --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/shapes.py @@ -0,0 +1,138 @@ +import copy +import pprint + +import mujoco +import numpy as np +import open3d as o3d + + +def create_hemisphere( + radius: float, resolution: int = 20, theta_steps: int = 50, phi_steps: int = 50 +): + + points = [] + for i in range(phi_steps + 1): + phi = np.pi / 2 * i / phi_steps + for j in range(theta_steps + 1): + theta = 2 * np.pi * j / theta_steps + x = radius * np.sin(phi) * np.cos(theta) + y = radius * np.sin(phi) * np.sin(theta) + z = radius * np.cos(phi) + points.append([x, y, z]) + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + + mesh = pcd.compute_convex_hull()[0] + + return mesh + + +def mesh_config_generator(name: str, geom_type: mujoco.mjtGeom, size: np.ndarray): + + if geom_type == mujoco.mjtGeom.mjGEOM_PLANE: + return { + "name": name, + "box": { + "width": size[0] * 2 if size[0] > 0 else 100, + "height": size[1] * 2 if size[1] > 0 else 100, + "depth": 0.001, + "map_texture_to_each_face": True, + }, + } + elif geom_type == mujoco.mjtGeom.mjGEOM_SPHERE: + return {"name": name, "sphere": {"radius": float(size[0])}} + elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE: + cylinder = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_CYLINDER, size) + return { + "name": name, + "cylinder": cylinder["cylinder"], + "left_hemisphere": { + "radius": size[0], + "transform": {"translate": (0, 0, -size[2]), "rotate": (np.pi, 0, 0)}, + }, + "right_hemisphere": { + "radius": size[0], + "transform": {"translate": (0, 0, size[2])}, + }, + } + elif geom_type == mujoco.mjtGeom.mjGEOM_ELLIPSOID: + sphere = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_SPHERE, [1.0]) + sphere["sphere"]["transform"] = {"scale": tuple(size)} + return { + "name": name, + "sphere": sphere["sphere"], + } + elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER: + return { + "name": name, + "cylinder": { + "radius": size[0], + "height": size[2] * 2, + }, + } + elif geom_type == mujoco.mjtGeom.mjGEOM_BOX: + return { + "name": name, + "box": { + "width": size[0] * 2, + "height": size[1] * 2, + "depth": size[2] * 2, + }, + } + else: + raise NotImplemented( + f"{geom_type} primitive geom type not implemented with USD integration" + ) + + +def mesh_generator(mesh_config: dict): + + assert "name" in mesh_config + + mesh = None + + for shape, config in mesh_config.items(): + + if shape == "name": + continue + + if "box" in shape: + prim_mesh = o3d.geometry.TriangleMesh.create_box( + width=mesh_config[shape]["width"], + height=mesh_config[shape]["height"], + depth=mesh_config[shape]["depth"], + create_uv_map=True, + map_texture_to_each_face=True, + ) + elif "hemisphere" in shape: + prim_mesh = create_hemisphere(radius=mesh_config[shape]["radius"]) + elif "sphere" in shape: + prim_mesh = o3d.geometry.TriangleMesh.create_sphere( + radius=mesh_config[shape]["radius"], create_uv_map=True + ) + elif "cylinder" in shape: + prim_mesh = o3d.geometry.TriangleMesh.create_cylinder( + radius=mesh_config[shape]["radius"], + height=mesh_config[shape]["height"], + create_uv_map=True, + ) + + if "transform" in config: + + if "rotate" in config["transform"]: + R = mesh.get_rotation_matrix_from_xyz(config["transform"]["rotate"]) + prim_mesh.rotate(R, center=(0, 0, 0)) + if "scale" in config["transform"]: + prim_mesh.vertices = o3d.utility.Vector3dVector( + np.asarray(prim_mesh.vertices) * np.array(config["transform"]["scale"]) + ) + if "translate" in config["transform"]: + prim_mesh.translate(config["transform"]["translate"]) + + if not mesh: + mesh = prim_mesh + else: + mesh += prim_mesh + + return mesh_config["name"], mesh diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/utils.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/utils.py new file mode 100644 index 0000000..8c4408c --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/usd/utils.py @@ -0,0 +1,27 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +import numpy as np + + +def create_transform_matrix(rotation_matrix, translation_vector): + # Ensure rotation_matrix and translation_vector are NumPy arrays + rotation_matrix = np.array(rotation_matrix) + translation_vector = np.array(translation_vector) + + transform_matrix = np.eye(4) + transform_matrix[:3, :3] = rotation_matrix + transform_matrix[:3, 3] = translation_vector + + return transform_matrix diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/visuals_utls.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/visuals_utls.py new file mode 100644 index 0000000..8b46fef --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/utils/visuals_utls.py @@ -0,0 +1,31 @@ +from dataclasses import dataclass + +import numpy as np + +from robocasa.models.objects.objects import MJCFObject + + +@dataclass(frozen=True) +class Gradient: + rgba_a: np.ndarray = np.array([0, 0, 0, 1]) + rgba_b: np.ndarray = np.array([1, 1, 1, 1]) + + def sample_linear(self, t: float) -> np.ndarray: + return (1 - t) * self.rgba_a + t * self.rgba_b + + def sample_per_channel(self, t: tuple[float, float, float, float]): + return [np.interp(t[i], [0, 1], [self.rgba_a[i], self.rgba_b[i]]) for i in range(4)] + + +def randomize_materials_rgba( + rng: np.random.Generator, mjcf_obj: MJCFObject, gradient: Gradient, linear: bool +): + for asset in mjcf_obj.asset: + if asset.tag == "material": + if linear: + t = rng.uniform(0, 1) + rgba = gradient.sample_linear(t) + else: + t = rng.uniform(low=[0, 0, 0, 1], high=[1, 1, 1, 1]).tolist() + rgba = gradient.sample_per_channel(t) + asset.set("rgba", f"{rgba[0]:.3f} {rgba[1]:.3f} {rgba[2]:.3f} {rgba[3]:.3f}") diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/__init__.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/__init__.py new file mode 100644 index 0000000..c3b45ba --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/__init__.py @@ -0,0 +1 @@ +from robocasa.wrappers.ik_wrapper import IKWrapper diff --git a/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/ik_wrapper.py b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/ik_wrapper.py new file mode 100644 index 0000000..24a714f --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/robocasa/wrappers/ik_wrapper.py @@ -0,0 +1,226 @@ +import xml.etree.ElementTree as ET +from copy import deepcopy + +import numpy as np + +from robocasa.models.grippers import G1ThreeFingerLeftHand, G1ThreeFingerRightHand +from robosuite.examples.third_party_controller.mink_controller import IKSolverMink +from robosuite.models.grippers import InspireLeftHand, InspireRightHand +from robosuite.models.grippers import FourierLeftHand, FourierRightHand +from robosuite.utils.mjcf_utils import new_body, new_geom, new_site +from robosuite.wrappers import VisualizationWrapper +import robosuite.utils.transform_utils as T + + +class IKWrapper(VisualizationWrapper): + def __init__(self, env, indicator_configs=None, ik_indicator=False): + self.ik_indicator = ik_indicator + if ik_indicator: + indicator_configs = [] + for i in range(2): + indicator_configs.append( + { + "name": f"indicator_x_{i}", + "type": "capsule", + "pos": [0.012, 0, 0], + "size": [0.003, 0.012], + "rgba": [1, 0.5, 0.5, 0.8], + "quat": [0, 0.7071, 0, 0.7071], + } + ) + indicator_configs.append( + { + "name": f"indicator_y_{i}", + "type": "capsule", + "pos": [0, 0.012, 0], + "size": [0.003, 0.012], + "rgba": [0.5, 1, 0.5, 0.8], + "quat": [0, 0, 0.7071, 0.7071], + } + ) + indicator_configs.append( + { + "name": f"indicator_z_{i}", + "type": "capsule", + "pos": [0, 0, 0.012], + "size": [0.003, 0.012], + "rgba": [0.5, 0.5, 1, 0.8], + } + ) + super().__init__(env, indicator_configs=indicator_configs) + else: + # Fall back to VisualizationWrapper + super().__init__env, indicator_configs() + + def step(self, action): + if self.ik_indicator: + composite_controller = self.robots[0].composite_controller + if composite_controller.name == "BASIC": + # indicators will be set by calling `set_target_poses_outside_env` + pass + elif composite_controller.name in [ + "WHOLE_BODY_MINK_IK", + "WHOLE_BODY_EXTERNAL_IK", + "HYBRID_WHOLE_BODY_MINK_IK", + ]: + assert composite_controller.joint_action_policy.input_ref_frame == "base" + input_action = action[: composite_controller.joint_action_policy.control_dim] + input_action = input_action.reshape( + len(composite_controller.joint_action_policy.site_names), -1 + ) + input_pos = input_action[:, :3] + input_ori = input_action[:, 3:] + input_quat_wxyz = np.array( + [np.roll(T.axisangle2quat(input_ori[i]), 1) for i in range(len(input_ori))] + ) + base_pos = self.sim.data.body_xpos[self.sim.model.body_name2id("robot0_base")] + base_ori = self.sim.data.body_xmat[ + self.sim.model.body_name2id("robot0_base") + ].reshape(3, 3) + base_pose = T.make_pose(base_pos, base_ori) + for _, name in enumerate(self.get_indicator_names()): + # indicators for x,y,z-axis share the same pose + i = int(name.split("_")[-1]) + input_pose = T.make_pose( + input_pos[i], T.quat2mat(np.roll(input_quat_wxyz[i], -1)) + ) + target_pose = np.dot(base_pose, input_pose) + self.set_indicator_pos(name, target_pose[:3, 3]) + self.set_indicator_ori(name, target_pose[:3, :3]) + else: + assert ( + False + ), f"Unsupported composite controller {composite_controller.name} for IKWrapper" + + ret = super().step(action) + + return ret + + def set_target_poses_outside_env(self, input_poses): + if self.env.robots[0].robot_model.default_base in ["NullBase", "NoActuationBase"]: + base_name = "robot0_base" + elif self.env.robots[0].robot_model.default_base == "FloatingLeggedBase": + base_name = "mobilebase0_support" + else: + assert False, f"Unsupported base type: {self.env.robots[0].robot_model.default_base}" + base_id = self.sim.model.body_name2id(base_name) + base_pos = self.sim.data.body_xpos[base_id] + base_ori = self.sim.data.body_xmat[base_id].reshape(3, 3) + base_pose = T.make_pose(base_pos, base_ori) + for _, name in enumerate(self.get_indicator_names()): + i = int(name.split("_")[-1]) + input_pose = input_poses[i].copy() + input_pose[3, 3] = 1 + target_pose = np.dot(base_pose, input_pose) + self.set_indicator_pos(name, target_pose[:3, 3]) + self.set_indicator_ori(name, target_pose[:3, :3]) + + # TODO: this will fix the duplicated body issue in the public robosuite repo + def _add_indicators_to_model(self, xml): + """ + Adds indicators to the mujoco simulation model + + Args: + xml (string): MJCF model in xml format, for the current simulation to be loaded + """ + if self.indicator_configs is not None: + root = ET.fromstring(xml) + worldbody = root.find("worldbody") + + from robosuite.utils.mjcf_utils import ( + find_elements, + find_parent, + new_element, + ) + + for arm in ["right", "left"]: + gripper = self.env.robots[0].gripper[arm] + if ( + not isinstance(gripper, InspireLeftHand) + and not isinstance(gripper, InspireRightHand) + and not isinstance(gripper, FourierLeftHand) + and not isinstance(gripper, FourierRightHand) + and not isinstance(gripper, G1ThreeFingerLeftHand) + and not isinstance(gripper, G1ThreeFingerRightHand) + ): + continue + + eef = find_elements( + root=worldbody, + tags="body", + attribs={"name": f"gripper0_{arm}_eef"}, + return_first=True, + ) + mount = find_parent(worldbody, eef) + for i in range(4): + elem = find_elements( + root=worldbody, + tags="site", + attribs={"name": f"robot0_{arm}_pinch_spheres_{i}"}, + return_first=True, + ) + if elem is not None: + continue + if ( + isinstance(gripper, InspireLeftHand) + or isinstance(gripper, InspireRightHand) + or isinstance(gripper, FourierLeftHand) + or isinstance(gripper, FourierRightHand) + ): + mount.append( + new_element( + tag="site", + name=f"robot0_{arm}_pinch_spheres_{i}", + pos=f"-0.09 -0.10 {-0.03 + i * 0.02}", + size="0.01 0.01 0.01", + rgba="1 0.5 0.5 1", + type="sphere", + group="1", + ) + ) + else: + if arm == "right": + mount.append( + new_element( + tag="site", + name=f"robot0_{arm}_pinch_spheres_{i}", + pos=f"0.15 0.05 {-0.03 + i * 0.02}", + size="0.01 0.01 0.01", + rgba="1 0.5 0.5 1", + type="sphere", + group="1", + ) + ) + else: + mount.append( + new_element( + tag="site", + name=f"robot0_{arm}_pinch_spheres_{i}", + pos=f"0.15 -0.05 {-0.03 + i * 0.02}", + size="0.01 0.01 0.01", + rgba="1 0.5 0.5 1", + type="sphere", + group="1", + ) + ) + + for indicator_config in self.indicator_configs: + config = deepcopy(indicator_config) + + from robosuite.utils.mjcf_utils import find_elements + + body = find_elements( + root=worldbody, + tags="body", + attribs={"name": config["name"] + "_body"}, + ) + if body is not None: + continue + + indicator_body = new_body(name=config["name"] + "_body") + indicator_body.append(new_site(**config)) + worldbody.append(indicator_body) + + xml = ET.tostring(root, encoding="utf8").decode("utf8") + + return xml diff --git a/gr00t_wbc/dexmg/gr00trobocasa/setup.py b/gr00t_wbc/dexmg/gr00trobocasa/setup.py new file mode 100644 index 0000000..f21922b --- /dev/null +++ b/gr00t_wbc/dexmg/gr00trobocasa/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + + +setup( + name="robocasa", + packages=[package for package in find_packages() if package.startswith("robocasa")], + install_requires=[ + "numpy==1.26.4", + "numba==0.61.0", + "scipy>=1.2.3", + "mujoco==3.2.6", + "pygame", + "Pillow", + "opencv-python", + "pyyaml", + "pynput", + "tqdm", + "termcolor", + "imageio", + "h5py", + "lxml", + "hidapi", + "tianshou==0.4.10", + ], + eager_resources=["*"], + include_package_data=True, + python_requires=">=3", + description="Gr00t RoboCasa for loco-manipulation", + version="0.2.0", +) diff --git a/gr00t_wbc/sim2mujoco/.gitattributes b/gr00t_wbc/sim2mujoco/.gitattributes new file mode 100644 index 0000000..c94badc --- /dev/null +++ b/gr00t_wbc/sim2mujoco/.gitattributes @@ -0,0 +1 @@ +*.stl filter=lfs diff=lfs merge=lfs -text diff --git a/gr00t_wbc/sim2mujoco/.gitignore b/gr00t_wbc/sim2mujoco/.gitignore new file mode 100644 index 0000000..c01bd8c --- /dev/null +++ b/gr00t_wbc/sim2mujoco/.gitignore @@ -0,0 +1 @@ +*/__pycache__/ diff --git a/gr00t_wbc/sim2mujoco/README.md b/gr00t_wbc/sim2mujoco/README.md new file mode 100644 index 0000000..588b2a4 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/README.md @@ -0,0 +1,5 @@ +urdf to mjcf: +``` +pip install urdf2mjcf +urdf2mjcf path_to_urdf +``` \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/requirements.txt b/gr00t_wbc/sim2mujoco/requirements.txt new file mode 100644 index 0000000..40bc58a --- /dev/null +++ b/gr00t_wbc/sim2mujoco/requirements.txt @@ -0,0 +1,8 @@ +mujoco==3.3.4 +matplotlib==3.10.3 +numpy==2.2.6 +trimesh +ipdb +open3d +onnxruntime +keyboard \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/README.md b/gr00t_wbc/sim2mujoco/resources/robots/g1/README.md new file mode 100644 index 0000000..6fe48ce --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/README.md @@ -0,0 +1,30 @@ +# Unitree G1 Description (URDF & MJCF) + +## Overview + +This package includes a universal humanoid robot description (URDF & MJCF) for the [Unitree G1](https://www.unitree.com/g1/), developed by [Unitree Robotics](https://www.unitree.com/). + +MJCF/URDF for the G1 robot: + +| MJCF/URDF file name | `mode_machine` | Hip roll reduction ratio | Update status | dof#leg | dof#waist | dof#arm | dof#hand | +| ----------------------------- | :------------: | :----------------------: | ------------- | :-----: | :-------: | :-----: | :------: | +| `g1_23dof` | 1 | 14.5 | Beta | 6*2 | 1 | 5*2 | 0 | +| `g1_29dof` | 2 | 14.5 | Beta | 6*2 | 3 | 7*2 | 0 | +| `g1_29dof_with_hand` | 2 | 14.5 | Beta | 6*2 | 3 | 7*2 | 7*2 | +| `g1_29dof_lock_waist` | 3 | 14.5 | Beta | 6*2 | 1 | 7*2 | 0 | +| `g1_23dof_rev_1_0` | 4 | 22.5 | Up-to-date | 6*2 | 1 | 5*2 | 0 | +| `g1_29dof_rev_1_0` | 5 | 22.5 | Up-to-date | 6*2 | 3 | 7*2 | 0 | +| `g1_29dof_with_hand_rev_1_0` | 5 | 22.5 | Up-to-date | 6*2 | 3 | 7*2 | 7*2 | +| `g1_29dof_lock_waist_rev_1_0` | 6 | 22.5 | Up-to-date | 6*2 | 1 | 7*2 | 0 | +| `g1_dual_arm` | 9 | null | Up-to-date | 0 | 0 | 7*2 | 0 | + +## Visulization with [MuJoCo](https://github.com/google-deepmind/mujoco) + +1. Open MuJoCo Viewer + + ```bash + pip install mujoco + python -m mujoco.viewer + ``` + +2. Drag and drop the MJCF/URDF model file (`g1_XXX.xml`/`g1_XXX.urdf`) to the MuJoCo Viewer. diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.urdf b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.urdf new file mode 100644 index 0000000..aae5441 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.urdf @@ -0,0 +1,1262 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.xml b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.xml new file mode 100644 index 0000000..0e6fd21 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.xml @@ -0,0 +1,429 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.yaml b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.yaml new file mode 100644 index 0000000..4a5d5cb --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1.yaml @@ -0,0 +1,34 @@ +# Policy and model paths +policy_path: "policy/deploy.onnx" +xml_path: "g1.xml" + +# Simulation parameters +simulation_duration: 1000.0 +simulation_dt: 0.002 +control_decimation: 10 + +# PD gains +# PD gains +kps: [150, 150, 150, 200, 40, 40, + 150, 150, 150, 200, 40, 40] +kds: [2.0, 2.0, 2.0, 4.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 4.0, 2.0, 2.0] + +# Default joint angles for legs +default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + -0.1, 0.0, 0.0, 0.3, -0.2, 0.0] +# Scaling factors +ang_vel_scale: 0.25 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +action_scale: 0.25 +cmd_scale: [2.0, 2.0, 0.25] + +# Number of actions and observations +num_actions: 12 +num_obs: 456 # 76 * 6 (observation dimension * history length) +obs_history_len: 6 + +# Initial commands +cmd_init: [0.0, 0.0, 0.0] +height_cmd: 0.74 \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.xml b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.xml new file mode 100644 index 0000000..b5879a3 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.xml @@ -0,0 +1,415 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml new file mode 100644 index 0000000..d629c83 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml @@ -0,0 +1,40 @@ +# Policy and model paths +policy_path: "policy/ft92.onnx" #best ft 49 71? 75 +walk_policy_path: "policy/ft109.onnx" #best ft 49 71? 75 +xml_path: "g1_gear_wbc.xml" + +# Simulation parameters +simulation_duration: 1000.0 +simulation_dt: 0.005 +control_decimation: 4 + +# PD gains +# PD gains +kps: [150, 150, 150, 200, 40, 40, + 150, 150, 150, 200, 40, 40, + 250.0, 250.0, 250.0] +kds: [2.0, 2.0, 2.0, 4.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 4.0, 2.0, 2.0, + 5.0, 5.0, 5.0] + +# Default joint angles for legs +default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + -0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + 0.0, 0.0, 0.0] +# Scaling factors +ang_vel_scale: 0.5 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +action_scale: 0.25 +cmd_scale: [2.0, 2.0, 0.5] + +# Number of actions and observations +num_actions: 15 +num_obs: 516 # 516 # 76 * 6 (observation dimension * history length) +obs_history_len: 6 + +# Initial commands +cmd_init: [0.0, 0.0, 0.0] +height_cmd: 0.74 +rpy_cmd: [0.0, 0.0, 0.0] +freq_cmd: 0.75 \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/head_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/head_link.STL new file mode 100644 index 0000000..401f822 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/head_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:005fb67fbd3eff94aa8bf4a6e83238174e9f91b6721f7111594322f223724411 +size 932784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 0000000..7cd7052 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d49e3abc6f5b12e532062cd575b87b5ef40cd2a3fc18f54a1ca5bba4f773d51d +size 71184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 0000000..cb69f65 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4092af943141d4d9f74232f3cfa345afc6565f46a077793b8ae0e68b39dc33f +size 653384 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_elbow_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_elbow_link.STL new file mode 100644 index 0000000..7fa0527 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fa752198accd104d5c4c3a01382e45165b944fbbc5acce085059223324e5bed3 +size 88784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_0_link.STL new file mode 100644 index 0000000..a87568d --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_1_link.STL new file mode 100644 index 0000000..c6c91dd --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_palm_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_palm_link.STL new file mode 100644 index 0000000..c8fcc3e --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:23a486b75bd78a9bf03cec25d84d87f97f3dae038cf21a743b6d469b337e4004 +size 2140184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_0_link.STL new file mode 100644 index 0000000..7fb38c1 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a90c721661c0622685488a3c74d1e122c7da89242d3a1daef75edb83422d05e0 +size 8884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_1_link.STL new file mode 100644 index 0000000..54f7754 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:445c54a45bc13ce36001556f66bc0f49c83cb40321205ae4d676bb2874325684 +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_2_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_2_link.STL new file mode 100644 index 0000000..3e4f124 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3d8dbe5085acfc213d21aa8b0782e89cd79084e9678f3a85fc7b04a86b029db5 +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000..4cf7475 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4725168105ee768ee31638ef22b53f6be2d7641bfd7cfefe803488d884776fa4 +size 181684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_roll_link.STL new file mode 100644 index 0000000..585f604 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:91f25922ee8a7c3152790051bebad17b4d9cd243569c38fe340285ff93a97acf +size 192184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 0000000..b46a741 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a16d88aa6ddac8083aa7ad55ed317bea44b1fa003d314fba88965b7ed0f3b55b +size 296284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_knee_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_knee_link.STL new file mode 100644 index 0000000..2dcf84e --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d92b9e3d3a636761150bb8025e32514c4602b91c7028d523ee42b3e632de477 +size 854884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_rubber_hand.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_rubber_hand.STL new file mode 100644 index 0000000..04a2fa2 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cff2221a690fa69303f61fce68f2d155c1517b52efb6ca9262dd56e0bc6e70fe +size 2287484 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 0000000..926d980 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0d1cfd02fcf0d42f95e678eeca33da3afbcc366ffba5c052847773ec4f31d52 +size 176784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 0000000..4c6840b --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fb9df21687773522598dc384f1a2945c7519f11cbc8bd372a49170316d6eee88 +size 400284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 0000000..89b0e06 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1aa97e9748e924336567992181f78c7cd0652fd52a4afcca3df6b2ef6f9e712e +size 249184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 0000000..f9c9e4f --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b251d8e05047f695d0f536cd78c11973cfa4e78d08cfe82759336cc3471de3a9 +size 85984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 0000000..2097ca3 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:edc387c9a0ba8c2237e9b296d32531426fabeb6f53e58df45c76106bca74148c +size 356184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_rubber_hand.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..7c58819 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e81030abd023bd9e4a308ef376d814a2c12d684d8a7670c335bbd5cd7809c909 +size 3484884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 0000000..692f4b0 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/left_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:83f8fb3a726bf9613d65dd14f0f447cb918c3c95b3938042a0c9c09749267d3b +size 318684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/logo_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/logo_link.STL new file mode 100644 index 0000000..6c25961 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/logo_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8571a0a19bc4916fa55f91449f51d5fdefd751000054865a842449429d5f155b +size 243384 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis.STL new file mode 100644 index 0000000..f98a88d --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ba6bbc888e630550140d3c26763f10206da8c8bd30ed886b8ede41c61f57a31 +size 1060884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis_contour_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis_contour_link.STL new file mode 100644 index 0000000..8025bc0 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/pelvis_contour_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5cc5c2c7a312329e3feeb2b03d3fc09fc29705bd01864f6767e51be959662420 +size 1805184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 0000000..94a586a --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:15be426539ec1be70246d4d82a168806db64a41301af8b35c197a33348c787a9 +size 71184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 0000000..3c38507 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b66222ea56653e627711b56d0a8949b4920da5df091da0ceb343f54e884e3a5 +size 653784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_elbow_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_elbow_link.STL new file mode 100644 index 0000000..52aa0eb --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_elbow_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1be925d7aa268bb8fddf5362b9173066890c7d32092c05638608126e59d1e2ab +size 88784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_index_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_0_link.STL new file mode 100644 index 0000000..5e2ef47 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_1_link.STL new file mode 100644 index 0000000..8572ae1 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_middle_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_palm_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_palm_link.STL new file mode 100644 index 0000000..fd2f7f0 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:86c0b231cc44477d64a6493e5a427ba16617a00738112dd187c652675b086fb9 +size 2140184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_0_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_0_link.STL new file mode 100644 index 0000000..a385d96 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_0_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:544298d0ea1088f5b276a10cc6a6a9e533efdd91594955fdc956c46211d07f83 +size 8884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_1_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_1_link.STL new file mode 100644 index 0000000..c118de7 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0a9a820da8dd10f298778b714f1364216e8a5976f4fd3a05689ea26327d44bf6 +size 475984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_2_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_2_link.STL new file mode 100644 index 0000000..0979fb6 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hand_thumb_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3f1bfb37668e8f61801c8d25f171fa1949e08666be86c67acad7e0079937cc45 +size 1521784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 0000000..064085f --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e4f3c99d7f4a7d34eadbef9461fc66e3486cb5442db1ec50c86317d459f1a9c6 +size 181284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_roll_link.STL new file mode 100644 index 0000000..6544025 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4c254ef66a356f492947f360dd931965477b631e3fcc841f91ccc46d945d54f6 +size 192684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 0000000..0ad7bee --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e479c2936ca2057e9eb2f7dff6c189b7419d7b8484dea0b298cbb36a2a6aa668 +size 296284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_knee_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_knee_link.STL new file mode 100644 index 0000000..65e8a70 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:63c4008449c9bbe701a6e2b557b7a252e90cf3a5abcf54cee46862b9a69f8ec8 +size 852284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_rubber_hand.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_rubber_hand.STL new file mode 100644 index 0000000..58148fb --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:99533b778bca6246144fa511bb9d4e555e075c641f2a0251e04372869cd99d67 +size 2192684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 0000000..48a1c46 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24cdb387e0128dfe602770a81c56cdce3a0181d34d039a11d1aaf8819b7b8c02 +size 176784 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 0000000..2a5d22f --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:962b97c48f9ce9e8399f45dd9522e0865d19aa9fd299406b2d475a8fc4a53e81 +size 401884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 0000000..0882a56 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a0b76489271da0c72461a344c9ffb0f0c6e64f019ea5014c1624886c442a2fe5 +size 249984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_pitch_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 0000000..2efd25c --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d22f8f3b3127f15a63e5be1ee273cd5075786c3142f1c3d9f76cbf43d2a26477 +size 79584 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 0000000..77d23a7 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a7ee9212ff5b94d6cb7f52bb1bbf3f352194d5b598acff74f4c77d340c5b344f +size 356084 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_rubber_hand.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_rubber_hand.STL new file mode 100644 index 0000000..6f122af --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_roll_rubber_hand.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0729aff1ac4356f9314de13a46906267642e58bc47f0d8a7f17f6590a6242ccf +size 3481584 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 0000000..77edbb4 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/right_wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc9dece2d12509707e0057ba2e48df8f3d56db0c79410212963a25e8a50f61a6 +size 341484 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_link.STL new file mode 100644 index 0000000..cc2cbbf --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:82be7f93e85b3d303a1d1e1847e2c916939bd61c424ed1ebd28691ec33909dd1 +size 203584 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_rod_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_rod_link.STL new file mode 100644 index 0000000..dd439bf --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_L_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c10de1effa7ea797ac268006aa2a739036c7e1f326b2012d711ee2c20c5a6e96 +size 74884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_link.STL new file mode 100644 index 0000000..422ffe4 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:54ded433a3a0c76027365856fdbd55215643de88846f7d436598a4071e682725 +size 203584 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_rod_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_rod_link.STL new file mode 100644 index 0000000..6df46c8 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_constraint_R_rod_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cb83fd38a9f06c99e3f301c70f12d94ae770a8f0cf9501f83580a27f908990b4 +size 74884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link.STL new file mode 100644 index 0000000..e4fb87c --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e96d023f0368a4e3450b86ca5d4f10227d8141156a373e7da8cb3c93266523e0 +size 2232984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_23dof_rev_1_0.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_23dof_rev_1_0.STL new file mode 100644 index 0000000..edaf96a --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_23dof_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3cd0d56fde14b73c1623304684805029971c4f84b596f9914e823ca70a107fd2 +size 7825434 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_rev_1_0.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_rev_1_0.STL new file mode 100644 index 0000000..836b992 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/torso_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11ddb46f2098efbbd8816b1d65893632d8e78be936376c7cdcd6771899ccc723 +size 2570584 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_L.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_L.STL new file mode 100644 index 0000000..6ec689b --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ebafdcb4de6871113f0ca2c356618d6e46b1d50f6c0bf9e37f47b9d8e100d99 +size 114684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_R.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_R.STL new file mode 100644 index 0000000..69fd76a --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_constraint_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:791902a291ffbd35ab97383b7b44ea5d975de7c80eef838797c970790b382ca9 +size 114684 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link.STL new file mode 100644 index 0000000..007f56d --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34f0aa73f41131230be4d25876c944fdf6c24d62553f199ff8b980c15e8913df +size 24184 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link_rev_1_0.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link_rev_1_0.STL new file mode 100644 index 0000000..36a5a70 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_roll_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b67c347a05abc3e8ddae600d98a082bee273bb39f8e651647708b0a7140a8a97 +size 85884 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_support_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_support_link.STL new file mode 100644 index 0000000..4a50f94 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_support_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fae9e1bb609848a1667d32eed8d6083ae443538a306843056a2a660f1b2926a +size 150484 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link.STL new file mode 100644 index 0000000..c049deb --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2883f20e03f09b669b5b4ce10677ee6b5191c0934b584d7cbaef2d0662856ffb +size 336284 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link_rev_1_0.STL b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link_rev_1_0.STL new file mode 100644 index 0000000..dc628fb --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/meshes/waist_yaw_link_rev_1_0.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec6db442b11f25eed898b5add07940c85d804f300de24dcbd264ccd8be7d554c +size 619984 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Balance.onnx b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Balance.onnx new file mode 100644 index 0000000..425a905 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Balance.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f645da599d4ca3d29ed273c8f4712620bb680d34977469ca3aeabe5bb9631c18 +size 1886682 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Walk.onnx b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Walk.onnx new file mode 100644 index 0000000..d66452c --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/GR00T-WholeBodyControl-Walk.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7c82255b6905ffcc4468fa7f8ddcf7b70db168cf1042107ccab887cb6a8e5407 +size 1886682 diff --git a/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/NVIDIA Open Model License b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/NVIDIA Open Model License new file mode 100644 index 0000000..0fd2c70 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/resources/robots/g1/policy/NVIDIA Open Model License @@ -0,0 +1,62 @@ +NVIDIA Open Model License Agreement + +Last Modified: October 24, 2025 + +This NVIDIA Open Model License Agreement (the “Agreement”) is a legal agreement between the Legal Entity You represent, or if no entity is identified, You and NVIDIA Corporation and its Affiliates (“NVIDIA”) and governs Your use of the Models that NVIDIA provides to You under this Agreement. NVIDIA and You are each a “party” and collectively the “parties.” + +NVIDIA models released under this Agreement are intended to be used permissively and enable the further development of AI technologies. Subject to the terms of this Agreement, NVIDIA confirms that: + +Models are commercially usable. +You are free to create and distribute Derivative Models. +NVIDIA does not claim ownership to any outputs generated using the Models or Derivative Models. +By using, reproducing, modifying, distributing, performing or displaying any portion or element of the Model or Derivative Model, or otherwise accepting the terms of this Agreement, you agree to be bound by this Agreement. + +Definitions. The following definitions apply to this Agreement: + +"Derivative Model" means all (a) modifications to the Model, (b) works based on the Model, and (c) any other derivative works of the Model. An output is not a Derivative Model. + +"Legal Entity" means the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (a) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (b) ownership of fifty percent (50%) or more of the outstanding shares, or (c) beneficial ownership of such entity. + +“Model” means the machine learning model, software, checkpoints, learnt weights, algorithms, parameters, configuration files and documentation shared under this Agreement. + +"NVIDIA Cosmos Model" means a multimodal Model shared under this Agreement + +"Special-Purpose Model" means a Model that is only competent in a narrow set of purpose-specific tasks and should not be used for unintended or general-purpose applications + +“You” or “Your” means an individual or Legal Entity exercising permissions granted by this Agreement. + +Conditions for Use, License Grant, AI Ethics and IP Ownership. + +Conditions for Use. The Model and any Derivative Model are subject to additional terms as described in Section 2 and Section 3 of this Agreement and govern Your use. If You institute copyright or patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Model or a Derivative Model constitutes direct or contributory copyright or patent infringement, then any licenses granted to You under this Agreement for that Model or Derivative Model will terminate as of the date such litigation is filed. If You bypass, disable, reduce the efficacy of, or circumvent any technical limitation, safety guardrail or associated safety guardrail hyperparameter, encryption, security, digital rights management, or authentication mechanism (collectively “Guardrail”) contained in the Model without a substantially similar Guardrail appropriate for your use case, your rights under this Agreement will automatically terminate. NVIDIA may indicate in relevant documentation that a Model is a Special-Purpose Model. NVIDIA may update this Agreement to comply with legal and regulatory requirements at any time and You agree to either comply with any updated license or cease Your copying, use, and distribution of the Model and any Derivative Model. + +License Grant. The rights granted herein are explicitly conditioned on Your full compliance with the terms of this Agreement. Subject to the terms and conditions of this Agreement, NVIDIA hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, revocable (as stated in Section 2.1) license to publicly perform, publicly display, reproduce, use, create derivative works of, make, have made, sell, offer for sale, distribute (through multiple tiers of distribution) and import the Model. + +AI Ethics. Use of the Models under the Agreement must be consistent with NVIDIA’s Trustworthy AI terms found at https://www.nvidia.com/en-us/agreements/trustworthy-ai/terms/. + +NVIDIA owns the Model and any Derivative Models created by NVIDIA. Subject to NVIDIA’s underlying ownership rights in the Model or its Derivative Models, You are and will be the owner of Your Derivative Models. NVIDIA claims no ownership rights in outputs. You are responsible for outputs and their subsequent uses. Except as expressly granted in this Agreement, (a) NVIDIA reserves all rights, interests and remedies in connection with the Model and (b) no other license or right is granted to you by implication, estoppel or otherwise. + +Redistribution. You may reproduce and distribute copies of the Model or Derivative Models thereof in any medium, with or without modifications, provided that You meet the following conditions: + +If you distribute the Model, You must give any other recipients of the Model a copy of this Agreement and include the following attribution notice within a “Notice” text file with such copies: “Licensed by NVIDIA Corporation under the NVIDIA Open Model License”; + +If you distribute or make available a NVIDIA Cosmos Model, or a product or service (including an AI model) that contains or uses a NVIDIA Cosmos Model, use a NVIDIA Cosmos Model to create a Derivative Model, or use a NVIDIA Cosmos Model or its outputs to create, train, fine tune, or otherwise improve an AI model, you will include “Built on NVIDIA Cosmos” on a related website, user interface, blogpost, about page, or product documentation; and + +You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Models as a whole, provided Your use, reproduction, and distribution of the Model otherwise complies with the conditions stated in this Agreement. + +Separate Components. The Models may include or be distributed with components provided with separate legal notices or terms that accompany the components, such as an Open Source Software License or other third-party license. The components are subject to the applicable other licenses, including any proprietary notices, disclaimers, requirements and extended use rights; except that this Agreement will prevail regarding the use of third-party Open Source Software License, unless a third-party Open Source Software License requires its license terms to prevail. “Open Source Software License” means any software, data or documentation subject to any license identified as an open source license by the Open Source Initiative (https://opensource.org), Free Software Foundation (https://www.fsf.org) or other similar open source organization or listed by the Software Package Data Exchange (SPDX) Workgroup under the Linux Foundation (https://www.spdx.org). + +Trademarks. This Agreement does not grant permission to use the trade names, trademarks, service marks, or product names of NVIDIA, except as required for reasonable and customary use in describing the origin of the Model and reproducing the content of the “Notice” text file. + +Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, NVIDIA provides the Model on an “AS IS” BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for reviewing Model documentation, including any Special-Purpose Model limitations, and determining the appropriateness of using or redistributing the Model, Derivative Models and outputs. You assume any risks associated with Your exercise of permissions under this Agreement. + +Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, will NVIDIA be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this Agreement or out of the use or inability to use the Model, Derivative Models or outputs (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if NVIDIA has been advised of the possibility of such damages. + +Indemnity. You will indemnify and hold harmless NVIDIA from and against any claim by any third party arising out of or related to your use or distribution of the Model, Derivative Models or outputs. + +Feedback. NVIDIA appreciates your feedback, and You agree that NVIDIA may use it without restriction or compensation to You. + +Governing Law. This Agreement will be governed in all respects by the laws of the United States and the laws of the State of Delaware, without regard to conflict of laws principles or the United Nations Convention on Contracts for the International Sale of Goods. The state and federal courts residing in Santa Clara County, California will have exclusive jurisdiction over any dispute or claim arising out of or related to this Agreement, and the parties irrevocably consent to personal jurisdiction and venue in those courts; except that, either party may apply for injunctive remedies or an equivalent type of urgent legal relief in any jurisdiction. + +Trade and Compliance. You agree to comply with all applicable export, import, trade and economic sanctions laws and regulations, as amended, including without limitation U.S. Export Administration Regulations and Office of Foreign Assets Control regulations. These laws include restrictions on destinations, end-users and end-use. + +Version Release Date: October 24, 2025 \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc.py b/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc.py new file mode 100644 index 0000000..6d5b859 --- /dev/null +++ b/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc.py @@ -0,0 +1,307 @@ +import sys +import time +import collections +import yaml +import torch +import numpy as np +import sys +import time +import collections +import yaml +import torch +import numpy as np +import mujoco +import mujoco.viewer +import onnxruntime as ort +import threading +from pynput import keyboard as pkb +import os + +class GearWbcController: + def __init__(self, config_path): + self.CONFIG_PATH = config_path + self.cmd_lock = threading.Lock() + self.config = self.load_config(os.path.join(self.CONFIG_PATH, "g1_gear_wbc.yaml")) + + self.control_dict = { + 'loco_cmd': self.config['cmd_init'], + "height_cmd": self.config['height_cmd'], + "rpy_cmd": self.config.get('rpy_cmd', [0.0, 0.0, 0.0]), + "freq_cmd": self.config.get('freq_cmd', 1.5) + } + + self.model = mujoco.MjModel.from_xml_path(self.config['xml_path']) + self.data = mujoco.MjData(self.model) + self.model.opt.timestep = self.config['simulation_dt'] + self.n_joints = self.data.qpos.shape[0] - 7 + self.torso_index = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso_link") + self.base_index = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis") + self.action = np.zeros(self.config['num_actions'], dtype=np.float32) + self.target_dof_pos = self.config['default_angles'].copy() + self.policy = self.load_onnx_policy(self.config['policy_path']) + self.walk_policy = self.load_onnx_policy(self.config['walk_policy_path']) + self.gait_indices = torch.zeros((1), dtype=torch.float32) + self.counter = 0 + self.just_started = 0.0 + self.walking_mask = False + self.frozen_FL = False + self.frozen_FR = False + self.single_obs, self.single_obs_dim = self.compute_observation( + self.data, self.config, self.action, self.control_dict, self.n_joints + ) + self.obs_history = collections.deque( + [np.zeros(self.single_obs_dim, dtype=np.float32)] * self.config['obs_history_len'], + maxlen=self.config['obs_history_len'] + ) + self.obs = np.zeros(self.config['num_obs'], dtype=np.float32) + self.keyboard_listener(self.control_dict, self.config) + + + def keyboard_listener(self, control_dict, config): + """Listen to key press events and update cmd and height_cmd""" + def on_press(key): + try: + k = key.char + except AttributeError: + return # Special keys ignored + + with self.cmd_lock: + if k == 'w': + control_dict['loco_cmd'][0] += 0.1 + elif k == 's': + control_dict['loco_cmd'][0] -= 0.1 + elif k == 'a': + control_dict['loco_cmd'][1] += 0.1 + elif k == 'd': + control_dict['loco_cmd'][1] -= 0.1 + elif k == 'q': + control_dict['loco_cmd'][2] += 0.1 + elif k == 'e': + control_dict['loco_cmd'][2] -= 0.1 + elif k == 'z': + control_dict['loco_cmd'][:] = config['cmd_init'] + control_dict["height_cmd"] = config['height_cmd'] + control_dict['rpy_cmd'][:] = config['rpy_cmd'] + control_dict['freq_cmd'] = config['freq_cmd'] + elif k == '1': + control_dict["height_cmd"] += 0.05 + elif k == '2': + control_dict["height_cmd"] -= 0.05 + elif k == '3': + control_dict['rpy_cmd'][0] += 0.2 + elif k == '4': + control_dict['rpy_cmd'][0] -= 0.2 + elif k == '5': + control_dict['rpy_cmd'][1] += 0.2 + elif k == '6': + control_dict['rpy_cmd'][1] -= 0.2 + elif k == '7': + control_dict['rpy_cmd'][2] += 0.2 + elif k == '8': + control_dict['rpy_cmd'][2] -= 0.2 + elif k == 'm': + control_dict['freq_cmd'] += 0.1 + elif k == 'n': + control_dict['freq_cmd'] -= 0.1 + + print(f"Current Commands: loco_cmd = {control_dict['loco_cmd']}, height_cmd = {control_dict['height_cmd']}, rpy_cmd = {control_dict['rpy_cmd']}, freq_cmd = {control_dict['freq_cmd']}") + + listener = pkb.Listener(on_press=on_press) + listener.daemon = True + listener.start() + + def load_config(self, config_path): + with open(config_path, 'r') as f: + config = yaml.safe_load(f) + + for path_key in ['policy_path', 'xml_path', 'walk_policy_path']: + config[path_key] = os.path.join(CONFIG_PATH, config[path_key]) + + array_keys = ['kps', 'kds', 'default_angles', 'cmd_scale', 'cmd_init'] + for key in array_keys: + config[key] = np.array(config[key], dtype=np.float32) + + return config + + def pd_control(self, target_q, q, kp, target_dq, dq, kd): + return (target_q - q) * kp + (target_dq - dq) * kd + + def quat_rotate_inverse(self, q, v): + w, x, y, z = q + q_conj = np.array([w, -x, -y, -z]) + return np.array([ + v[0] * (q_conj[0]**2 + q_conj[1]**2 - q_conj[2]**2 - q_conj[3]**2) + + v[1] * 2 * (q_conj[1]*q_conj[2] - q_conj[0]*q_conj[3]) + + v[2] * 2 * (q_conj[1]*q_conj[3] + q_conj[0]*q_conj[2]), + v[0] * 2 * (q_conj[1]*q_conj[2] + q_conj[0]*q_conj[3]) + + v[1] * (q_conj[0]**2 - q_conj[1]**2 + q_conj[2]**2 - q_conj[3]**2) + + v[2] * 2 * (q_conj[2]*q_conj[3] - q_conj[0]*q_conj[1]), + v[0] * 2 * (q_conj[1]*q_conj[3] - q_conj[0]*q_conj[2]) + + v[1] * 2 * (q_conj[2]*q_conj[3] + q_conj[0]*q_conj[1]) + + v[2] * (q_conj[0]**2 - q_conj[1]**2 - q_conj[2]**2 + q_conj[3]**2) + ]) + + def get_gravity_orientation(self, quat): + gravity_vec = np.array([0.0, 0.0, -1.0]) + return self.quat_rotate_inverse(quat, gravity_vec) + + def compute_observation(self, d, config, action, control_dict, n_joints): + command = np.zeros(7, dtype=np.float32) + command[:3] = control_dict['loco_cmd'][:3] * config['cmd_scale'] + command[3] = control_dict['height_cmd'] + # command[4] = control_dict['freq_cmd'] + command[4:7] = control_dict['rpy_cmd'] + + # # gait indice + # is_static = np.linalg.norm(command[:3]) < 0.1 + # just_entered_walk = (not is_static) and (not self.walking_mask) + # self.walking_mask = not is_static + + # if just_entered_walk: + # self.just_started = 0.0 + # self.gait_indices = torch.tensor([-0.25]) + # if not is_static: + # self.just_started += 0.02 + # else: + # self.just_started = 0.0 + + # if not is_static: + # self.frozen_FL = False + # self.frozen_FR = False + + # self.gait_indices = torch.remainder(self.gait_indices + 0.02 * command[4], 1.0) + + # # Parameters + # duration = 0.5 + # phase = 0.5 + + # # Gait indices + # gait_FR = self.gait_indices.clone() + # gait_FL = torch.remainder(gait_FR + phase, 1.0) + + # if self.just_started < (0.5 / command[4]): + # gait_FR = torch.tensor([0.25]) + # gait_pair = [gait_FL.clone(), gait_FR.clone()] + + # for i, fi in enumerate(gait_pair): + # if fi.item() < duration: + # gait_pair[i] = fi * (0.5 / duration) + # else: + # gait_pair[i] = 0.5 + (fi - duration) * (0.5 / (1 - duration)) + + # # Clock signal + # clock = [torch.sin(2 * np.pi * fi) for fi in gait_pair] + + + # for i, (clk, frozen_mask_attr) in enumerate( + # zip(clock, ['frozen_FL', 'frozen_FR']) + # ): + # frozen_mask = getattr(self, frozen_mask_attr) + # # Freeze condition: static and at sin peak + # if is_static and (not frozen_mask) and clk.item() > 0.98: + # setattr(self, frozen_mask_attr, True) + # clk = torch.tensor([1.0]) + # if getattr(self, frozen_mask_attr): + # clk = torch.tensor([1.0]) + # clock[i] = clk + + # self.clock_inputs = torch.stack(clock).unsqueeze(0) + qj = d.qpos[7:7+n_joints].copy() + dqj = d.qvel[6:6+n_joints].copy() + quat = d.qpos[3:7].copy() + omega = d.qvel[3:6].copy() + # omega = self.data.xmat[self.base_index].reshape(3, 3).T @ self.data.cvel[self.base_index][3:6] + padded_defaults = np.zeros(n_joints, dtype=np.float32) + L = min(len(config['default_angles']), n_joints) + padded_defaults[:L] = config['default_angles'][:L] + + qj_scaled = (qj - padded_defaults) * config['dof_pos_scale'] + dqj_scaled = dqj * config['dof_vel_scale'] + gravity_orientation = self.get_gravity_orientation(quat) + omega_scaled = omega * config['ang_vel_scale'] + + torso_quat = self.data.xquat[self.torso_index] + torso_omega = self.data.xmat[self.torso_index].reshape(3, 3).T @ self.data.cvel[self.torso_index][3:6] + torso_omega_scaled = torso_omega * config['ang_vel_scale'] + torso_gravity_orientation = self.get_gravity_orientation(torso_quat) + + single_obs_dim = 86 + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:7] = command[:7] + single_obs[7:10] = omega_scaled + single_obs[10:13] = gravity_orientation + # single_obs[14:17] = 0.#torso_omega_scaled + # single_obs[17:20] = 0.#torso_gravity_orientation + single_obs[13:13+n_joints] = qj_scaled + single_obs[13+n_joints:13+2*n_joints] = dqj_scaled + single_obs[13+2*n_joints:13+2*n_joints+15] = action + # single_obs[20+2*n_joints+15:20+2*n_joints+15+2] = self.clock_inputs.cpu().numpy().reshape(2) + + return single_obs, single_obs_dim + + def load_onnx_policy(self, path): + model = ort.InferenceSession(path) + def run_inference(input_tensor): + ort_inputs = {model.get_inputs()[0].name: input_tensor.cpu().numpy()} + ort_outs = model.run(None, ort_inputs) + return torch.tensor(ort_outs[0], device="cuda:0") + return run_inference + + def run(self): + + self.counter = 0 + + with mujoco.viewer.launch_passive(self.model, self.data) as viewer: + start = time.time() + while viewer.is_running() and time.time() - start < self.config['simulation_duration']: + step_start = time.time() + + leg_tau = self.pd_control( + self.target_dof_pos, + self.data.qpos[7:7+self.config['num_actions']], + self.config['kps'], + np.zeros_like(self.config['kps']), + self.data.qvel[6:6+self.config['num_actions']], + self.config['kds'] + ) + self.data.ctrl[:self.config['num_actions']] = leg_tau + + if self.n_joints > self.config['num_actions']: + arm_tau = self.pd_control( + np.zeros(self.n_joints - self.config['num_actions'], dtype=np.float32), + self.data.qpos[7+self.config['num_actions']:7+self.n_joints], + np.full(self.n_joints - self.config['num_actions'], 100.0), + np.zeros(self.n_joints - self.config['num_actions']), + self.data.qvel[6+self.config['num_actions']:6+self.n_joints], + np.full(self.n_joints - self.config['num_actions'], 0.5) + ) + self.data.ctrl[self.config['num_actions']:] = arm_tau + + mujoco.mj_step(self.model, self.data) + + self.counter += 1 + if self.counter % self.config['control_decimation'] == 0: + with self.cmd_lock: + current_cmd = self.control_dict + + single_obs, _ = self.compute_observation(self.data, self.config, self.action, current_cmd, self.n_joints) + self.obs_history.append(single_obs) + + for i, hist_obs in enumerate(self.obs_history): + self.obs[i * self.single_obs_dim:(i + 1) * self.single_obs_dim] = hist_obs + + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + if (np.linalg.norm(np.array(current_cmd['loco_cmd']))) <= 0.05: + self.action = self.policy(obs_tensor).cpu().detach().numpy().squeeze() + else: + self.action = self.walk_policy(obs_tensor).cpu().detach().numpy().squeeze() + self.target_dof_pos = self.action * self.config['action_scale'] + self.config['default_angles'] + + viewer.sync() + # time.sleep(max(0, self.model.opt.timestep - (time.time() - step_start))) + + +if __name__ == "__main__": + CONFIG_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "resources", "robots", "g1") + controller = GearWbcController(CONFIG_PATH) + controller.run() \ No newline at end of file diff --git a/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc_gait.py b/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc_gait.py new file mode 100644 index 0000000..e909dcc --- /dev/null +++ b/gr00t_wbc/sim2mujoco/scripts/run_mujoco_gear_wbc_gait.py @@ -0,0 +1,303 @@ +import sys +import time +import collections +import yaml +import torch +import numpy as np +import sys +import time +import collections +import yaml +import torch +import numpy as np +import mujoco +import mujoco.viewer +import onnxruntime as ort +import threading +from pynput import keyboard as pkb +import os + +class GearWbcController: + def __init__(self, config_path): + self.CONFIG_PATH = config_path + self.cmd_lock = threading.Lock() + self.config = self.load_config(os.path.join(self.CONFIG_PATH, "g1_gear_wbc.yaml")) + + self.control_dict = { + 'loco_cmd': self.config['cmd_init'], + "height_cmd": self.config['height_cmd'], + "rpy_cmd": self.config.get('rpy_cmd', [0.0, 0.0, 0.0]), + "freq_cmd": self.config.get('freq_cmd', 1.5) + } + + self.model = mujoco.MjModel.from_xml_path(self.config['xml_path']) + self.data = mujoco.MjData(self.model) + self.model.opt.timestep = self.config['simulation_dt'] + self.n_joints = self.data.qpos.shape[0] - 7 + self.torso_index = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso_link") + self.base_index = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis") + self.action = np.zeros(self.config['num_actions'], dtype=np.float32) + self.target_dof_pos = self.config['default_angles'].copy() + self.policy = self.load_onnx_policy(self.config['policy_path']) + self.gait_indices = torch.zeros((1), dtype=torch.float32) + self.counter = 0 + self.just_started = 0.0 + self.walking_mask = False + self.frozen_FL = False + self.frozen_FR = False + self.single_obs, self.single_obs_dim = self.compute_observation( + self.data, self.config, self.action, self.control_dict, self.n_joints + ) + self.obs_history = collections.deque( + [np.zeros(self.single_obs_dim, dtype=np.float32)] * self.config['obs_history_len'], + maxlen=self.config['obs_history_len'] + ) + self.obs = np.zeros(self.config['num_obs'], dtype=np.float32) + self.keyboard_listener(self.control_dict, self.config) + + + def keyboard_listener(self, control_dict, config): + """Listen to key press events and update cmd and height_cmd""" + def on_press(key): + try: + k = key.char + except AttributeError: + return # Special keys ignored + + with self.cmd_lock: + if k == 'w': + control_dict['loco_cmd'][0] += 0.2 + elif k == 's': + control_dict['loco_cmd'][0] -= 0.2 + elif k == 'a': + control_dict['loco_cmd'][1] += 0.5 + elif k == 'd': + control_dict['loco_cmd'][1] -= 0.5 + elif k == 'q': + control_dict['loco_cmd'][2] += 0.5 + elif k == 'e': + control_dict['loco_cmd'][2] -= 0.5 + elif k == 'z': + control_dict['loco_cmd'][:] = config['cmd_init'] + control_dict["height_cmd"] = config['height_cmd'] + control_dict['rpy_cmd'][:] = config['rpy_cmd'] + control_dict['freq_cmd'] = config['freq_cmd'] + elif k == '1': + control_dict["height_cmd"] += 0.05 + elif k == '2': + control_dict["height_cmd"] -= 0.05 + elif k == '3': + control_dict['rpy_cmd'][0] += 0.2 + elif k == '4': + control_dict['rpy_cmd'][0] -= 0.2 + elif k == '5': + control_dict['rpy_cmd'][1] += 0.2 + elif k == '6': + control_dict['rpy_cmd'][1] -= 0.2 + elif k == '7': + control_dict['rpy_cmd'][2] += 0.2 + elif k == '8': + control_dict['rpy_cmd'][2] -= 0.2 + elif k == 'm': + control_dict['freq_cmd'] += 0.1 + elif k == 'n': + control_dict['freq_cmd'] -= 0.1 + + print(f"Current Commands: loco_cmd = {control_dict['loco_cmd']}, height_cmd = {control_dict['height_cmd']}, rpy_cmd = {control_dict['rpy_cmd']}, freq_cmd = {control_dict['freq_cmd']}") + + listener = pkb.Listener(on_press=on_press) + listener.daemon = True + listener.start() + + def load_config(self, config_path): + with open(config_path, 'r') as f: + config = yaml.safe_load(f) + + for path_key in ['policy_path', 'xml_path']: + config[path_key] = os.path.join(CONFIG_PATH, config[path_key]) + + array_keys = ['kps', 'kds', 'default_angles', 'cmd_scale', 'cmd_init'] + for key in array_keys: + config[key] = np.array(config[key], dtype=np.float32) + + return config + + def pd_control(self, target_q, q, kp, target_dq, dq, kd): + return (target_q - q) * kp + (target_dq - dq) * kd + + def quat_rotate_inverse(self, q, v): + w, x, y, z = q + q_conj = np.array([w, -x, -y, -z]) + return np.array([ + v[0] * (q_conj[0]**2 + q_conj[1]**2 - q_conj[2]**2 - q_conj[3]**2) + + v[1] * 2 * (q_conj[1]*q_conj[2] - q_conj[0]*q_conj[3]) + + v[2] * 2 * (q_conj[1]*q_conj[3] + q_conj[0]*q_conj[2]), + v[0] * 2 * (q_conj[1]*q_conj[2] + q_conj[0]*q_conj[3]) + + v[1] * (q_conj[0]**2 - q_conj[1]**2 + q_conj[2]**2 - q_conj[3]**2) + + v[2] * 2 * (q_conj[2]*q_conj[3] - q_conj[0]*q_conj[1]), + v[0] * 2 * (q_conj[1]*q_conj[3] - q_conj[0]*q_conj[2]) + + v[1] * 2 * (q_conj[2]*q_conj[3] + q_conj[0]*q_conj[1]) + + v[2] * (q_conj[0]**2 - q_conj[1]**2 - q_conj[2]**2 + q_conj[3]**2) + ]) + + def get_gravity_orientation(self, quat): + gravity_vec = np.array([0.0, 0.0, -1.0]) + return self.quat_rotate_inverse(quat, gravity_vec) + + def compute_observation(self, d, config, action, control_dict, n_joints): + command = np.zeros(8, dtype=np.float32) + command[:3] = control_dict['loco_cmd'][:3] * config['cmd_scale'] + command[3] = control_dict['height_cmd'] + command[4] = control_dict['freq_cmd'] + command[5:8] = control_dict['rpy_cmd'] + + # gait indice + is_static = np.linalg.norm(command[:3]) < 0.1 + just_entered_walk = (not is_static) and (not self.walking_mask) + self.walking_mask = not is_static + + if just_entered_walk: + self.just_started = 0.0 + self.gait_indices = torch.tensor([-0.25]) + if not is_static: + self.just_started += 0.02 + else: + self.just_started = 0.0 + + if not is_static: + self.frozen_FL = False + self.frozen_FR = False + + self.gait_indices = torch.remainder(self.gait_indices + 0.02 * command[4], 1.0) + + # Parameters + duration = 0.5 + phase = 0.5 + + # Gait indices + gait_FR = self.gait_indices.clone() + gait_FL = torch.remainder(gait_FR + phase, 1.0) + + if self.just_started < (0.5 / command[4]): + gait_FR = torch.tensor([0.25]) + gait_pair = [gait_FL.clone(), gait_FR.clone()] + + for i, fi in enumerate(gait_pair): + if fi.item() < duration: + gait_pair[i] = fi * (0.5 / duration) + else: + gait_pair[i] = 0.5 + (fi - duration) * (0.5 / (1 - duration)) + + # Clock signal + clock = [torch.sin(2 * np.pi * fi) for fi in gait_pair] + + + for i, (clk, frozen_mask_attr) in enumerate( + zip(clock, ['frozen_FL', 'frozen_FR']) + ): + frozen_mask = getattr(self, frozen_mask_attr) + # Freeze condition: static and at sin peak + if is_static and (not frozen_mask) and clk.item() > 0.98: + setattr(self, frozen_mask_attr, True) + clk = torch.tensor([1.0]) + if getattr(self, frozen_mask_attr): + clk = torch.tensor([1.0]) + clock[i] = clk + + self.clock_inputs = torch.stack(clock).unsqueeze(0) + qj = d.qpos[7:7+n_joints].copy() + dqj = d.qvel[6:6+n_joints].copy() + quat = d.qpos[3:7].copy() + omega = d.qvel[3:6].copy() + # omega = self.data.xmat[self.base_index].reshape(3, 3).T @ self.data.cvel[self.base_index][3:6] + padded_defaults = np.zeros(n_joints, dtype=np.float32) + L = min(len(config['default_angles']), n_joints) + padded_defaults[:L] = config['default_angles'][:L] + + qj_scaled = (qj - padded_defaults) * config['dof_pos_scale'] + dqj_scaled = dqj * config['dof_vel_scale'] + gravity_orientation = self.get_gravity_orientation(quat) + omega_scaled = omega * config['ang_vel_scale'] + + torso_quat = self.data.xquat[self.torso_index] + torso_omega = self.data.xmat[self.torso_index].reshape(3, 3).T @ self.data.cvel[self.torso_index][3:6] + torso_omega_scaled = torso_omega * config['ang_vel_scale'] + torso_gravity_orientation = self.get_gravity_orientation(torso_quat) + + single_obs_dim = 95 + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:8] = command[:8] + single_obs[8:11] = omega_scaled + single_obs[11:14] = gravity_orientation + single_obs[14:17] = 0.#torso_omega_scaled + single_obs[17:20] = 0.#torso_gravity_orientation + single_obs[20:20+n_joints] = qj_scaled + single_obs[20+n_joints:20+2*n_joints] = dqj_scaled + single_obs[20+2*n_joints:20+2*n_joints+15] = action + single_obs[20+2*n_joints+15:20+2*n_joints+15+2] = self.clock_inputs.cpu().numpy().reshape(2) + + return single_obs, single_obs_dim + + def load_onnx_policy(self, path): + model = ort.InferenceSession(path) + def run_inference(input_tensor): + ort_inputs = {model.get_inputs()[0].name: input_tensor.cpu().numpy()} + ort_outs = model.run(None, ort_inputs) + return torch.tensor(ort_outs[0], device="cuda:0") + return run_inference + + def run(self): + + self.counter = 0 + + with mujoco.viewer.launch_passive(self.model, self.data) as viewer: + start = time.time() + while viewer.is_running() and time.time() - start < self.config['simulation_duration']: + step_start = time.time() + + leg_tau = self.pd_control( + self.target_dof_pos, + self.data.qpos[7:7+self.config['num_actions']], + self.config['kps'], + np.zeros_like(self.config['kps']), + self.data.qvel[6:6+self.config['num_actions']], + self.config['kds'] + ) + self.data.ctrl[:self.config['num_actions']] = leg_tau + + if self.n_joints > self.config['num_actions']: + arm_tau = self.pd_control( + np.zeros(self.n_joints - self.config['num_actions'], dtype=np.float32), + self.data.qpos[7+self.config['num_actions']:7+self.n_joints], + np.full(self.n_joints - self.config['num_actions'], 100.0), + np.zeros(self.n_joints - self.config['num_actions']), + self.data.qvel[6+self.config['num_actions']:6+self.n_joints], + np.full(self.n_joints - self.config['num_actions'], 0.5) + ) + self.data.ctrl[self.config['num_actions']:] = arm_tau + + mujoco.mj_step(self.model, self.data) + + self.counter += 1 + if self.counter % self.config['control_decimation'] == 0: + with self.cmd_lock: + current_cmd = self.control_dict + + single_obs, _ = self.compute_observation(self.data, self.config, self.action, current_cmd, self.n_joints) + self.obs_history.append(single_obs) + + for i, hist_obs in enumerate(self.obs_history): + self.obs[i * self.single_obs_dim:(i + 1) * self.single_obs_dim] = hist_obs + + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + self.action = self.policy(obs_tensor).cpu().detach().numpy().squeeze() + self.target_dof_pos = self.action * self.config['action_scale'] + self.config['default_angles'] + + viewer.sync() + # time.sleep(max(0, self.model.opt.timestep - (time.time() - step_start))) + + +if __name__ == "__main__": + CONFIG_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "resources", "robots", "g1") + controller = GearWbcController(CONFIG_PATH) + controller.run() \ No newline at end of file diff --git a/gr00t_wbc/version.py b/gr00t_wbc/version.py new file mode 100644 index 0000000..4e48c3d --- /dev/null +++ b/gr00t_wbc/version.py @@ -0,0 +1,11 @@ +_MAJOR = "0" +_MINOR = "1" +# On main and in a nightly release the patch should be one ahead of the last +# released build. +_PATCH = "0" +# This is mainly for nightly builds which have the suffix ".dev$DATE". See +# https://semver.org/#is-v123-a-semantic-version for the semantics. +_SUFFIX = "" + +VERSION_SHORT = "{0}.{1}".format(_MAJOR, _MINOR) +VERSION = "0.1.0" # or whatever version you want diff --git a/install_scripts/install_leap_sdk.sh b/install_scripts/install_leap_sdk.sh new file mode 100644 index 0000000..b396693 --- /dev/null +++ b/install_scripts/install_leap_sdk.sh @@ -0,0 +1,19 @@ +#!/bin/bash +set -e + +# Install UltraLeap repository and key +wget -qO - https://repo.ultraleap.com/keys/apt/gpg | gpg --dearmor | sudo tee /etc/apt/trusted.gpg.d/ultraleap.gpg + +echo 'deb [arch=amd64] https://repo.ultraleap.com/apt stable main' | sudo tee /etc/apt/sources.list.d/ultraleap.list +sudo apt update + +# Install UltraLeap hand tracking (auto-accept license) +sudo apt install -y ultraleap-hand-tracking + +# Clone and install leapc-python-bindings +git clone https://github.com/ultraleap/leapc-python-bindings /tmp/leapc-python-bindings +cd /tmp/leapc-python-bindings +pip install -r requirements.txt +python -m build leapc-cffi +pip install leapc-cffi/dist/leapc_cffi-0.0.1.tar.gz +pip install -e leapc-python-api diff --git a/install_scripts/install_ros.sh b/install_scripts/install_ros.sh new file mode 100644 index 0000000..05b231c --- /dev/null +++ b/install_scripts/install_ros.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +set -e # Exit immediately on error + +echo "🔄 Cleaning up incomplete or cached packages..." +conda clean --packages --tarballs --yes + +echo "🔧 Adding RoboStack and conda-forge channels to the current environment..." +conda config --env --add channels conda-forge +conda config --env --add channels robostack-staging + +# Optional: remove defaults to avoid conflicts (ignore error if not present) +echo "⚙️ Removing 'defaults' channel if present..." +conda config --env --remove channels defaults || true + +echo "📦 Installing ROS 2 Humble Desktop from RoboStack..." +conda install -y ros-humble-desktop + +echo "✅ Sourcing ROS environment from current conda env..." +source "$CONDA_PREFIX/setup.bash" + +# Add ROS setup to bashrc if not already present +SETUP_LINE="source \"\$CONDA_PREFIX/setup.bash\" && export ROS_LOCALHOST_ONLY=1" +if ! grep -q "$SETUP_LINE" ~/.bashrc; then + echo "📝 Adding ROS setup to ~/.bashrc..." + echo "$SETUP_LINE" >> ~/.bashrc + echo "✅ Added ROS setup to ~/.bashrc" +else + echo "ℹ️ ROS setup already exists in ~/.bashrc" +fi + +echo "🚀 Launching rviz2 to verify installation (will auto-close in 5 seconds)..." +timeout 5s rviz2 diff --git a/lint.sh b/lint.sh new file mode 100755 index 0000000..a272f11 --- /dev/null +++ b/lint.sh @@ -0,0 +1,45 @@ +#!/bin/bash +# Script to run linters the same way as in the GitLab CI pipeline + +# Default mode is check only +FIX_MODE=false + +# Parse command line arguments +while [[ "$#" -gt 0 ]]; do + case $1 in + --fix) FIX_MODE=true ;; + *) echo "Unknown parameter: $1"; exit 1 ;; + esac + shift +done + +# Install required packages if not already installed +echo "Checking for required linting tools..." +pip install black ruff + +# Set the mode message +if [ "$FIX_MODE" = true ]; then + echo "Running in FIX mode - will automatically correct issues" +else + echo "Running in CHECK mode - will only report issues" +fi + +# Run Ruff lint checks +echo "Running Ruff linting checks..." +if [ "$FIX_MODE" = true ]; then + python -m ruff check --fix . +else + python -m ruff check . +fi + +# Run Ruff import sorting and Black +echo "Running style checks..." +if [ "$FIX_MODE" = true ]; then + python -m ruff check --select I --fix . + python -m black . +else + python -m ruff check --select I . + python -m black --check . +fi + +echo "Linting completed!" \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..3823d5e --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,179 @@ +[build-system] +requires = ["setuptools>=67", "wheel", "pip"] +build-backend = "setuptools.build_meta" + +[project] +# See https://setuptools.pypa.io/en/latest/userguide/quickstart.html for more project configuration options. +name = "gr00t_wbc" +dynamic = ["version"] +readme = "README.md" +classifiers = [ + "Intended Audience :: Science/Research", + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: Apache Software License", + "Programming Language :: Python :: 3", + "Topic :: Scientific/Engineering :: Artificial Intelligence", +] +authors = [ + {name = "NVIDIA Gear Lab"} +] +requires-python = "~=3.10" +dependencies = [ + "av>=14.2", + "torch", + "pyttsx3==2.90", + "scipy==1.15.3", + "numpy==1.26.4", + "matplotlib", + "hydra-core", + "ray[default]", + "click", + "gymnasium", + "mujoco", + "termcolor", + "flask", + "python-socketio>=5.13.0", + "flask_socketio", + "loguru", + "meshcat", + "meshcat-shapes", + "onnxruntime", + "rerun-sdk==0.21.0", + "pygame", + "sshkeyboard", + "msgpack", + "msgpack-numpy", + "pyzmq", + "PyQt6; platform_machine != 'aarch64'", + "pin", + "pin-pink", + "pyrealsense2; sys_platform != 'darwin'", + "pyrealsense2-macosx; sys_platform == 'darwin'", + "qpsolvers[osqp,quadprog]", + "tyro", + "cv-bridge", + "lark", + "lerobot @ git+https://github.com/huggingface/lerobot.git@a445d9c9da6bea99a8972daa4fe1fdd053d711d2", + "datasets==3.6.0", + "pandas", + "evdev; sys_platform == 'linux'" +] +license = {file = "LICENSE"} + +[project.optional-dependencies] +dev = [ + "pytest==7.4.0", + "build", + "setuptools", + "wheel", + "ruff", + "black", + "ipdb" +] + +[project.scripts] +gr00t_wbc = "gr00t_wbc.control.teleop.gui.cli:cli" + +[tool.setuptools.packages.find] +where = ["."] +include = ["gr00t_wbc*"] +exclude = [ + "*.tests", + "*.tests.*", + "tests.*", + "tests", + "docs*", + "scripts*" +] + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.package-data] +gr00t_wbc = ["py.typed", "**/*.json", "**/*.yaml"] + +[tool.setuptools.dynamic] +version = {attr = "gr00t_wbc.version.VERSION"} + +[tool.black] +line-length = 100 +include = '\.pyi?$' +exclude = ''' +( + __pycache__ + | \.git + | \.mypy_cache + | \.pytest_cache + | \.vscode + | \.venv + | \bdist\b + | \bdoc\b + | external_dependencies + | logs + | source + | /( + | external_dependencies/ + )/ +) +''' + +[tool.isort] +profile = "black" +multi_line_output = 3 +skip = [ + "external_dependencies", + "external_dependencies/**", +] +skip_glob = [ + "external_dependencies/*", +] + +[tool.pyright] +reportPrivateImportUsage = false + +[tool.ruff] +line-length = 115 +target-version = "py310" +exclude = [ + "gr00t_wbc/dexmg", + "external_dependencies", + "external_dependencies/**", + "./external_dependencies/**", + "*.ipynb", +] + + +[tool.ruff.lint] +select = ["E", "F", "I"] + +[tool.ruff.lint.per-file-ignores] +"__init__.py" = ["F401"] + +[tool.ruff.lint.isort] +case-sensitive = false +combine-as-imports = true +force-sort-within-sections = true +from-first = false +single-line-exclusions = ["typing"] +section-order = ["future", "standard-library", "third-party", "first-party", "local-folder"] + +[tool.mypy] +ignore_missing_imports = true +no_site_packages = true +check_untyped_defs = true +exclude = [ + "external_dependencies", +] + +[[tool.mypy.overrides]] +module = "tests.*" +strict_optional = false + +[tool.pytest.ini_options] +testpaths = "tests/" +python_classes = [ + "Test*", + "*Test" +] +log_format = "%(asctime)s - %(levelname)s - %(name)s - %(message)s" +log_level = "DEBUG" diff --git a/scripts/deploy_g1.py b/scripts/deploy_g1.py new file mode 100644 index 0000000..8e79ac2 --- /dev/null +++ b/scripts/deploy_g1.py @@ -0,0 +1,472 @@ +from pathlib import Path +import signal +import subprocess +import sys +import time + +import tyro + +from gr00t_wbc.control.main.teleop.configs.configs import DeploymentConfig +from gr00t_wbc.control.utils.run_real_checklist import show_deployment_checklist + + +class G1Deployment: + """ + Unified deployment manager for G1 robot with one-click operation. + Handles camera setup, control loop, teleoperation, and data collection. + Uses tmux for process management and I/O handling. + """ + + def __init__(self, config: DeploymentConfig): + self.config = config + + # Process directories + self.project_root = Path(__file__).resolve().parent.parent + + # Tmux session name + self.session_name = "g1_deployment" + + # Create tmux session if it doesn't exist + self._create_tmux_session() + + def _create_tmux_session(self): + """Create a new tmux session if it doesn't exist""" + # Check if session exists + result = subprocess.run( + ["tmux", "has-session", "-t", self.session_name], capture_output=True, text=True + ) + + if result.returncode != 0: + # Create new session + subprocess.run(["tmux", "new-session", "-d", "-s", self.session_name]) + print(f"Created new tmux session: {self.session_name}") + + # Set up the default window for control, data collection, and teleop + # First rename the default window (which is 0) to our desired name + subprocess.run( + ["tmux", "rename-window", "-t", f"{self.session_name}:0", "control_data_teleop"] + ) + # Split the window horizontally (left and right) + subprocess.run(["tmux", "split-window", "-t", f"{self.session_name}:0", "-h"]) + # Split the right pane vertically (top and bottom) + subprocess.run(["tmux", "split-window", "-t", f"{self.session_name}:0.1", "-v"]) + # Select the left pane (control) + subprocess.run(["tmux", "select-pane", "-t", f"{self.session_name}:0.0"]) + + def _run_in_tmux(self, name, cmd, wait_time=2, pane_index=None): + """Run a command in a new tmux window or pane""" + if pane_index is not None: + # Run in existing window's pane + target = f"{self.session_name}:0.{pane_index}" + else: + # Create new window + subprocess.run(["tmux", "new-window", "-t", self.session_name, "-n", name]) + target = f"{self.session_name}:{name}" + + # Set up trap for Ctrl+\ in the window + trap_cmd = f"trap 'tmux kill-session -t {self.session_name}' QUIT" + + # Set environment variable for the tmux session name + env_cmd = f"export GR00T_WBC_TMUX_SESSION={self.session_name}" + + # Construct the command with proper escaping and trap + cmd_str = " ".join(str(x) for x in cmd) + full_cmd = f"{trap_cmd}; {env_cmd}; {cmd_str}" + + # Send command to tmux window/pane + subprocess.run(["tmux", "send-keys", "-t", target, full_cmd, "C-m"]) + + # Wait for process to start + time.sleep(wait_time) + + # Check if process is still running + result = subprocess.run( + ["tmux", "list-panes", "-t", target, "-F", "#{pane_dead}"], + capture_output=True, + text=True, + ) + + if result.stdout.strip() == "1": + print(f"ERROR: {name} failed to start!") + return False + + return True + + def start_camera_sensor(self): + """Start the camera sensor in local mode if we are using replay video""" + if self.config.egoview_replay_dummy is None and self.config.head_replay_dummy is None: + return + + print("Starting camera sensor in local mode...") + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/sensor/composed_camera.py"), + "--egoview_camera", + self.config.egoview_replay_dummy, + "--head_camera", + self.config.head_replay_dummy, + "--port", + str(self.config.camera_port), + "--host", + "localhost", + ] + + if not self._run_in_tmux("camera_sensor", cmd): + print("ERROR: Camera sensor failed to start!") + print("Continuing without camera sensor...") + else: + print("Camera sensor started successfully.") + + def start_camera_viewer(self): + """Start the ROS rqt camera viewer""" + if not self.config.view_camera: + return + + print("Starting camera viewer...") + # Use rqt directly instead of ros2 run + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/main/teleop/run_camera_viewer.py"), + "--camera_host", + self.config.camera_host, + "--camera_port", + str(self.config.camera_port), + "--fps", + str(self.config.fps), + ] + + if not self._run_in_tmux("camera_viewer", cmd): + print("ERROR: Camera viewer failed to start!") + print("Continuing without camera viewer...") + else: + print("Camera viewer started successfully.") + + def start_sim_loop(self): + """Start the simulation loop in a separate process""" + print("Starting simulation loop...") + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/main/teleop/run_sim_loop.py"), + "--wbc_version", + self.config.wbc_version, + "--interface", + self.config.interface, + "--simulator", + self.config.simulator, + "--sim_frequency", + str(self.config.sim_frequency), + "--env_name", + self.config.env_name, + "--camera_port", + str(self.config.camera_port), + ] + + # Handle boolean flags + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.with_hands: + cmd.append("--with_hands") + else: + cmd.append("--no-with_hands") + + if self.config.image_publish: + cmd.append("--enable_image_publish") + cmd.append("--enable_offscreen") + else: + cmd.append("--no-enable_image_publish") + + if self.config.enable_onscreen: + cmd.append("--enable_onscreen") + else: + cmd.append("--no-enable_onscreen") + + if not self._run_in_tmux("sim_loop", cmd, wait_time=5): + print("ERROR: Simulation loop failed to start!") + self.cleanup() + sys.exit(1) + + print("Simulation loop started successfully. Waiting for warmup for 10 seconds...") + time.sleep(10) # Wait for sim loop to warm up + + def start_control_loop(self): + """Start the G1 control loop""" + print("Starting G1 control loop...") + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/main/teleop/run_g1_control_loop.py"), + "--wbc_version", + self.config.wbc_version, + "--wbc_model_path", + self.config.wbc_model_path, + "--wbc_policy_class", + self.config.wbc_policy_class, + "--interface", + self.config.interface, + "--simulator", + "None" if self.config.sim_in_single_process else self.config.simulator, + "--control_frequency", + str(self.config.control_frequency), + ] + + # Handle boolean flag using presence/absence pattern + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.with_hands: + cmd.append("--with_hands") + else: + cmd.append("--no-with_hands") + + if self.config.high_elbow_pose: + cmd.append("--high_elbow_pose") + else: + cmd.append("--no-high_elbow_pose") + + # Gravity compensation configuration + # Note: This is where gravity compensation is actually applied since the control loop + # contains the G1Body that interfaces directly with the robot motors + if self.config.enable_gravity_compensation: + cmd.append("--enable_gravity_compensation") + # Add joint groups if specified + if self.config.gravity_compensation_joints: + cmd.extend( + ["--gravity_compensation_joints"] + self.config.gravity_compensation_joints + ) + else: + cmd.append("--no-enable_gravity_compensation") + + if not self._run_in_tmux("control", cmd, wait_time=3, pane_index=0): + print("ERROR: Control loop failed to start!") + self.cleanup() + sys.exit(1) + + print("Control loop started successfully.") + print("Controls: 'i' for initial pose, ']' to activate locomotion") + + def start_policy(self): + """Start either teleop or inference policy based on configuration""" + if not self.config.enable_upper_body_operation: + print("Upper body operation disabled in config.") + return + + self.start_teleop() + + def start_teleop(self): + """Start the teleoperation policy""" + print("Starting teleoperation policy...") + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/main/teleop/run_teleop_policy_loop.py"), + "--body_control_device", + self.config.body_control_device, + "--hand_control_device", + self.config.hand_control_device, + "--body_streamer_ip", + self.config.body_streamer_ip, + "--body_streamer_keyword", + self.config.body_streamer_keyword, + ] + + # Handle boolean flags using tyro syntax + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.high_elbow_pose: + cmd.append("--high_elbow_pose") + else: + cmd.append("--no-high_elbow_pose") + + if self.config.enable_visualization: + cmd.append("--enable_visualization") + else: + cmd.append("--no-enable_visualization") + + if self.config.enable_real_device: + cmd.append("--enable_real_device") + else: + cmd.append("--no-enable_real_device") + + if not self._run_in_tmux("teleop", cmd, pane_index=2): + print("ERROR: Teleoperation policy failed to start!") + print("Continuing without teleoperation...") + else: + print("Teleoperation policy started successfully.") + print("Press 'l' in the control loop terminal to start teleoperation.") + + def start_data_collection(self): + """Start the data collection process""" + if not self.config.data_collection: + print("Data collection disabled in config.") + return + + print("Starting data collection...") + cmd = [ + sys.executable, + str(self.project_root / "gr00t_wbc/control/main/teleop/run_g1_data_exporter.py"), + "--data_collection_frequency", + str(self.config.data_collection_frequency), + "--root_output_dir", + self.config.root_output_dir, + "--lower_body_policy", + self.config.wbc_version, + "--wbc_model_path", + self.config.wbc_model_path, + "--camera_host", + self.config.camera_host, + "--camera_port", + str(self.config.camera_port), + ] + + if not self._run_in_tmux("data", cmd, pane_index=1): + print("ERROR: Data collection failed to start!") + print("Continuing without data collection...") + else: + print("Data collection started successfully.") + print("Press 'c' in the control loop terminal to start/stop recording data.") + + def start_webcam_recording(self): + """Start webcam recording for real robot deployment monitoring""" + if not self.config.enable_webcam_recording or self.config.env_type != "real": + return + + print("Starting webcam recording for deployment monitoring...") + cmd = [ + sys.executable, + str(self.project_root / "scripts/run_webcam_recorder.py"), + "--output_dir", + self.config.webcam_output_dir, + ] + + if not self._run_in_tmux("webcam", cmd): + print("ERROR: Webcam recording failed to start!") + print("Continuing without webcam recording...") + else: + print("Webcam recording started successfully.") + print("External camera recording deployment activities to logs_experiment/") + + def deploy(self): + """ + Run the complete deployment process + """ + print("Starting G1 deployment with config:") + print(f" Robot IP: {self.config.robot_ip}") + print(f" WBC Version: {self.config.wbc_version}") + print(f" Interface: {self.config.interface}") + print(f" Policy Mode: {self.config.upper_body_operation_mode}") + print(f" With Hands: {self.config.with_hands}") + print(f" View Camera: {self.config.view_camera}") + print(f" Enable Waist: {self.config.enable_waist}") + print(f" High Elbow Pose: {self.config.high_elbow_pose}") + print(f" Gravity Compensation: {self.config.enable_gravity_compensation}") + if self.config.enable_gravity_compensation: + print(f" Gravity Comp Joints: {self.config.gravity_compensation_joints}") + print( + f" Webcam Recording: {self.config.enable_webcam_recording and self.config.env_type == 'real'}" + ) + print(f" Sim in Single Process: {self.config.sim_in_single_process}") + if self.config.sim_in_single_process: + print(f" Image Publish: {self.config.image_publish}") + + # Check if this is a real robot deployment and run safety checklist + if self.config.env_type == "real": + if not show_deployment_checklist(): + sys.exit(1) + + # Register signal handler for clean shutdown + signal.signal(signal.SIGINT, self.signal_handler) + + # Start components in sequence + # Start sim loop first if sim_in_single_process is enabled + if self.config.sim_in_single_process: + self.start_sim_loop() + + self.start_control_loop() + self.start_camera_viewer() + self.start_policy() # This will start either teleop or inference policy + self.start_data_collection() + self.start_webcam_recording() # Only runs for real robot deployment + + print("\n--- G1 DEPLOYMENT COMPLETE ---") + print("All systems running in tmux session:", self.session_name) + print("Press Ctrl+b then d to detach from the session") + print("Press Ctrl+\\ in any window to shutdown all components.") + + try: + # Automatically attach to the tmux session and switch to control window + subprocess.run( + [ + "tmux", + "attach", + "-t", + self.session_name, + ";", + "select-window", + "-t", + "control_data_teleop", + ] + ) + except KeyboardInterrupt: + print("\nShutdown requested...") + self.cleanup() + sys.exit(0) + + # Keep main thread alive to handle signals + try: + while True: + # Check if tmux session still exists + result = subprocess.run( + ["tmux", "has-session", "-t", self.session_name], capture_output=True, text=True + ) + + if result.returncode != 0: + print("Tmux session terminated. Exiting.") + break + + time.sleep(1) + except KeyboardInterrupt: + print("\nShutdown requested...") + finally: + self.cleanup() + + def cleanup(self): + """Clean up tmux session""" + print("Cleaning up tmux session...") + try: + # Kill the tmux session + subprocess.run(["tmux", "kill-session", "-t", self.session_name], timeout=5) + print("Tmux session terminated successfully.") + except subprocess.TimeoutExpired: + print("Warning: Tmux session termination timed out, forcing kill...") + subprocess.run(["tmux", "kill-session", "-t", self.session_name, "-9"]) + except Exception as e: + print(f"Warning: Error during cleanup: {e}") + print("Cleanup complete.") + + def signal_handler(self, sig, frame): + """Handle SIGINT (Ctrl+C) gracefully""" + print("\nShutdown signal received...") + self.cleanup() + sys.exit(0) + + +def main(): + """Main entry point with automatic CLI generation from G1Config dataclass""" + # This single line automatically generates a complete CLI from the dataclass! + config = tyro.cli(DeploymentConfig) + + # Run deployment with the configured settings + deployment = G1Deployment(config) + deployment.deploy() + + +if __name__ == "__main__": + # Edited outside docker + + main() diff --git a/scripts/leap_tracking_example.py b/scripts/leap_tracking_example.py new file mode 100644 index 0000000..04fde4b --- /dev/null +++ b/scripts/leap_tracking_example.py @@ -0,0 +1,50 @@ +"""Prints the palm position of each hand, every frame. When a device is +connected we set the tracking mode to desktop and then generate logs for +every tracking frame received. The events of creating a connection to the +server and a device being plugged in also generate logs. +""" + +import time + +import leap + + +class MyListener(leap.Listener): + def on_connection_event(self, event): + print("Connected") + + def on_device_event(self, event): + try: + with event.device.open(): + info = event.device.get_info() + except leap.LeapCannotOpenDeviceError: + info = event.device.get_info() + + print(f"Found device {info.serial}") + + def on_tracking_event(self, event): + print(f"Frame {event.tracking_frame_id} with {len(event.hands)} hands.") + for hand in event.hands: + hand_type = "left" if str(hand.type) == "HandType.Left" else "right" + print( + f"Hand id {hand.id} is a {hand_type} hand with position ({hand.palm.position.x}, " + f"{hand.palm.position.y}, {hand.palm.position.z})." + ) + + +def main(): + my_listener = MyListener() + + connection = leap.Connection() + connection.add_listener(my_listener) + + running = True + + with connection.open(): + connection.set_tracking_mode(leap.TrackingMode.Desktop) + while running: + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/scripts/run_webcam_recorder.py b/scripts/run_webcam_recorder.py new file mode 100755 index 0000000..18e1965 --- /dev/null +++ b/scripts/run_webcam_recorder.py @@ -0,0 +1,392 @@ +#!/usr/bin/env python3 +""" +Simple webcam recorder with optional preview window. +Usage: + webcam_recorder.py [--output-dir DIR] [--no-preview] + webcam_recorder.py --help +""" + +import argparse +from datetime import datetime +import os +from pathlib import Path +import signal +import subprocess +import sys +import time + + +class WebcamRecorder: + def __init__(self, debug=False): + self.process = None + self.debug = debug + + def _debug_print(self, message): + """Print debug message only if debug mode is enabled.""" + if self.debug: + print(f"[webcam_recorder] {message}", file=sys.stderr) + + def _info_print(self, message): + """Print info message always.""" + print(f"[webcam_recorder] {message}", file=sys.stderr) + + def find_webcam(self): + """Find available webcam device.""" + # Check container status only in debug mode + if self.debug and (os.path.exists("/.dockerenv") or os.environ.get("CONTAINER")): + self._debug_print("Running in Docker container") + # Check if we have video group permissions + try: + import grp + + groups = os.getgroups() + video_gid = grp.getgrnam("video").gr_gid + if video_gid in groups: + self._debug_print("Has video group permissions ✓") + else: + self._debug_print("Warning: Not in video group") + except Exception: + pass + + # Check /dev/v4l/by-id for Logitech webcam first + by_id_path = Path("/dev/v4l/by-id") + if by_id_path.exists(): + self._debug_print("Checking /dev/v4l/by-id for devices...") + for device in by_id_path.iterdir(): + if device.is_symlink(): + device_name = device.name + if "Logitech" in device_name or "046d" in device_name: + resolved_device = str(device.resolve()) + self._debug_print( + f"Found Logitech device: {device_name} -> {resolved_device}" + ) + if self._is_video_capture_device(resolved_device): + return resolved_device + + # No Logitech found, try any webcam + for device in by_id_path.iterdir(): + if device.is_symlink() and "metadata" not in device.name: + resolved_device = str(device.resolve()) + self._debug_print(f"Checking device: {device.name} -> {resolved_device}") + if self._is_video_capture_device(resolved_device): + return resolved_device + + # Fallback to /dev/video* - prioritize external cameras + self._debug_print("Scanning /dev/video* devices...") + external_devices = [] + integrated_devices = [] + + for i in range(20): + device = f"/dev/video{i}" + if os.path.exists(device): + # Check if we can access the device + if not os.access(device, os.R_OK): + self._debug_print(f"Found {device} but no read access") + continue + + # Check if it's a video capture device (not metadata) + if self._is_video_capture_device(device): + # Check device name to prioritize external cameras + device_name = self._get_device_name(device) + if device_name: + if any( + keyword in device_name.lower() + for keyword in ["logitech", "brio", "c920", "c930", "c925"] + ): + self._info_print(f"Found external camera: {device} ({device_name})") + external_devices.append(device) + elif ( + "integrated" in device_name.lower() or "internal" in device_name.lower() + ): + self._debug_print(f"Found integrated camera: {device} ({device_name})") + integrated_devices.append(device) + else: + self._debug_print(f"Found unknown camera: {device} ({device_name})") + external_devices.append(device) # Assume external if unknown + else: + self._debug_print(f"Found camera: {device} (no name info)") + external_devices.append(device) + + # Return external camera first, then integrated as fallback + if external_devices: + self._info_print(f"Using external camera: {external_devices[0]}") + return external_devices[0] + elif integrated_devices: + self._info_print( + "No external camera found - integrated camera available but not preferred" + ) + self._info_print( + "Please connect an external camera (Logitech, Brio, etc.) for recording" + ) + return None + + return None + + def _get_device_name(self, device): + """Get the friendly name of a video device.""" + try: + # Extract device number from /dev/videoX + device_num = device.split("video")[-1] + name_path = f"/sys/class/video4linux/video{device_num}/name" + if os.path.exists(name_path): + with open(name_path, "r") as f: + return f.read().strip() + except Exception: + pass + return None + + def _is_video_capture_device(self, device): + """Check if a device is a video capture device (not metadata).""" + try: + # Try v4l2-ctl first if available + if subprocess.run(["which", "v4l2-ctl"], capture_output=True).returncode == 0: + result = subprocess.run( + ["v4l2-ctl", "--device=" + device, "--info"], capture_output=True, timeout=2 + ) + if result.returncode == 0: + output = result.stdout.decode() + # Look for "Video Capture" capability in Device Caps, not just general Capabilities + lines = output.split("\n") + in_device_caps = False + for line in lines: + if "Device Caps" in line: + in_device_caps = True + continue + if in_device_caps: + if line.strip().startswith("Video Capture"): + # Double-check with ffmpeg + ffmpeg_result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + timeout=2, + ) + return ffmpeg_result.returncode == 0 + # Stop checking if we hit another section + elif line.strip() and not line.startswith("\t"): + break + + # Fallback: try ffmpeg directly if v4l2-ctl not available + self._debug_print(f"Testing {device} with ffmpeg...") + ffmpeg_result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + timeout=5, + ) + if ffmpeg_result.returncode == 0: + self._debug_print(f"{device} is a working video capture device") + return True + else: + stderr_output = ffmpeg_result.stderr.decode() + # Check if it's just a metadata device + if ( + "metadata" in stderr_output.lower() + or "not a capture device" in stderr_output.lower() + ): + self._debug_print(f"{device} is metadata device, skipping") + return False + else: + self._debug_print(f"{device} failed ffmpeg test: {stderr_output[:100]}") + return False + except Exception as e: + self._debug_print(f"Error testing {device}: {e}") + pass + return False + + def test_camera_format(self, device): + """Test camera and determine best format.""" + # Try default format + result = subprocess.run( + ["ffmpeg", "-f", "v4l2", "-i", device, "-frames:v", "1", "-f", "null", "-"], + capture_output=True, + ) + if result.returncode == 0: + return [] # Default format works + + # Try MJPEG format + result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-input_format", + "mjpeg", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + ) + if result.returncode == 0: + return ["-input_format", "mjpeg"] + + raise RuntimeError(f"Cannot access camera at {device}") + + def record(self, output_dir="./logs_experiment", preview=True): + """Start recording from webcam.""" + # Find webcam + device = self.find_webcam() + if not device: + self._info_print("No webcam found") + return False + + self._info_print(f"Recording from: {device}") + + # Test camera format + try: + format_args = self.test_camera_format(device) + except RuntimeError as e: + self._info_print(str(e)) + return False + + # Create output directory with date subfolder and filename + now = datetime.now() + date_folder = now.strftime("%Y_%m_%d") + output_path = Path(output_dir) / date_folder + output_path.mkdir(parents=True, exist_ok=True) + timestamp = now.strftime("%Y_%m_%d_%H_%M_%S") + output_file = output_path / f"robot_video_{timestamp}.mp4" + + self._info_print(f"Saving to: {output_file}") + if self.debug: + self._debug_print(f"Absolute path: {output_file.absolute()}") + + # Check if preview is possible + if preview and not os.environ.get("DISPLAY"): + self._debug_print("No DISPLAY found, disabling preview") + preview = False + + # Build ffmpeg command + cmd = ( + ["ffmpeg", "-loglevel", "error", "-f", "v4l2"] + + format_args + + ["-i", device, "-c:v", "libx264", "-preset", "ultrafast"] + ) + + if preview: + # Try with preview first + preview_cmd = cmd + ["-f", "tee", "-map", "0:v", f"[f=mp4]{output_file}|[f=nut]pipe:"] + + try: + ffmpeg_proc = subprocess.Popen( + preview_cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL + ) + ffplay_proc = subprocess.Popen( + [ + "ffplay", + "-loglevel", + "quiet", + "-f", + "nut", + "-i", + "pipe:", + "-window_title", + "Webcam Preview", + ], + stdin=ffmpeg_proc.stdout, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + + # Wait a bit to see if it starts successfully + time.sleep(0.5) + if ffmpeg_proc.poll() is None and ffplay_proc.poll() is None: + self.process = ffplay_proc # Kill ffplay to stop both + self._info_print("Recording with preview started") + return True + else: + self._debug_print("Preview failed, falling back to no-preview mode") + # Clean up failed processes + try: + ffmpeg_proc.kill() + ffplay_proc.kill() + except Exception: + pass + except Exception as e: + self._debug_print(f"Preview error: {e}, using no-preview mode") + + # Record without preview + cmd.append(str(output_file)) + self.process = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE) + + # Verify it's running + time.sleep(0.5) + if self.process.poll() is None: + self._info_print("Recording started (no preview)") + return True + else: + stderr = self.process.stderr.read().decode() if self.process.stderr else "" + self._info_print(f"Failed to start recording: {stderr}") + return False + + def stop(self): + """Stop recording.""" + if self.process: + self.process.terminate() + try: + self.process.wait(timeout=5) + except subprocess.TimeoutExpired: + self.process.kill() + self._info_print("Recording stopped") + self.process = None + + +def main(): + parser = argparse.ArgumentParser(description="Simple webcam recorder") + parser.add_argument( + "--output-dir", default="./logs_experiment", help="Output directory for recordings" + ) + parser.add_argument("--no-preview", action="store_true", help="Disable preview window") + parser.add_argument("--test", action="store_true", help="Enable debug output") + args = parser.parse_args() + + recorder = WebcamRecorder(debug=args.test) + + # Set up signal handlers for clean shutdown + def signal_handler(signum, frame): + print("\nStopping recording...", file=sys.stderr) + recorder.stop() + sys.exit(0) + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + # Start recording + if recorder.record(args.output_dir, preview=not args.no_preview): + print("Press Ctrl+C to stop recording", file=sys.stderr) + # Keep running until interrupted + try: + signal.pause() + except KeyboardInterrupt: + pass + else: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..d614373 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,15 @@ +import pytest + + +def pytest_addoption(parser): + parser.addoption( + "--tensorboard-log-dir", + action="store", + default="logs/Gr00t_TRL_loco/.tensorboard", + help="Directory containing tensorboard logs", + ) + + +@pytest.fixture +def tensorboard_log_dir(request): + return request.config.getoption("--tensorboard-log-dir") diff --git a/tests/control/__init__.py b/tests/control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/main/__init__.py b/tests/control/main/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/main/teleop/__init__.py b/tests/control/main/teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/main/teleop/test_g1_control_loop.py b/tests/control/main/teleop/test_g1_control_loop.py new file mode 100644 index 0000000..1248ab2 --- /dev/null +++ b/tests/control/main/teleop/test_g1_control_loop.py @@ -0,0 +1,465 @@ +import argparse +import os +from pathlib import Path +import signal +import subprocess +import threading +import time + +import numpy as np +import pytest +import rclpy +from scipy.spatial.transform import Rotation as R +from std_msgs.msg import String as RosStringMsg + +from gr00t_wbc.control.main.constants import CONTROL_GOAL_TOPIC, KEYBOARD_INPUT_TOPIC, STATE_TOPIC_NAME +from gr00t_wbc.control.utils.ros_utils import ROSMsgPublisher, ROSMsgSubscriber +from gr00t_wbc.control.utils.term_color_constants import GREEN_BOLD, RESET, YELLOW_BOLD +from gr00t_wbc.data.viz.rerun_viz import RerunViz + + +class KeyboardPublisher: + def __init__(self, topic_name: str = KEYBOARD_INPUT_TOPIC): + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] + self.publisher = self.node.create_publisher(RosStringMsg, topic_name, 1) + + def publish(self, key: str): + msg = RosStringMsg() + msg.data = key + self.publisher.publish(msg) + + +def is_robot_fallen_from_quat(mujoco_quat): + # Convert MuJoCo [w, x, y, z] → SciPy [x, y, z, w] + w, x, y, z = mujoco_quat + scipy_quat = [x, y, z, w] + + r = R.from_quat(scipy_quat) + roll, pitch, _ = r.as_euler("xyz", degrees=False) + + MAX_ROLL_PITCH = np.radians(60) + print(f"[Fall Check] roll={roll:.3f} rad, pitch={pitch:.3f} rad") + return abs(roll) > MAX_ROLL_PITCH or abs(pitch) > MAX_ROLL_PITCH + + +class LocomotionRunner: + def __init__(self, test_mode: str = "squat"): + self.test_mode = test_mode + if not rclpy.ok(): + rclpy.init(args=None) + self.node = rclpy.create_node(f"EvalDriver_{test_mode}_{int(time.time())}") + + # gracefully shutdown the spin thread when the test is done + self._stop_event = threading.Event() + + self.spin_thread = threading.Thread(target=self._spin_loop, daemon=False) + self.spin_thread.start() + + self.keyboard_event_publisher = KeyboardPublisher(KEYBOARD_INPUT_TOPIC) + self.control_publisher = ROSMsgPublisher(CONTROL_GOAL_TOPIC) + self.state_subscriber = ROSMsgSubscriber(STATE_TOPIC_NAME) + print(f"{test_mode} test initialized...") + + def _spin_loop(self): + try: + while rclpy.ok() and not self._stop_event.is_set(): + rclpy.spin_once(self.node) + except rclpy.executors.ExternalShutdownException: + print("[INFO] Spin thread exiting due to shutdown.") + finally: + print("spin loop stopped...") + + def warm_up(self): + """Stabilize and release the robot.""" + print("waiting for 2 seconds...") + time.sleep(2) + print(f"running {self.test_mode} test...") + self.activate() + print("activated...") + time.sleep(1) + self.release() + print("released...") + time.sleep(5) + + def _run_walk_test(self): + self.walk_forward() # speed up to 0.2 m/s + time.sleep(1) + self.walk_forward() # speed up to 0.4 m/s + + rate = self.node.create_rate(0.5) + start_time = time.time() + while rclpy.ok() and (time.time() - start_time) < 10.0: + obs = self.state_subscriber.get_msg() + + if is_robot_fallen_from_quat(obs["torso_quat"]): + print("robot fallen...") + return 0 + elif self._check_success_condition(obs): + print(f"robot reaching target ({self.test_mode})...") + return 1, {} + else: + rate.sleep() + + print("test timed out after 10 seconds...") + return 0, {} + + def _run_squat_test(self): + rate = self.node.create_rate(0.5) + start_time = time.time() + while rclpy.ok() and (time.time() - start_time) < 10.0: + obs = self.state_subscriber.get_msg() + + if is_robot_fallen_from_quat(obs["torso_quat"]): + print("robot fallen...") + return 0, {} + elif self._check_success_condition(obs): + print(f"robot reaching target ({self.test_mode})...") + return 1, {} + else: + self.go_down() + rate.sleep() + + print("test timed out after 10 seconds...") + return 0, {} + + def cmd_to_velocity(self, cmd_list): + cmd_to_velocity = { + "w": np.array([0.2, 0.0, 0.0]), + "s": np.array([-0.2, 0.0, 0.0]), + "q": np.array([0.0, 0.2, 0.0]), + "e": np.array([0.0, -0.2, 0.0]), + "z": np.array([0.0, 0.0, 0.0]), + } + + accumulated_velocity = np.array([0.0, 0.0, 0.0]) + velocity_list = [] + for cmd in cmd_list: + if cmd == "z": + accumulated_velocity = [0.0, 0.0, 0.0] + elif cmd in ["CHECK", "SKIP"]: + accumulated_velocity = velocity_list[-1] + else: + accumulated_velocity += cmd_to_velocity[cmd] + velocity_list.append(accumulated_velocity.copy()) + + return velocity_list + + def _run_stop_test(self): + base_vel_thres = 0.25 + + cmd_list = ( + ["w", "w", "w", "w", "s", "s", "s", "z", "SKIP", "CHECK"] + + ["s", "s", "q", "w", "w", "w", "e", "s", "s", "z", "SKIP", "CHECK"] + + ["q", "q", "w", "q", "e", "s", "s", "e", "w", "z", "SKIP", "CHECK"] + + ["w", "w", "w", "w", "w", "s", "s", "s", "s", "z", "SKIP", "CHECK"] + ) + + success_flag = 1 + + statistics = { + "floating_base_pose": {"state": []}, + "floating_base_vel": {"state": [], "cmd": []}, + "timestamp": [], + } + for cmd in cmd_list: + self.keyboard_event_publisher.publish(cmd) + time.sleep(0.5) + obs = self.state_subscriber.get_msg() + statistics["floating_base_pose"]["state"].append( + np.linalg.norm(obs["floating_base_pose"]) + ) + statistics["floating_base_vel"]["state"].append( + np.linalg.norm(obs["floating_base_vel"]) + ) + statistics["timestamp"].append(time.time()) + + if cmd == "CHECK" and np.linalg.norm(obs["floating_base_vel"]) > base_vel_thres: + print( + f" [{YELLOW_BOLD}WARNING{RESET}] robot is not stopped fully. " + f"Current base velocity: {np.linalg.norm(obs['floating_base_vel']):.3f} > {base_vel_thres:.3f}" + ) + # success_flag = 0 # robot is not stopped + + time.sleep(0.5) + + vel_cmd = self.cmd_to_velocity(cmd_list) + vel_cmd = [np.linalg.norm(v) for v in vel_cmd] + statistics["floating_base_vel"]["cmd"] = vel_cmd + return success_flag, statistics + + def _run_eef_track_test(self): + from gr00t_wbc.control.policy.lerobot_replay_policy import LerobotReplayPolicy + + parquet_path = ( + Path(__file__).parent.parent.parent.parent / "replay_data" / "g1_pnpbottle.parquet" + ) + replay_policy = LerobotReplayPolicy(parquet_path=str(parquet_path)) + + freq = 50 + rate = self.node.create_rate(freq) + + statistics = { + # "floating_base_pose": {"state": [], "cmd": []}, + "eef_base_pose": {"state": [], "cmd": []}, + "timestamp": [], + } + + for ii in range(500): + action = replay_policy.get_action() + action = replay_policy.action_to_cmd(action) + action["timestamp"] = time.monotonic() + action["target_time"] = time.monotonic() + ii / freq + self.control_publisher.publish(action) + obs = self.state_subscriber.get_msg() + if obs is None: + print("no obs...") + continue + gt_obs = replay_policy.get_observation() + + # statistics["floating_base_pose"]["state"].append(obs["floating_base_pose"]) + # statistics["floating_base_pose"]["cmd"].append(np.zeros_like(obs["floating_base_pose"])) + statistics["eef_base_pose"]["state"].append(obs["wrist_pose"]) + statistics["eef_base_pose"]["cmd"].append(gt_obs["wrist_pose"]) + statistics["timestamp"].append(time.time()) + + pos_err = np.linalg.norm(obs["wrist_pose"][:3] - gt_obs["wrist_pose"][:3]) + if pos_err > 1e-1: + print( + f" [{YELLOW_BOLD}WARNING{RESET}] robot failed to track the eef, " + f"error: {pos_err:.3f} ({self.test_mode})..." + ) + return 0, statistics + + if is_robot_fallen_from_quat(obs["torso_quat"]): + print("robot fallen...") + return 0, statistics + else: + rate.sleep() + + return 1, statistics + + def run(self): + self.warm_up() + + test_mode_to_func = { + "squat": self._run_squat_test, + "walk": self._run_walk_test, + "stop": self._run_stop_test, + "eef_track": self._run_eef_track_test, + } + + result, statistics = test_mode_to_func[self.test_mode]() + + self.post_process(statistics) + return result + + def _check_success_condition(self, obs): + if self.test_mode == "squat": + return obs["floating_base_pose"][2] < 0.4 + elif self.test_mode == "walk": + return np.linalg.norm(obs["floating_base_pose"][0:2]) > 1.0 + return False + + def activate(self): + self.keyboard_event_publisher.publish("]") + + def release(self): + self.keyboard_event_publisher.publish("9") + + def go_down(self): + self.keyboard_event_publisher.publish("2") + + def walk_forward(self): + self.keyboard_event_publisher.publish("w") + + def walk_stop(self): + self.keyboard_event_publisher.publish("z") + + def post_process(self, statistics): + if len(statistics) == 0: + return + + # plot the statistics + plot_keys = [key for key in statistics.keys() if key != "timestamp"] + viz = RerunViz( + image_keys=[], + tensor_keys=plot_keys, + window_size=10.0, + app_name=f"{self.test_mode}_test", + ) + + for ii in range(len(statistics[plot_keys[0]]["state"])): + tensor_data = {} + for k in plot_keys: + if "state" in statistics[k] and "cmd" in statistics[k]: + tensor_data[k] = np.array( + (statistics[k]["state"][ii], statistics[k]["cmd"][ii]) + ).reshape(2, -1) + else: + tensor_data[k] = np.asarray(statistics[k]["state"][ii]).reshape(1, -1) + viz.plot_tensors( + tensor_data, + statistics["timestamp"][ii], + ) + + if self.test_mode == "stop": + base_velocity = statistics["floating_base_vel"]["state"] + base_velocity_cmd = statistics["floating_base_vel"]["cmd"] + + base_velocity_tracking_err = [] + for v_cmd, v in zip(base_velocity_cmd, base_velocity): # TODO: check if this is correct + if v_cmd.max() < 1e-4: + base_velocity_tracking_err.append(v) + print( + f" [{GREEN_BOLD}INFO{RESET}] Base velocity tracking when stopped: " + f"{np.mean(base_velocity_tracking_err):.3f}" + ) + + if self.test_mode == "eef_track": + eef_pose = statistics["eef_base_pose"]["state"] + eef_pose_cmd = statistics["eef_base_pose"]["cmd"] + eef_pose_tracking_err = [] + for p_cmd, p in zip(eef_pose_cmd, eef_pose): + eef_pose_tracking_err.append(np.linalg.norm(p - p_cmd)) + print( + f" [{GREEN_BOLD}INFO{RESET}] Eef pose tracking error: {np.mean(eef_pose_tracking_err):.3f}" + ) + + def shutdown(self): + self._stop_event.set() + self.spin_thread.join() + del self.state_subscriber + del self.keyboard_event_publisher + # Don't shutdown ROS between tests - let pytest handle it + + +def start_g1_control_loop(): + proc = subprocess.Popen( + [ + "python3", + "gr00t_wbc/control/main/teleop/run_g1_control_loop.py", + "--keyboard_dispatcher_type", + "ros", + "--enable-offscreen", + ], + preexec_fn=os.setsid, + ) + time.sleep(10) + return proc + + +def run_test(test_mode: str): + """Run a single test with the specified mode.""" + proc = start_g1_control_loop() + print(f"G1 control loop started for {test_mode} test...") + + test = LocomotionRunner(test_mode) + result = test.run() + + print("Shutting down...") + test.shutdown() + proc.send_signal(signal.SIGKILL) + proc.wait() + + return result + + +def test_squat(): + """Pytest function for squat test.""" + result = run_test("squat") + assert result == 1, "Squat test failed - robot either fell or didn't reach target height" + + +def test_walk(): + """Pytest function for walk test.""" + result = run_test("walk") + assert result == 1, "Walk test failed - robot either fell or didn't reach target distance" + + +@pytest.mark.skip(reason="skipping test for now, cicd test always gets killed") +def test_stop(): + """Pytest function for walking to a nearby position and stop test.""" + result = run_test("stop") + assert result == 1, "Stop test failed - robot either fell or didn't reach target distance" + + +@pytest.mark.skip(reason="skipping test for now, cicd test always gets killed") +def test_eef_track(): + """Pytest function for eef track test.""" + result = run_test("eef_track") + assert result == 1, "Eef track test failed - robot either fell or didn't reach target distance" + + +def main(): + parser = argparse.ArgumentParser(description="Run locomotion tests") + parser.add_argument("--squat", action="store_true", help="Run squat test only") + parser.add_argument("--walk", action="store_true", help="Run walk test only") + parser.add_argument("--stop", action="store_true", help="Run stop test only") + parser.add_argument("--eef_track", action="store_true", help="Run eef track test only") + + args = parser.parse_args() + + if args.squat and args.walk: + print("Error: Cannot specify both --squat and --walk") + return 1 + + if args.squat: + print("Running squat test only...") + result = run_test("squat") + if result == 1: + print("✓ Squat test PASSED") + return 0 + else: + print("✗ Squat test FAILED") + return 1 + + elif args.walk: + print("Running walk test only...") + result = run_test("walk") + if result == 1: + print("✓ Walk test PASSED") + return 0 + else: + print("✗ Walk test FAILED") + return 1 + + elif args.stop: + print("Running stop test only...") + result = run_test("stop") + if result == 1: + print("✓ Stop test PASSED") + return 0 + else: + print("✗ Stop test FAILED") + return 1 + + elif args.eef_track: + print("Running eef track test only...") + result = run_test("eef_track") + if result == 1: + print("✓ Eef track test PASSED") + return 0 + else: + print("✗ Eef track test FAILED") + return 1 + + else: + print("Running both tests...") + squat_result = run_test("squat") + walk_result = run_test("walk") + + if squat_result == 1 and walk_result == 1: + print("✓ All tests PASSED") + return 0 + else: + print( + f"✗ Test results: squat={'PASSED' if squat_result == 1 else 'FAILED'}, " + f"walk={'PASSED' if walk_result == 1 else 'FAILED'}" + ) + return 1 + + +if __name__ == "__main__": + exit(main()) diff --git a/tests/control/main/test_data_exporter_loop.py b/tests/control/main/test_data_exporter_loop.py new file mode 100644 index 0000000..3f0708f --- /dev/null +++ b/tests/control/main/test_data_exporter_loop.py @@ -0,0 +1,403 @@ +import glob +import os +import tempfile +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest + +try: + from gr00t_wbc.control.main.teleop.run_g1_data_exporter import Gr00tDataCollector + from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model + from gr00t_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + from gr00t_wbc.data.exporter import Gr00tDataExporter + from gr00t_wbc.data.utils import get_dataset_features +except ModuleNotFoundError as e: + if "No module named 'rclpy'" in str(e): + pytestmark = pytest.mark.skip(reason="ROS (rclpy) is not installed") + else: + raise e + + +import json + +# How does mocking ROS work? +# +# This test file uses mocking to simulate a ROS environment without requiring actual ROS hardware: +# +# 1. ros_ok_side_effect: Controls how long the ROS loop runs by returning a sequence of +# True/False values. [True, True, False] means "run for 2 iterations then stop" +# +# 2. MockROSMsgSubscriber: Simulates sensors (camera/state) by returning pre-defined data: +# +# 3. MockKeyboardListenerSubscriber: Simulates user input: +# - 'c' = start/stop recording +# - 'd' = discard episode +# - KeyboardInterrupt = simulate Ctrl+C +# - None = no input +# +# 4. MockROSEnvironment: A context manager that patches all ROS dependencies to use our mocks, +# allowing us to test ROS-dependent code without actual ROS running. + + +class MockROSMsgSubscriber: + def __init__(self, return_value: list[dict]): + self.return_value = return_value + self.counter = 0 + + def get_image(self): + if self.counter < len(self.return_value): + self.counter += 1 + return self.return_value[self.counter - 1] + else: + return None + + def get_msg(self): + if self.counter < len(self.return_value): + self.counter += 1 + return self.return_value[self.counter - 1] + else: + return None + + +class MockKeyboardListenerSubscriber: + def __init__(self, return_value: list[str]): + self.return_value = return_value + self.counter = 0 + + def get_keyboard_input(self): + return self.return_value[self.counter] + + def read_msg(self): + if self.counter < len(self.return_value): + result = self.return_value[self.counter] + if isinstance(result, KeyboardInterrupt): + raise result + self.counter += 1 + return result + return None + + +class MockROSEnvironment: + """Context manager for mocking ROS environment and subscribers.""" + + def __init__(self, ok_side_effect, keyboard_listener, img_subscriber, state_subscriber): + self.ok_side_effect = ok_side_effect + self.keyboard_listener = keyboard_listener + self.img_subscriber = img_subscriber + self.state_subscriber = state_subscriber + self.patches = [] + + def __enter__(self): + self.patches = [ + patch("rclpy.init"), + patch("rclpy.create_node"), + patch("rclpy.spin"), + patch("rclpy.ok", side_effect=self.ok_side_effect), + patch("rclpy.shutdown"), + patch( + "gr00t_wbc.control.main.teleop.run_g1_data_exporter.KeyboardListenerSubscriber", + return_value=self.keyboard_listener, + ), + patch( + "gr00t_wbc.control.main.teleop.run_g1_data_exporter.ROSImgMsgSubscriber", + return_value=self.img_subscriber, + ), + patch( + "gr00t_wbc.control.main.teleop.run_g1_data_exporter.ROSMsgSubscriber", + return_value=self.state_subscriber, + ), + ] + + for p in self.patches: + p.start() + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + for p in reversed(self.patches): + p.stop() + return False + + +def verify_parquet_files_exist(file_path: str, num_episodes: int): + parquet_files = glob.glob(os.path.join(file_path, "data/chunk-*/episode_*.parquet")) + assert ( + len(parquet_files) == num_episodes + ), f"Expected {num_episodes} parquet files, but found {len(parquet_files)}" + + +def verify_video_files_exist(file_path: str, observation_keys: list[str], num_episodes: int): + for observation_key in observation_keys: + video_files = glob.glob( + os.path.join(file_path, f"videos/chunk-*/{observation_key}/episode_*.mp4") + ) + assert ( + len(video_files) == num_episodes + ), f"Expected {num_episodes} video files, but found {len(video_files)}" + + +def verify_metadata_files(file_path: str): + files_to_check = ["episodes.jsonl", "info.json", "tasks.jsonl", "modality.json"] + for file in files_to_check: + assert os.path.exists(os.path.join(file_path, "meta", file)), f"meta/{file} not created" + + +@pytest.fixture +def lerobot_features(): + robot_model = instantiate_g1_robot_model() + return get_dataset_features(robot_model) + + +@pytest.fixture +def modality_config(): + return { + "state": {"feature1": {"start": 0, "end": 4}, "feature2": {"start": 4, "end": 9}}, + "action": {"feature1": {"start": 0, "end": 4}, "feature2": {"start": 4, "end": 9}}, + "video": {"rs_view": {"original_key": "observation.images.ego_view"}}, + "annotation": {"human.task_description": {"original_key": "task_index"}}, + } + + +def _get_image_stream_data(episode_length: int, frame_rate: int, img_height: int, img_width: int): + return [ + { + "image": np.zeros((img_height, img_width, 3), dtype=np.uint8), + "timestamp": (i * 1 / frame_rate), + } + for i in range(episode_length) + ] + + +def _get_state_act_stream_data( + episode_length: int, frame_rate: int, state_dim: int, action_dim: int +): + return [ + { + "q": np.zeros(state_dim), + "action": np.zeros(action_dim), + "timestamp": (i * 1 / frame_rate), + "navigate_command": np.zeros(3, dtype=np.float64), + "base_height_command": 0.0, + "wrist_pose": np.zeros(14, dtype=np.float64), + "action.eef": np.zeros(14, dtype=np.float64), + } + for i in range(episode_length) + ] + + +def test_control_loop_happy_path_workflow(lerobot_features, modality_config): + """ + This test records a single episode and saves it to disk. + """ + episode_length = 10 + frame_rate = 20 + img_stream_data = _get_image_stream_data( + episode_length, frame_rate, RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + ) + robot_model = instantiate_g1_robot_model() + state_act_stream_data = _get_state_act_stream_data( + episode_length, frame_rate, robot_model.num_joints, robot_model.num_joints + ) + + keyboard_sub_output = [None for _ in range(episode_length)] + keyboard_sub_output[0] = "c" # Start recording + keyboard_sub_output[-1] = "c" # Stop recording and save + + # --------- Save the first episode --------- + mock_img_sub = MockROSMsgSubscriber(img_stream_data) + mock_state_sub = MockROSMsgSubscriber(state_act_stream_data) + mock_keyboard_listner = MockKeyboardListenerSubscriber(keyboard_sub_output) + + with tempfile.TemporaryDirectory() as temp_dir: + dataset_dir = os.path.join(temp_dir, "dataset") + + data_exporter = Gr00tDataExporter.create( + save_root=dataset_dir, + fps=frame_rate, + features=lerobot_features, + modality_config=modality_config, + task="test", + ) + + ros_ok_side_effect = [True] * (episode_length + 1) + [False] + with MockROSEnvironment( + ros_ok_side_effect, mock_keyboard_listner, mock_img_sub, mock_state_sub + ): + data_collector = Gr00tDataCollector( + camera_topic_name="mock_camera_topic", + state_topic_name="mock_state_topic", + data_exporter=data_exporter, + frequency=frame_rate, + ) + + # mocking to avoid actual sleeping + data_collector.rate = MagicMock() + + data_collector.run() + + verify_parquet_files_exist(dataset_dir, 1) + verify_video_files_exist(dataset_dir, data_exporter.meta.video_keys, 1) + verify_metadata_files(dataset_dir) + + # --------- Save the second episode --------- + # we reset the mock subscribers and re-run the control loop + # This immitates the case where the user starts recording a new episode on an existing dataset + mock_img_sub = MockROSMsgSubscriber(img_stream_data) + mock_state_sub = MockROSMsgSubscriber(state_act_stream_data) + ros_ok_side_effect = [True] * (episode_length + 1) + [False] + mock_keyboard_listner = MockKeyboardListenerSubscriber(keyboard_sub_output) + with MockROSEnvironment( + ros_ok_side_effect, mock_keyboard_listner, mock_img_sub, mock_state_sub + ): + data_collector = Gr00tDataCollector( + camera_topic_name="mock_camera_topic", + state_topic_name="mock_state_topic", + data_exporter=data_exporter, + frequency=frame_rate, + ) + + # mocking to avoid actual sleeping + data_collector.rate = MagicMock() + + data_collector.run() + + # now there should be 2 episodes in the dataset + verify_parquet_files_exist(dataset_dir, 2) + verify_video_files_exist(dataset_dir, data_exporter.meta.video_keys, 2) + verify_metadata_files(dataset_dir) + + +def test_control_loop_keyboard_interrupt_workflow(lerobot_features, modality_config): + """ + This test simulates a keyboard interruption in the middle of recording. + Expected behavior: + - The episode is saved to disk + - The episode is marked as discarded + """ + episode_length = 15 + frame_rate = 20 + img_stream_data = _get_image_stream_data( + episode_length, frame_rate, RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + ) + robot_model = instantiate_g1_robot_model() + state_act_stream_data = _get_state_act_stream_data( + episode_length, frame_rate, robot_model.num_joints, robot_model.num_joints + ) + + keyboard_sub_output = [None for _ in range(episode_length)] + keyboard_sub_output[0] = "c" # Start recording + keyboard_sub_output[5] = KeyboardInterrupt() # keyboard interruption in the middle of recording + + mock_img_sub = MockROSMsgSubscriber(img_stream_data) + mock_state_sub = MockROSMsgSubscriber(state_act_stream_data) + mock_keyboard_listener = MockKeyboardListenerSubscriber(keyboard_sub_output) + + with tempfile.TemporaryDirectory() as temp_dir: + dataset_dir = os.path.join(temp_dir, "dataset") + + data_exporter = Gr00tDataExporter.create( + save_root=dataset_dir, + fps=frame_rate, + features=lerobot_features, + modality_config=modality_config, + task="test", + ) + + ros_ok_side_effect = [True] * episode_length + [False] + with MockROSEnvironment( + ros_ok_side_effect, mock_keyboard_listener, mock_img_sub, mock_state_sub + ): + data_collector = Gr00tDataCollector( + camera_topic_name="mock_camera_topic", + state_topic_name="mock_state_topic", + data_exporter=data_exporter, + frequency=frame_rate, + ) + + data_collector.rate = MagicMock() + # try: + data_collector.run() + # except KeyboardInterrupt: + # pass + + verify_parquet_files_exist(dataset_dir, 1) + verify_video_files_exist(dataset_dir, data_exporter.meta.video_keys, 1) + verify_metadata_files(dataset_dir) + + # verify that the episode is marked as discarded + ep_info = json.load(open(os.path.join(dataset_dir, "meta", "info.json"))) + assert ep_info["discarded_episode_indices"][0] == 0 + assert ep_info["total_frames"] == 5 + assert ep_info["total_episodes"] == 1 + + +def test_discarded_episode_workflow(lerobot_features, modality_config): + """ + This test simulates a case where the user discards an episode in the middle of recording. + Expected behavior: + - Record 3 episodes, discard episode 0 and 2 + - There should be 3 episodes saved to disk + - Episode 0 and 2 should be flagged as discarded + """ + episode_length = 17 + frame_rate = 20 + robot_model = instantiate_g1_robot_model() + state_dim = robot_model.num_joints + action_dim = robot_model.num_joints + img_stream_data = _get_image_stream_data( + episode_length, frame_rate, RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + ) + state_act_stream_data = _get_state_act_stream_data( + episode_length, frame_rate, state_dim, action_dim + ) + + keyboard_sub_output = [None for _ in range(episode_length)] + keyboard_sub_output[0] = "c" # Start recording episode index 0 + keyboard_sub_output[5] = "x" # Discard episode index 0 + keyboard_sub_output[7] = "c" # Start recording episode index 1 + keyboard_sub_output[10] = "c" # stop recording and save episode index 1 + keyboard_sub_output[12] = "c" # start recording episode index 2 + keyboard_sub_output[15] = "x" # discard episode index 2 + + mock_img_sub = MockROSMsgSubscriber(img_stream_data) + mock_state_sub = MockROSMsgSubscriber(state_act_stream_data) + mock_keyboard_listener = MockKeyboardListenerSubscriber(keyboard_sub_output) + + with tempfile.TemporaryDirectory() as temp_dir: + dataset_dir = os.path.join(temp_dir, "dataset") + + data_exporter = Gr00tDataExporter.create( + save_root=dataset_dir, + fps=frame_rate, + features=lerobot_features, + modality_config=modality_config, + task="test", + ) + + ros_ok_side_effect = [True] * episode_length + [False] + with MockROSEnvironment( + ros_ok_side_effect, mock_keyboard_listener, mock_img_sub, mock_state_sub + ): + data_collector = Gr00tDataCollector( + camera_topic_name="mock_camera_topic", + state_topic_name="mock_state_topic", + data_exporter=data_exporter, + frequency=frame_rate, + ) + + data_collector.rate = MagicMock() + try: + data_collector.run() + except Exception: + pass + + # vrify if the episode is marked as discarded + ep_info = json.load(open(os.path.join(dataset_dir, "meta", "info.json"))) + assert len(ep_info["discarded_episode_indices"]) == 2 + assert ep_info["discarded_episode_indices"][0] == 0 + assert ep_info["discarded_episode_indices"][1] == 2 + + # verify that all episodes are saved regardless of being discarded + verify_parquet_files_exist(dataset_dir, 3) + verify_video_files_exist(dataset_dir, data_exporter.meta.video_keys, 3) + verify_metadata_files(dataset_dir) diff --git a/tests/control/policy/__init__.py b/tests/control/policy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/policy/interpolation_policy/__init__.py b/tests/control/policy/interpolation_policy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/policy/interpolation_policy/test_interpolation_policy.py b/tests/control/policy/interpolation_policy/test_interpolation_policy.py new file mode 100644 index 0000000..c3afff6 --- /dev/null +++ b/tests/control/policy/interpolation_policy/test_interpolation_policy.py @@ -0,0 +1,47 @@ +from pathlib import Path +import pickle + +import numpy as np +import pytest + +from gr00t_wbc.control.policy.interpolation_policy import ( + InterpolationPolicy, +) + + +def get_test_data_path(filename: str) -> str: + """Get the absolute path to a test data file.""" + test_dir = Path(__file__).parent + return str(test_dir / ".." / ".." / ".." / "replay_data" / filename) + + +@pytest.fixture +def logged_data(): + """Load the logged data from file.""" + data_path = get_test_data_path("interpolation_data.pkl") + with open(data_path, "rb") as f: + return pickle.load(f) + + +def test_replay_logged_data(logged_data): + """Test that the wrapper produces the same pose commands as logged data.""" + init_args = logged_data["init_args"] + interp = InterpolationPolicy( + init_time=init_args["curr_t"], + init_values={"target_pose": init_args["curr_pose"]}, + max_change_rate=np.inf, + ) + + # Test all data points including the first one + for c in logged_data["calls"]: + # Get the action from wrapper + if c["type"] == "get_action": + action = interp.get_action(**c["args"]) + expected_action = c["result"] + np.testing.assert_allclose( + action["target_pose"], expected_action["q"], rtol=1e-9, atol=1e-9 + ) + # print(action, expected_action) + + else: + interp.set_goal(**c["args"]) diff --git a/tests/control/policy/interpolation_policy/test_interpolation_ramp_up.py b/tests/control/policy/interpolation_policy/test_interpolation_ramp_up.py new file mode 100644 index 0000000..118f9d2 --- /dev/null +++ b/tests/control/policy/interpolation_policy/test_interpolation_ramp_up.py @@ -0,0 +1,78 @@ +import numpy as np +import pytest + +from gr00t_wbc.control.policy.interpolation_policy import ( + InterpolationPolicy, +) + + +def test_trajectory_interpolation(): + """ + Test that the InterpolationPolicy correctly interpolates between waypoints. + + Initial pose is at all zeros. + At t=4sec, the index 27 position (right_shoulder_yaw_joint) should be -1.5. + We run at 100Hz to see all intermediate waypoints. + + Notes: + - The trajectory data is at 'trajectory_data.npy' in the current directory + - The visualization is at 'trajectory.png' in the current directory + """ + # Create a pose with 32 joints (all zeros initially) + num_joints = 32 + initial_pose = np.zeros(num_joints) + + # Initial time (use a fixed value for reproducibility) + initial_time = 0.0 + + # Create the wrapper with initial pose + interpolator = InterpolationPolicy( + init_time=initial_time, + init_values={"target_pose": initial_pose}, + max_change_rate=np.inf, + ) + + # Target pose: all zeros except index 27 which should be -1.5 + target_pose = np.zeros(num_joints) + target_pose[27] = -1.5 # right_shoulder_yaw_joint + target_time = 4.0 # 4 seconds from now + + # Set the planner command to schedule the waypoint + interpolator.set_goal( + { + "target_pose": target_pose, + "target_time": target_time, + "interpolation_garbage_collection_time": initial_time, + } + ) + + # Sample the trajectory at 100Hz + frequency = 100 + dt = 1.0 / frequency + sample_times = np.arange(initial_time, target_time + dt, dt) + + # Collect the interpolated poses + sampled_poses = [] + for t in sample_times: + action = interpolator.get_action(t) + sampled_poses.append(action["target_pose"]) + + # Convert to numpy array for easier analysis + sampled_poses = np.array(sampled_poses) + + # Check specific requirements + # Verify we actually moved from 0 to -1.5 + joint_27_positions = sampled_poses[:, 27] + assert joint_27_positions[0] == pytest.approx(0.0) + assert joint_27_positions[-1] == pytest.approx(-1.5) + + # Calculate the absolute changes between each step + changes = np.abs(np.diff(joint_27_positions)) + assert np.all(changes < 0.004), "Joint 27 position should change by less than 0.004" + + # Print some statistics about the trajectory + print(f"Total time steps: {len(sample_times)}") + print(f"Joint 27 trajectory start: {joint_27_positions[0]}") + print(f"Joint 27 trajectory end: {joint_27_positions[-1]}") + print(f"Joint 27 max velocity: {np.max(np.abs(np.diff(joint_27_positions) / dt))}") + print(f"Max velocity timestep: {np.argmax(np.abs(np.diff(joint_27_positions) / dt))}") diff --git a/tests/control/policy/interpolation_policy/trajectory.png b/tests/control/policy/interpolation_policy/trajectory.png new file mode 100644 index 0000000..765a915 --- /dev/null +++ b/tests/control/policy/interpolation_policy/trajectory.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1f93f1951cc46e952774d6d9beeddfb4ee34a7b52a67e19737ad6f48ab03ddf +size 34753 diff --git a/tests/control/policy/interpolation_policy/trajectory_data.npy b/tests/control/policy/interpolation_policy/trajectory_data.npy new file mode 100644 index 0000000..6dc8ece Binary files /dev/null and b/tests/control/policy/interpolation_policy/trajectory_data.npy differ diff --git a/tests/control/robot_model/__init__.py b/tests/control/robot_model/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/robot_model/robot_model_test.py b/tests/control/robot_model/robot_model_test.py new file mode 100644 index 0000000..cb279a4 --- /dev/null +++ b/tests/control/robot_model/robot_model_test.py @@ -0,0 +1,911 @@ +# test_robot_model.py + +import numpy as np +import pinocchio as pin +import pytest + +from gr00t_wbc.control.robot_model import ReducedRobotModel +from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model + + +@pytest.fixture +def g1_robot_model(): + """ + Fixture that creates and returns a G1 RobotModel instance. + """ + return instantiate_g1_robot_model() + + +def test_robot_model_initialization(g1_robot_model): + """ + Test initialization of the RobotModel and its main attributes. + """ + for robot_model in [g1_robot_model]: + # Check that the Pinocchio wrapper exists + assert robot_model.pinocchio_wrapper is not None + + # Check number of degrees of freedom (nq) + assert robot_model.num_dofs > 0 + + # Check we have the expected number of joints beyond the floating base + assert len(robot_model.joint_names) > 0 + + # Check that supplemental info is present + assert robot_model.supplemental_info is not None + + +def test_robot_model_joint_names(g1_robot_model): + """ + Test that joint_names is populated correctly + and that dof_index works. + """ + for robot_model in [g1_robot_model]: + # Extract joint names + joint_names = robot_model.joint_names + + # Pick the first joint name and get its index + first_joint_name = joint_names[0] + idx = robot_model.dof_index(first_joint_name) + assert idx >= 0 + + # Test that an unknown joint name raises an error + with pytest.raises(ValueError, match="Unknown joint name"): + _ = robot_model.dof_index("non_existent_joint") + + +def test_robot_model_forward_kinematics_valid_q(g1_robot_model): + """ + Test that cache_forward_kinematics works with a valid q. + """ + for robot_model in [g1_robot_model]: + nq = robot_model.num_dofs + + # Construct a valid configuration (e.g., zero vector) + q_valid = np.zeros(nq) + + # Should not raise any exception + robot_model.cache_forward_kinematics(q_valid) + + +def test_robot_model_forward_kinematics_invalid_q(g1_robot_model): + """ + Test that cache_forward_kinematics raises an error with an invalid q. + """ + for robot_model in [g1_robot_model]: + nq = robot_model.num_dofs + + # Construct an invalid configuration (wrong size) + q_invalid = np.zeros(nq + 1) + + with pytest.raises(ValueError, match="Expected q of length"): + robot_model.cache_forward_kinematics(q_invalid) + + +def test_robot_model_frame_placement(g1_robot_model): + """ + Test the frame_placement method with a valid and invalid frame name. + Also test that frame placements change with different configurations. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing") + + # Use the hand frame from supplemental info + test_frame = robot_model.supplemental_info.hand_frame_names["left"] + + # Test with zero configuration + q_zero = np.zeros(robot_model.num_dofs) + robot_model.cache_forward_kinematics(q_zero) + placement_zero = robot_model.frame_placement(test_frame) + assert isinstance(placement_zero, pin.SE3) + + # Test with non-zero configuration + q_non_zero = np.zeros(robot_model.num_dofs) + root_nq = 7 if robot_model.is_floating_base_model else 0 + + # Set a more significant configuration change + # Use π/2 for all joints to create a more noticeable difference + q_non_zero[root_nq:] = np.pi / 2 # 90 degrees for all joints + + robot_model.cache_forward_kinematics(q_non_zero) + placement_non_zero = robot_model.frame_placement(test_frame) + + # Verify that frame placements are different with different configurations + assert not np.allclose( + placement_zero.translation, placement_non_zero.translation + ) or not np.allclose(placement_zero.rotation, placement_non_zero.rotation) + + # Should raise an error for an invalid frame + with pytest.raises(ValueError, match="Unknown frame"): + robot_model.frame_placement("non_existent_frame") + + +# Tests for ReducedRobotModel +def test_reduced_robot_model_initialization(g1_robot_model): + """ + Test initialization of the ReducedRobotModel. + """ + for robot_model in [g1_robot_model]: + # Create a reduced model by fixing some actual joints from the robot + fixed_joints = robot_model.joint_names[:2] # Use first two joints from the robot + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Check that the full robot is stored + assert reduced_robot.full_robot is robot_model + + # Check that fixed joints are stored correctly + assert reduced_robot.fixed_joints == fixed_joints + assert len(reduced_robot.fixed_values) == len(fixed_joints) + + # Check that the number of dofs is reduced + assert reduced_robot.num_dofs == robot_model.num_dofs - len(fixed_joints) + + +def test_reduced_robot_model_joint_names(g1_robot_model): + """ + Test that joint_names in ReducedRobotModel excludes fixed joints. + """ + for robot_model in [g1_robot_model]: + # Use actual joints from the robot + fixed_joints = robot_model.joint_names[:2] # Use first two joints from the robot + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Check that fixed joints are not in the reduced model's joint names + for joint in fixed_joints: + assert joint not in reduced_robot.joint_names + + # Check that other joints are still present + for joint in robot_model.joint_names: + if joint not in fixed_joints: + assert joint in reduced_robot.joint_names + + +def test_reduced_robot_model_configuration_conversion(g1_robot_model): + """ + Test conversion between reduced and full configurations. + """ + for robot_model in [g1_robot_model]: + # Use actual joints from the robot + fixed_joints = robot_model.joint_names[:2] # Use first two joints from the robot + fixed_values = [0.5, 1.0] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints, fixed_values) + + # Create a reduced configuration + q_reduced = np.zeros(reduced_robot.num_dofs) + q_reduced[0] = 0.3 # Set some value for testing + + # Convert to full configuration + q_full = reduced_robot.reduced_to_full_configuration(q_reduced) + + # Check that fixed joints have the correct values + for joint_name, value in zip(fixed_joints, fixed_values): + full_idx = robot_model.dof_index(joint_name) + assert q_full[full_idx] == value + + # Convert back to reduced configuration + q_reduced_back = reduced_robot.full_to_reduced_configuration(q_full) + + # Check that the conversion is reversible + np.testing.assert_array_almost_equal(q_reduced, q_reduced_back) + + +def test_reduced_robot_model_forward_kinematics(g1_robot_model): + """ + Test forward kinematics with the reduced model. + """ + for robot_model in [g1_robot_model]: + # Use actual joints from the robot + fixed_joints = robot_model.joint_names[:2] # Use first two joints from the robot + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Create a reduced configuration + q_reduced = np.zeros(reduced_robot.num_dofs) + + # Should not raise any exception + reduced_robot.cache_forward_kinematics(q_reduced) + + # Check that frame placement works + model = robot_model.pinocchio_wrapper.model + if len(model.frames) > 1: + valid_frame = model.frames[1].name + placement = reduced_robot.frame_placement(valid_frame) + assert isinstance(placement, pin.SE3) + + +def test_robot_model_clip_configuration(g1_robot_model): + """ + Test that clip_configuration properly clips values to joint limits. + """ + for robot_model in [g1_robot_model]: + # Create a configuration with some values outside limits + q = np.zeros(robot_model.num_dofs) + root_nq = 7 if robot_model.is_floating_base_model else 0 + # Create extreme values for all joints + q[root_nq:] = np.array([100.0, -100.0, 50.0, -50.0] * (robot_model.num_joints // 4 + 1))[ + : robot_model.num_joints + ] + + # Clip the configuration + q_clipped = robot_model.clip_configuration(q) + + # Check that values are within limits + assert np.all(q_clipped[root_nq:] <= robot_model.upper_joint_limits) + assert np.all(q_clipped[root_nq:] >= robot_model.lower_joint_limits) + + +def test_robot_model_get_actuated_joints(g1_robot_model): + """ + Test getting body and hand actuated joints from configuration. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing actuated joints") + + # Create a test configuration + q = np.zeros(robot_model.num_dofs) + root_nq = 7 if robot_model.is_floating_base_model else 0 + q[root_nq:] = np.arange(robot_model.num_joints) # Set some values for joints + + # Test body actuated joints + body_joints = robot_model.get_body_actuated_joints(q) + assert len(body_joints) == len(robot_model.get_body_actuated_joint_indices()) + + # Test hand actuated joints + hand_joints = robot_model.get_hand_actuated_joints(q) + assert len(hand_joints) == len(robot_model.get_hand_actuated_joint_indices()) + + # Test left hand joints + left_hand_joints = robot_model.get_hand_actuated_joints(q, side="left") + assert len(left_hand_joints) == len(robot_model.get_hand_actuated_joint_indices("left")) + + # Test right hand joints + right_hand_joints = robot_model.get_hand_actuated_joints(q, side="right") + assert len(right_hand_joints) == len(robot_model.get_hand_actuated_joint_indices("right")) + + +def test_robot_model_get_configuration_from_actuated_joints(g1_robot_model): + """ + Test creating full configuration from actuated joint values. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing actuated joints") + + # Create test values for body and hands + body_values = np.ones(len(robot_model.get_body_actuated_joint_indices())) + hand_values = np.ones(len(robot_model.get_hand_actuated_joint_indices())) + left_hand_values = np.ones(len(robot_model.get_hand_actuated_joint_indices("left"))) + right_hand_values = np.ones(len(robot_model.get_hand_actuated_joint_indices("right"))) + + # Test with combined hand values + q = robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_values, hand_actuated_joint_values=hand_values + ) + assert q.shape == (robot_model.num_dofs,) + + # Test with separate hand values + q = robot_model.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_values, + left_hand_actuated_joint_values=left_hand_values, + right_hand_actuated_joint_values=right_hand_values, + ) + assert q.shape == (robot_model.num_dofs,) + + +def test_robot_model_reset_forward_kinematics(g1_robot_model): + """ + Test resetting forward kinematics to default configuration. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing") + + # Create a more significant configuration change + q = np.zeros(robot_model.num_dofs) + root_nq = 7 if robot_model.is_floating_base_model else 0 + # Set some extreme joint angles + q[root_nq:] = np.pi / 2 # 90 degrees for all joints + robot_model.cache_forward_kinematics(q) + + # Use a hand frame from supplemental info + test_frame = robot_model.supplemental_info.hand_frame_names["left"] + + # Reset to default + robot_model.reset_forward_kinematics() + # Get frame placement after reset + placement_default = robot_model.frame_placement(test_frame) + + # Check that frame placement matches what we get with q_zero + robot_model.cache_forward_kinematics(robot_model.q_zero) + placement_q_zero = robot_model.frame_placement(test_frame) + np.testing.assert_array_almost_equal( + placement_default.translation, placement_q_zero.translation + ) + np.testing.assert_array_almost_equal(placement_default.rotation, placement_q_zero.rotation) + + +# Additional tests for ReducedRobotModel +def test_reduced_robot_model_clip_configuration(g1_robot_model): + """ + Test that clip_configuration works in reduced space. + """ + for robot_model in [g1_robot_model]: + fixed_joints = robot_model.joint_names[:2] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Create a configuration with some values outside limits + q_reduced = np.zeros(reduced_robot.num_dofs) + root_nq = 7 if reduced_robot.full_robot.is_floating_base_model else 0 + # Create extreme values for all joints + q_reduced[root_nq:] = np.array( + [100.0, -100.0, 50.0, -50.0] * (reduced_robot.num_joints // 4 + 1) + )[: reduced_robot.num_joints] + + # Clip the configuration + q_clipped = reduced_robot.clip_configuration(q_reduced) + + # Check that values are within limits + assert np.all(q_clipped[root_nq:] <= reduced_robot.upper_joint_limits) + assert np.all(q_clipped[root_nq:] >= reduced_robot.lower_joint_limits) + + +def test_reduced_robot_model_get_actuated_joints(g1_robot_model): + """ + Test getting body and hand actuated joints from reduced configuration. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing actuated joints") + + fixed_joints = robot_model.joint_names[:2] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Create a test configuration + q_reduced = np.zeros(reduced_robot.num_dofs) + root_nq = 7 if reduced_robot.full_robot.is_floating_base_model else 0 + q_reduced[root_nq:] = np.arange(reduced_robot.num_joints) + + # Test body actuated joints + body_joints = reduced_robot.get_body_actuated_joints(q_reduced) + assert len(body_joints) == len(reduced_robot.get_body_actuated_joint_indices()) + + # Test hand actuated joints + hand_joints = reduced_robot.get_hand_actuated_joints(q_reduced) + assert len(hand_joints) == len(reduced_robot.get_hand_actuated_joint_indices()) + + +def test_reduced_robot_model_get_configuration_from_actuated_joints(g1_robot_model): + """ + Test creating reduced configuration from actuated joint values. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing actuated joints") + + fixed_joints = robot_model.joint_names[:2] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Create test values for body and hands + body_values = np.ones(len(reduced_robot.get_body_actuated_joint_indices())) + hand_values = np.ones(len(reduced_robot.get_hand_actuated_joint_indices())) + left_hand_values = np.ones(len(reduced_robot.get_hand_actuated_joint_indices("left"))) + right_hand_values = np.ones(len(reduced_robot.get_hand_actuated_joint_indices("right"))) + + # Test with combined hand values + q_reduced = reduced_robot.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_values, hand_actuated_joint_values=hand_values + ) + assert q_reduced.shape == (reduced_robot.num_dofs,) + + # Test with separate hand values + q_reduced = reduced_robot.get_configuration_from_actuated_joints( + body_actuated_joint_values=body_values, + left_hand_actuated_joint_values=left_hand_values, + right_hand_actuated_joint_values=right_hand_values, + ) + assert q_reduced.shape == (reduced_robot.num_dofs,) + + # Verify that the values were set correctly in the reduced configuration + # Check body actuated joints + body_indices = reduced_robot.get_body_actuated_joint_indices() + np.testing.assert_array_almost_equal(q_reduced[body_indices], body_values) + + # Check left hand actuated joints + left_hand_indices = reduced_robot.get_hand_actuated_joint_indices("left") + np.testing.assert_array_almost_equal(q_reduced[left_hand_indices], left_hand_values) + + # Check right hand actuated joints + right_hand_indices = reduced_robot.get_hand_actuated_joint_indices("right") + np.testing.assert_array_almost_equal(q_reduced[right_hand_indices], right_hand_values) + + +def test_reduced_robot_model_reset_forward_kinematics(g1_robot_model): + """ + Test resetting forward kinematics in reduced model. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing") + + fixed_joints = robot_model.joint_names[:2] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Create a more significant configuration change + q_reduced = np.zeros(reduced_robot.num_dofs) + root_nq = 7 if reduced_robot.full_robot.is_floating_base_model else 0 + # Set some extreme joint angles + q_reduced[root_nq:] = np.pi / 2 # 90 degrees for all joints + reduced_robot.cache_forward_kinematics(q_reduced) + + # Reset to default + reduced_robot.reset_forward_kinematics() + + # Check that frame placement matches what we get with q_zero + reduced_robot.cache_forward_kinematics(reduced_robot.q_zero) + placement_q_zero = reduced_robot.frame_placement( + reduced_robot.supplemental_info.hand_frame_names["left"] + ) + placement_reset = reduced_robot.frame_placement( + reduced_robot.supplemental_info.hand_frame_names["left"] + ) + np.testing.assert_array_almost_equal( + placement_reset.translation, placement_q_zero.translation + ) + np.testing.assert_array_almost_equal(placement_reset.rotation, placement_q_zero.rotation) + + +def test_reduced_robot_model_from_fixed_groups(g1_robot_model): + """ + Test creating reduced model from fixed joint groups. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing joint groups") + + # Get a group name from the supplemental info + group_name = next(iter(robot_model.supplemental_info.joint_groups.keys())) + group_info = robot_model.supplemental_info.joint_groups[group_name] + + # Get all joints that should be fixed (including those from subgroups) + expected_fixed_joints = set() + # Add direct joints + expected_fixed_joints.update(group_info["joints"]) + # Add joints from subgroups + for subgroup_name in group_info["groups"]: + subgroup_joints = robot_model.get_joint_group_indices(subgroup_name) + expected_fixed_joints.update([robot_model.joint_names[idx] for idx in subgroup_joints]) + + # Test from_fixed_groups + reduced_robot = ReducedRobotModel.from_fixed_groups(robot_model, [group_name]) + assert reduced_robot.full_robot is robot_model + + # Verify that fixed joints are not in reduced model's joint names + for joint in expected_fixed_joints: + assert joint not in reduced_robot.joint_names + + # Verify that fixed joints maintain their values in configuration + q_reduced = np.ones(reduced_robot.num_dofs) # Set some non-zero values + q_full = reduced_robot.reduced_to_full_configuration(q_reduced) + + # Get the fixed values from the reduced model + fixed_values = dict(zip(reduced_robot.fixed_joints, reduced_robot.fixed_values)) + + # Check that all expected fixed joints have their values preserved + for joint in expected_fixed_joints: + full_idx = robot_model.dof_index(joint) + assert q_full[full_idx] == fixed_values[joint] + + # Test from_fixed_group (convenience method) + reduced_robot = ReducedRobotModel.from_fixed_group(robot_model, group_name) + assert reduced_robot.full_robot is robot_model + + # Verify that fixed joints are not in reduced model's joint names + for joint in expected_fixed_joints: + assert joint not in reduced_robot.joint_names + + # Verify that fixed joints maintain their values in configuration + q_reduced = np.ones(reduced_robot.num_dofs) # Set some non-zero values + q_full = reduced_robot.reduced_to_full_configuration(q_reduced) + + # Get the fixed values from the reduced model + fixed_values = dict(zip(reduced_robot.fixed_joints, reduced_robot.fixed_values)) + + # Check that all expected fixed joints have their values preserved + for joint in expected_fixed_joints: + full_idx = robot_model.dof_index(joint) + assert q_full[full_idx] == fixed_values[joint] + + +def test_reduced_robot_model_from_active_groups(g1_robot_model): + """ + Test creating reduced model from active joint groups. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing joint groups") + + # Get a group name from the supplemental info + group_name = next(iter(robot_model.supplemental_info.joint_groups.keys())) + group_info = robot_model.supplemental_info.joint_groups[group_name] + + # Get all joints that should be active (including those from subgroups) + expected_active_joints = set() + # Add direct joints + expected_active_joints.update(group_info["joints"]) + # Add joints from subgroups + for subgroup_name in group_info["groups"]: + subgroup_joints = robot_model.get_joint_group_indices(subgroup_name) + expected_active_joints.update([robot_model.joint_names[idx] for idx in subgroup_joints]) + + # Get all joints from the model + all_joints = set(robot_model.joint_names) + # The fixed joints should be all joints minus the active joints + expected_fixed_joints = all_joints - expected_active_joints + + # Test from_active_groups + reduced_robot = ReducedRobotModel.from_active_groups(robot_model, [group_name]) + assert reduced_robot.full_robot is robot_model + + # Verify that active joints are in reduced model's joint names + for joint in expected_active_joints: + assert joint in reduced_robot.joint_names + + # Verify that fixed joints are not in reduced model's joint names + for joint in expected_fixed_joints: + assert joint not in reduced_robot.joint_names + + # Verify that fixed joints maintain their values in configuration + q_reduced = np.ones(reduced_robot.num_dofs) # Set some non-zero values + q_full = reduced_robot.reduced_to_full_configuration(q_reduced) + + # Get the fixed values from the reduced model + fixed_values = dict(zip(reduced_robot.fixed_joints, reduced_robot.fixed_values)) + + # Check that all expected fixed joints have their values preserved + for joint in expected_fixed_joints: + full_idx = robot_model.dof_index(joint) + assert q_full[full_idx] == fixed_values[joint] + + # Test from_active_group (convenience method) + reduced_robot = ReducedRobotModel.from_active_group(robot_model, group_name) + assert reduced_robot.full_robot is robot_model + + # Verify that active joints are in reduced model's joint names + for joint in expected_active_joints: + assert joint in reduced_robot.joint_names + + # Verify that fixed joints are not in reduced model's joint names + for joint in expected_fixed_joints: + assert joint not in reduced_robot.joint_names + + # Verify that fixed joints maintain their values in configuration + q_reduced = np.ones(reduced_robot.num_dofs) # Set some non-zero values + q_full = reduced_robot.reduced_to_full_configuration(q_reduced) + + # Get the fixed values from the reduced model + fixed_values = dict(zip(reduced_robot.fixed_joints, reduced_robot.fixed_values)) + + # Check that all expected fixed joints have their values preserved + for joint in expected_fixed_joints: + full_idx = robot_model.dof_index(joint) + assert q_full[full_idx] == fixed_values[joint] + + +def test_reduced_robot_model_frame_placement(g1_robot_model): + """ + Test the frame_placement method in reduced model with a valid and invalid frame name. + Also test that frame placements change with different configurations. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing") + + # Create a reduced model by fixing some joints + fixed_joints = robot_model.joint_names[:2] + reduced_robot = ReducedRobotModel(robot_model, fixed_joints) + + # Use the hand frame from supplemental info + test_frame = reduced_robot.supplemental_info.hand_frame_names["left"] + + # Test with zero configuration + q_reduced_zero = np.zeros(reduced_robot.num_dofs) + reduced_robot.cache_forward_kinematics(q_reduced_zero) + placement_zero = reduced_robot.frame_placement(test_frame) + assert isinstance(placement_zero, pin.SE3) + + # Test with non-zero configuration + q_reduced_non_zero = np.zeros(reduced_robot.num_dofs) + root_nq = 7 if reduced_robot.full_robot.is_floating_base_model else 0 + + # Set a valid non-zero value for each joint + for i in range(root_nq, reduced_robot.num_dofs): + # Use a value that's within the joint limits + q_reduced_non_zero[i] = 0.5 # 0.5 radians is within most joint limits + + reduced_robot.cache_forward_kinematics(q_reduced_non_zero) + placement_non_zero = reduced_robot.frame_placement(test_frame) + + # Verify that frame placements are different with different configurations + assert not np.allclose( + placement_zero.translation, placement_non_zero.translation + ) or not np.allclose(placement_zero.rotation, placement_non_zero.rotation) + + # Should raise an error for an invalid frame + with pytest.raises(ValueError, match="Unknown frame"): + reduced_robot.frame_placement("non_existent_frame") + + +def test_robot_model_gravity_compensation_basic(g1_robot_model): + """ + Test basic gravity compensation functionality. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + # Create a valid configuration + q = np.zeros(robot_model.num_dofs) + if robot_model.is_floating_base_model: + # Set floating base to upright position + q[:7] = [0, 0, 1.0, 0, 0, 0, 1] # [x, y, z, qx, qy, qz, qw] + + # Test gravity compensation for all joints + gravity_torques = robot_model.compute_gravity_compensation_torques(q) + + # Check output shape + assert gravity_torques.shape == (robot_model.num_dofs,) + + # For a humanoid robot with arms, there should be some non-zero gravity torques + assert np.any(np.abs(gravity_torques) > 1e-6), "Expected some non-zero gravity torques" + + +def test_robot_model_gravity_compensation_joint_groups(g1_robot_model): + """ + Test gravity compensation with different joint group specifications. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + # Create a valid configuration + q = np.zeros(robot_model.num_dofs) + if robot_model.is_floating_base_model: + q[:7] = [0, 0, 1.0, 0, 0, 0, 1] + + # Get available joint groups + available_groups = list(robot_model.supplemental_info.joint_groups.keys()) + if not available_groups: + pytest.skip("No joint groups available for testing") + + test_group = available_groups[0] # Use first available group + + # Test with string input + gravity_str = robot_model.compute_gravity_compensation_torques(q, test_group) + assert gravity_str.shape == (robot_model.num_dofs,) + + # Test with list input + gravity_list = robot_model.compute_gravity_compensation_torques(q, [test_group]) + np.testing.assert_array_equal(gravity_str, gravity_list) + + # Test with set input + gravity_set = robot_model.compute_gravity_compensation_torques(q, {test_group}) + np.testing.assert_array_equal(gravity_str, gravity_set) + + # Test that compensation is selective (some joints should be zero) + group_indices = robot_model.get_joint_group_indices(test_group) + if len(group_indices) < robot_model.num_dofs: + # Check that only specified joints have compensation + non_zero_mask = np.abs(gravity_str) > 1e-6 + compensated_indices = np.where(non_zero_mask)[0] + # The compensated indices should be a subset of the group indices + assert len(compensated_indices) <= len(group_indices) + + +def test_robot_model_gravity_compensation_multiple_groups(g1_robot_model): + """ + Test gravity compensation with multiple joint groups. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + # Create a valid configuration + q = np.zeros(robot_model.num_dofs) + if robot_model.is_floating_base_model: + q[:7] = [0, 0, 1.0, 0, 0, 0, 1] + + # Get available joint groups + available_groups = list(robot_model.supplemental_info.joint_groups.keys()) + if len(available_groups) < 2: + pytest.skip("Need at least 2 joint groups for testing") + + # Test with multiple groups + test_groups = available_groups[:2] + gravity_multiple = robot_model.compute_gravity_compensation_torques(q, test_groups) + assert gravity_multiple.shape == (robot_model.num_dofs,) + + # Test individual groups + gravity_1 = robot_model.compute_gravity_compensation_torques(q, test_groups[0]) + gravity_2 = robot_model.compute_gravity_compensation_torques(q, test_groups[1]) + + # The multiple group result should have at least as many non-zero elements + # as either individual group (could be more due to overlaps) + nonzero_multiple = np.count_nonzero(np.abs(gravity_multiple) > 1e-6) + nonzero_1 = np.count_nonzero(np.abs(gravity_1) > 1e-6) + nonzero_2 = np.count_nonzero(np.abs(gravity_2) > 1e-6) + assert nonzero_multiple >= max(nonzero_1, nonzero_2) + + +def test_robot_model_gravity_compensation_configuration_dependency(g1_robot_model): + """ + Test that gravity compensation changes with robot configuration. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + # Get available joint groups - prefer arms if available + available_groups = list(robot_model.supplemental_info.joint_groups.keys()) + test_group = None + for group in ["arms", "left_arm", "right_arm"]: + if group in available_groups: + test_group = group + break + if test_group is None and available_groups: + test_group = available_groups[0] + if test_group is None: + pytest.skip("No joint groups available for testing") + + # Test with different configurations + q1 = np.zeros(robot_model.num_dofs) + q2 = np.zeros(robot_model.num_dofs) + + if robot_model.is_floating_base_model: + # Both configurations upright but different joint positions + q1[:7] = [0, 0, 1.0, 0, 0, 0, 1] + q2[:7] = [0, 0, 1.0, 0, 0, 0, 1] + + # Change arm joint positions specifically (not random joints) + # This ensures we actually change joints that affect the gravity compensation + try: + arm_indices = robot_model.get_joint_group_indices(test_group) + if len(arm_indices) >= 2: + # Change first two arm joints significantly + q2[arm_indices[0]] = np.pi / 4 # 45 degrees + q2[arm_indices[1]] = np.pi / 6 # 30 degrees + elif len(arm_indices) >= 1: + # Change first arm joint if only one available + q2[arm_indices[0]] = np.pi / 3 # 60 degrees + except Exception: + # Fallback to changing some joints if arm indices not available + if robot_model.is_floating_base_model and robot_model.num_dofs > 9: + q2[7] = np.pi / 4 + q2[8] = np.pi / 6 + elif not robot_model.is_floating_base_model and robot_model.num_dofs > 2: + q2[0] = np.pi / 4 + q2[1] = np.pi / 6 + + # Compute gravity compensation for both configurations + gravity_1 = robot_model.compute_gravity_compensation_torques(q1, test_group) + gravity_2 = robot_model.compute_gravity_compensation_torques(q2, test_group) + + # They should be different (unless all compensated joints didn't change) + # Allow for small numerical differences + assert not np.allclose( + gravity_1, gravity_2, atol=1e-10 + ), "Gravity compensation should change with configuration" + + +def test_robot_model_gravity_compensation_error_handling(g1_robot_model): + """ + Test error handling in gravity compensation. + """ + for robot_model in [g1_robot_model]: + # Test with wrong configuration size + q_wrong = np.zeros(robot_model.num_dofs + 1) + with pytest.raises(ValueError, match="Expected q of length"): + robot_model.compute_gravity_compensation_torques(q_wrong) + + # Test with invalid joint group + q_valid = np.zeros(robot_model.num_dofs) + if robot_model.is_floating_base_model: + q_valid[:7] = [0, 0, 1.0, 0, 0, 0, 1] + + with pytest.raises(RuntimeError, match="Error computing gravity compensation"): + robot_model.compute_gravity_compensation_torques(q_valid, "non_existent_group") + + # Test with mixed valid/invalid groups + if robot_model.supplemental_info is not None: + available_groups = list(robot_model.supplemental_info.joint_groups.keys()) + if available_groups: + valid_group = available_groups[0] + with pytest.raises(RuntimeError, match="Error computing gravity compensation"): + robot_model.compute_gravity_compensation_torques( + q_valid, [valid_group, "non_existent_group"] + ) + + +def test_robot_model_gravity_compensation_auto_clip(g1_robot_model): + """ + Test auto-clipping functionality in gravity compensation. + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + # Create configuration with values outside joint limits + q = np.zeros(robot_model.num_dofs) + root_nq = 7 if robot_model.is_floating_base_model else 0 + + if robot_model.is_floating_base_model: + q[:7] = [0, 0, 1.0, 0, 0, 0, 1] # Valid floating base + + # Set extreme joint values (outside limits) + if robot_model.num_dofs > root_nq: + q[root_nq:] = 100.0 # Very large values + + # Should work with auto_clip=True (default) + try: + gravity_clipped = robot_model.compute_gravity_compensation_torques(q, auto_clip=True) + assert gravity_clipped.shape == (robot_model.num_dofs,) + except Exception as e: + pytest.skip(f"Auto-clip test skipped due to: {e}") + + # Test with auto_clip=False - might work or might not depending on limits + try: + gravity_no_clip = robot_model.compute_gravity_compensation_torques(q, auto_clip=False) + assert gravity_no_clip.shape == (robot_model.num_dofs,) + except Exception: + # This is expected if the configuration is invalid + pass + + +def test_robot_model_gravity_compensation_arms_specific(g1_robot_model): + """ + Test gravity compensation specifically for arm joints (if available). + """ + for robot_model in [g1_robot_model]: + # Skip if no supplemental info + if robot_model.supplemental_info is None: + pytest.skip("No supplemental info available for testing gravity compensation") + + available_groups = list(robot_model.supplemental_info.joint_groups.keys()) + + # Test arms specifically if available + if "arms" in available_groups: + q = np.zeros(robot_model.num_dofs) + if robot_model.is_floating_base_model: + q[:7] = [0, 0, 1.0, 0, 0, 0, 1] + + # Test arms gravity compensation + gravity_arms = robot_model.compute_gravity_compensation_torques(q, "arms") + assert gravity_arms.shape == (robot_model.num_dofs,) + + # Test left and right arms separately if available + if "left_arm" in available_groups and "right_arm" in available_groups: + gravity_left = robot_model.compute_gravity_compensation_torques(q, "left_arm") + gravity_right = robot_model.compute_gravity_compensation_torques(q, "right_arm") + + # Both arms should have non-zero compensation (for typical configurations) + if np.any(np.abs(gravity_arms) > 1e-6): + # If arms have compensation, at least one of left/right should too + assert np.any(np.abs(gravity_left) > 1e-6) or np.any( + np.abs(gravity_right) > 1e-6 + ) + else: + pytest.skip("No arm joint groups available for testing") diff --git a/tests/control/teleop/__init__.py b/tests/control/teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/teleop/test_teleop_retargeting_ik.py b/tests/control/teleop/test_teleop_retargeting_ik.py new file mode 100644 index 0000000..79bb527 --- /dev/null +++ b/tests/control/teleop/test_teleop_retargeting_ik.py @@ -0,0 +1,196 @@ +import time + +import numpy as np +import pytest + +from gr00t_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model +from gr00t_wbc.control.robot_model.robot_model import RobotModel +from gr00t_wbc.control.teleop.solver.hand.instantiation.g1_hand_ik_instantiation import ( + instantiate_g1_hand_ik_solver, +) +from gr00t_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK + + +@pytest.fixture(params=["lower_body", "lower_and_upper_body"]) +def retargeting_ik(request): + waist_location = request.param + robot_model = instantiate_g1_robot_model(waist_location=waist_location) + left_hand_ik_solver, right_hand_ik_solver = instantiate_g1_hand_ik_solver() + return TeleopRetargetingIK( + robot_model=robot_model, + left_hand_ik_solver=left_hand_ik_solver, + right_hand_ik_solver=right_hand_ik_solver, + enable_visualization=False, # Change to true to visualize movements + body_active_joint_groups=["upper_body"], + ) + + +def generate_target_wrist_poses(mode: str, side: str, full_robot: RobotModel) -> dict: + """ + Args: + mode: One of "rotation" or "translation" + side: One of "left" or "right" - specifies which side to animate + Returns: + Dictionary mapping link names to target poses for both wrists + """ + + assert mode in ["rotation", "translation", "both"] + assert side in ["left", "right", "both"] + + # Set up initial state + full_robot.cache_forward_kinematics(full_robot.q_zero) + + # Get both wrist link names + left_wrist_link = full_robot.supplemental_info.hand_frame_names["left"] + right_wrist_link = full_robot.supplemental_info.hand_frame_names["right"] + + # Initialize default poses for both sides + left_default_pose = full_robot.frame_placement(left_wrist_link).np + right_default_pose = full_robot.frame_placement(right_wrist_link).np + + left_initial_pose_matrix = full_robot.frame_placement(left_wrist_link).np + right_initial_pose_matrix = full_robot.frame_placement(right_wrist_link).np + + # Constants + translation_cycle_duration = 4.0 + rotation_cycle_duration = 4.0 + total_duration = 12.0 + translation_amplitude = 0.1 + rotation_amplitude = np.deg2rad(60) # 30 degrees + + body_data_list = [] + for t in np.linspace(0, total_duration, 100): + rotation_matrix = np.eye(3) + current_left_translation_vector = left_initial_pose_matrix[:3, 3].copy() + current_right_translation_vector = right_initial_pose_matrix[:3, 3].copy() + + if mode == "rotation" or mode == "both": + # For rotation-only mode, start rotating immediately + rotation_axis_index = int(t // rotation_cycle_duration) % 3 + time_within_cycle = t % rotation_cycle_duration + angle = rotation_amplitude * np.sin( + (2 * np.pi / rotation_cycle_duration) * time_within_cycle + ) + + if rotation_axis_index == 0: # Roll + rotation_matrix = np.array( + [ + [1, 0, 0], + [0, np.cos(angle), -np.sin(angle)], + [0, np.sin(angle), np.cos(angle)], + ] + ) + elif rotation_axis_index == 2: # Pitch + rotation_matrix = np.array( + [ + [np.cos(angle), 0, np.sin(angle)], + [0, 1, 0], + [-np.sin(angle), 0, np.cos(angle)], + ] + ) + else: # Yaw + rotation_matrix = np.array( + [ + [np.cos(angle), -np.sin(angle), 0], + [np.sin(angle), np.cos(angle), 0], + [0, 0, 1], + ] + ) + + if mode == "translation" or mode == "both": + translation_axis_index = int(t // translation_cycle_duration) % 3 + time_within_cycle = t % translation_cycle_duration + offset = translation_amplitude * np.sin( + (2 * np.pi / translation_cycle_duration) * time_within_cycle + ) + current_left_translation_vector[translation_axis_index] += offset + current_right_translation_vector[translation_axis_index] += offset + + # Construct the 4x4 pose matrix for the animated side + left_animated_pose = np.eye(4) + left_animated_pose[:3, :3] = rotation_matrix + left_animated_pose[:3, 3] = current_left_translation_vector + + right_animated_pose = np.eye(4) + right_animated_pose[:3, :3] = rotation_matrix + right_animated_pose[:3, 3] = current_right_translation_vector + + # Create body_data dictionary with both wrists + body_data = {} + if side == "left": + body_data[left_wrist_link] = left_animated_pose + body_data[right_wrist_link] = right_default_pose + elif side == "right": + body_data[left_wrist_link] = left_default_pose + body_data[right_wrist_link] = right_animated_pose + elif side == "both": + body_data[left_wrist_link] = left_animated_pose + body_data[right_wrist_link] = right_animated_pose + + body_data_list.append(body_data) + + return body_data_list + + +@pytest.mark.parametrize("mode", ["translation", "rotation"]) +@pytest.mark.parametrize("side", ["both", "left", "right"]) +def test_ik_matches_fk(retargeting_ik, mode, side): + full_robot = retargeting_ik.full_robot + + # Generate target wrist poses + body_data_list = generate_target_wrist_poses(mode, side, full_robot) + + max_pos_error = 0 + max_rot_error = 0 + + for body_data in body_data_list: + + time_start = time.time() + + # Run IK to get joint angles + q = retargeting_ik.compute_joint_positions( + body_data, + left_hand_data=None, # Hand IK not tested + right_hand_data=None, # Hand IK not tested + ) + + time_end = time.time() + ik_time = time_end - time_start + print(f"IK time: {ik_time} s") + # Test commented out because of inconsistency in CI/CD computation time + # assert ik_time < 0.05, f"IK time too high for 20Hz loop: {ik_time} s" + + # Run FK to compute where the wrists actually ended up + full_robot.cache_forward_kinematics(q, auto_clip=False) + left_wrist_link = full_robot.supplemental_info.hand_frame_names["left"] + right_wrist_link = full_robot.supplemental_info.hand_frame_names["right"] + T_fk_left = full_robot.frame_placement(left_wrist_link).np + T_fk_right = full_robot.frame_placement(right_wrist_link).np + T_target_left = body_data[left_wrist_link] + T_target_right = body_data[right_wrist_link] + + # Check that FK translation matches target translation + pos_fk_left = T_fk_left[:3, 3] + pos_target_left = T_target_left[:3, 3] + pos_fk_right = T_fk_right[:3, 3] + pos_target_right = T_target_right[:3, 3] + + max_pos_error = max(max_pos_error, np.linalg.norm(pos_fk_left - pos_target_left)) + max_pos_error = max(max_pos_error, np.linalg.norm(pos_fk_right - pos_target_right)) + + # Check that FK rotation matches target rotation + rot_fk_left = T_fk_left[:3, :3] + rot_target_left = T_target_left[:3, :3] + rot_diff_left = rot_fk_left @ rot_target_left.T + rot_error_left = np.arccos(np.clip((np.trace(rot_diff_left) - 1) / 2, -1, 1)) + rot_fk_right = T_fk_right[:3, :3] + rot_target_right = T_target_right[:3, :3] + rot_diff_right = rot_fk_right @ rot_target_right.T + rot_error_right = np.arccos(np.clip((np.trace(rot_diff_right) - 1) / 2, -1, 1)) + + max_rot_error = max(max_rot_error, rot_error_left) + max_rot_error = max(max_rot_error, rot_error_right) + + assert max_pos_error < 0.01 and max_rot_error < np.deg2rad( + 1 + ), f"Max position error: {max_pos_error}, Max rotation error: {np.rad2deg(max_rot_error)} deg" diff --git a/tests/control/visualization/__init__.py b/tests/control/visualization/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/control/visualization/test_meshcat_visualizer_env.py b/tests/control/visualization/test_meshcat_visualizer_env.py new file mode 100644 index 0000000..a09ed90 --- /dev/null +++ b/tests/control/visualization/test_meshcat_visualizer_env.py @@ -0,0 +1,84 @@ +import pathlib +import time + +import numpy as np +import pytest + +from gr00t_wbc.control.robot_model import RobotModel +from gr00t_wbc.control.robot_model.supplemental_info.g1.g1_supplemental_info import G1SupplementalInfo + + +@pytest.fixture +def env_fixture(): + """ + Pytest fixture that creates and yields the MeshcatVisualizerEnv. + After the test, it closes the environment to clean up. + """ + from gr00t_wbc.control.visualization.meshcat_visualizer_env import MeshcatVisualizerEnv + + root_dir = pathlib.Path(__file__).parent.parent.parent.parent + urdf_path = str(root_dir / "gr00t_wbc/control/robot_model/model_data/g1/g1_29dof_with_hand.urdf") + asset_path = str(root_dir / "gr00t_wbc/control/robot_model/model_data/g1") + robot_config = { + "asset_path": asset_path, + "urdf_path": urdf_path, + } + robot_model = RobotModel( + robot_config["urdf_path"], + robot_config["asset_path"], + supplemental_info=G1SupplementalInfo(), + ) + env = MeshcatVisualizerEnv(robot_model) + time.sleep(0.5) + yield env + env.close() + + +def test_meshcat_env_init(env_fixture): + """ + Test that the environment initializes without errors + and that reset() returns the proper data structure. + """ + env = env_fixture + initial_obs = env.reset() + assert isinstance(initial_obs, dict), "reset() should return a dictionary." + assert "q" in initial_obs, "The returned dictionary should contain key 'q'." + assert ( + len(initial_obs["q"]) == env.robot_model.num_dofs + ), "Length of 'q' should match the robot's DOF." + + +def test_meshcat_env_observation(env_fixture): + """ + Test that the observe() method returns a valid observation + conforming to the environment's observation space. + """ + env = env_fixture + observation = env.observe() + assert isinstance(observation, dict), "observe() should return a dictionary." + assert "q" in observation, "The returned dictionary should contain key 'q'." + assert ( + len(observation["q"]) == env.robot_model.num_dofs + ), "Length of 'q' should match the robot's DOF." + + +def test_meshcat_env_action(env_fixture): + """ + Test that we can queue an action and visualize it without error. + """ + env = env_fixture + # Build a dummy action within the action space + test_action = {"q": 0.2 * np.ones(env.robot_model.num_dofs)} + + # This should not raise an exception and should visualize the correct configuration + env.queue_action(test_action) + + +def test_meshcat_env_close(env_fixture): + """ + Test closing the environment. (Though the fixture calls env.close() + automatically, we can invoke it here to ensure it's safe to do so.) + """ + env = env_fixture + env.close() + # If close() triggers no exceptions, we're good. diff --git a/tests/data/test_exporter.py b/tests/data/test_exporter.py new file mode 100644 index 0000000..2439872 --- /dev/null +++ b/tests/data/test_exporter.py @@ -0,0 +1,522 @@ +import json +from pathlib import Path +import shutil +import tempfile +import time + +from lerobot.common.datasets.lerobot_dataset import LeRobotDataset +import numpy as np +import pytest + +from gr00t_wbc.data.exporter import DataCollectionInfo, Gr00tDataExporter + + +@pytest.fixture +def test_features(): + """Fixture providing test features dict.""" + return { + "observation.images.ego_view": { + "dtype": "video", + "shape": [64, 64, 3], # Small images for faster tests + "names": ["height", "width", "channel"], + }, + "observation.state": { + "dtype": "float32", + "shape": (8,), + "names": ["x1", "x2", "x3", "x4", "x5", "x6", "x7", "x8"], + }, + "action": { + "dtype": "float32", + "shape": (8,), + "names": ["a1", "a2", "a3", "a4", "a5", "a6", "a7", "a8"], + }, + } + + +@pytest.fixture +def test_modality_config(): + return { + "state": {"feature1": {"start": 0, "end": 4}, "feature2": {"start": 4, "end": 9}}, + "action": {"feature1": {"start": 0, "end": 4}, "feature2": {"start": 4, "end": 9}}, + "video": {"rs_view": {"original_key": "observation.images.ego_view"}}, + "annotation": {"human.task_description": {"original_key": "task_index"}}, + } + + +@pytest.fixture +def test_data_collection_info(): + return DataCollectionInfo( + teleoperator_username="test_user", + support_operator_username="test_user", + robot_type="test_robot", + lower_body_policy="test_policy", + wbc_model_path="test_path", + ) + + +def get_test_frame(step: int): + """Generate a test frame with data that varies by step.""" + # Create a simple, small image that will encode quickly + img = np.ones((64, 64, 3), dtype=np.uint8) * (step % 255) + # Add a pattern to make each frame unique and verifiable + img[step % 64, :, :] = 255 - (step % 255) + + return { + "observation.images.ego_view": img, + "observation.state": np.ones(8, dtype=np.float32) * step, + "action": np.ones(8, dtype=np.float32) * step, + } + + +@pytest.fixture +def temp_dir(): + """Create a temporary directory for test data that's cleaned up after tests.""" + temp_dir = tempfile.mkdtemp() + yield Path(temp_dir) / "dataset" + shutil.rmtree(temp_dir) + + +class TestInterruptAndResume: + """Test class for simulating interruption and resumption of recording.""" + + # Skip this test if ffmpeg is not installed + @pytest.mark.skipif( + shutil.which("ffmpeg") is None, reason="ffmpeg not installed, skipping test" + ) + def test_interrupted_mid_episode( + self, temp_dir, test_features, test_modality_config, test_data_collection_info + ): + """ + Test that simulates a recording session that gets interrupted and then resumes. + + This test uses the actual Gr00tDataExporter implementation with no mocks. + """ + # Constants for the test + NUM_EPISODES = 2 + FRAMES_PER_EPISODE = 5 + + # Pick a random episode and frame to interrupt at + interrupt_episode = 1 + interrupt_frame = 3 + + print(f"Will interrupt at episode {interrupt_episode}, frame {interrupt_frame}") + + # Track what we've added to verify later + completed_episodes = [] + frames_added_first_session = 0 + + # Initial recording session + try: + # Start recording with real Gr00tDataExporter + exporter1 = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + robot_type="test_robot", + vcodec="libx264", # Use a common codec that should be available + data_collection_info=test_data_collection_info, + ) + + # Record episodes until interruption + for episode in range(NUM_EPISODES): + for frame in range(FRAMES_PER_EPISODE): + # Simulate interruption + if episode == interrupt_episode and frame == interrupt_frame: + print(f"Simulating interruption at episode {episode}, frame {frame}") + raise KeyboardInterrupt("Simulated interruption") + + # Add frame + exporter1.add_frame(get_test_frame(frame)) + frames_added_first_session += 1 + + # Save episode + exporter1.save_episode() + completed_episodes.append(episode) + + except KeyboardInterrupt: + print(f"Recording interrupted at episode {interrupt_episode}, frame {interrupt_frame}") + print(f"Completed episodes: {completed_episodes}") + # Don't consolidate since we're interrupted + pass + + # Verify what was recorded before interruption + assert len(completed_episodes) == interrupt_episode + assert ( + frames_added_first_session == interrupt_episode * FRAMES_PER_EPISODE + interrupt_frame + ) + + # Let file system operations complete + time.sleep(0.5) + + # Resume recording - create a new exporter pointing to the same directory + exporter2 = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + robot_type="test_robot", + vcodec="libx264", + ) + + # The interrupted episode had frames added but wasn't saved + # In a real scenario with the current implementation, we need to restart that episode + + # Record all episodes from the beginning + frames_added_second_session = 0 + episodes_saved_second_session = 0 + + for episode in range(NUM_EPISODES): + for frame in range(FRAMES_PER_EPISODE): + exporter2.add_frame(get_test_frame(frame)) + frames_added_second_session += 1 + + # Save episode + exporter2.save_episode() + episodes_saved_second_session += 1 + + # Verify the result + assert frames_added_second_session == NUM_EPISODES * FRAMES_PER_EPISODE + assert episodes_saved_second_session == NUM_EPISODES + + # Verify actual files were created + for episode_idx in range(NUM_EPISODES): + video_path = exporter2.root / exporter2.meta.get_video_file_path( + episode_idx, "observation.images.ego_view" + ) + assert video_path.exists(), f"Video file not found: {video_path}" + + @pytest.mark.skipif( + shutil.which("ffmpeg") is None, reason="ffmpeg not installed, skipping test" + ) + def test_interrupted_after_episode_completion( + self, temp_dir, test_features, test_modality_config, test_data_collection_info + ): + """ + Test specifically for the case when interruption happens after an episode is completed. + Uses the real Gr00tDataExporter implementation. + """ + # First session - record 1 complete episode and then interrupt + exporter1 = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + vcodec="libx264", + ) + + # Record 1 complete episode + for frame in range(5): + exporter1.add_frame(get_test_frame(frame)) + exporter1.save_episode() + + # Let file system operations complete + time.sleep(0.5) + + # Verify the first episode was saved + video_path = exporter1.root / exporter1.meta.get_video_file_path( + 0, "observation.images.ego_view" + ) + assert video_path.exists(), f"First episode video file not found: {video_path}" + + # Second session - resume and record another episode + exporter2 = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + vcodec="libx264", + ) + + # Record the second episode + for frame in range(5): + exporter2.add_frame(get_test_frame(frame)) + exporter2.save_episode() + + # Verify the second episode was saved + video_path = exporter2.root / exporter2.meta.get_video_file_path( + 1, "observation.images.ego_view" + ) + assert video_path.exists(), f"Second episode video file not found: {video_path}" + + @pytest.mark.skipif( + shutil.which("ffmpeg") is None, reason="ffmpeg not installed, skipping test" + ) + def test_interrupted_no_episode_completion( + self, temp_dir, test_features, test_modality_config, test_data_collection_info + ): + """ + Test specifically for the case when interruption happens in the middle of recording an episode. + Uses the real Gr00tDataExporter implementation. + """ + # First session - add some frames and interrupt before saving + exporter1 = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + vcodec="libx264", + ) + + # Add 3 frames but don't save + for frame in range(3): + exporter1.add_frame(get_test_frame(frame)) + # Don't save episode or consolidate to simulate interruption + # The episode buffer is only in memory and will be lost on interruption + + # Let file system operations complete + time.sleep(0.5) + + # Verify no episode was saved + video_path = exporter1.root / exporter1.meta.get_video_file_path( + 0, "observation.images.ego_view" + ) + assert not video_path.exists(), f"Episode should not have been saved: {video_path}" + + # Second session - will raise an error because no meta file exist, so we can't resume + with pytest.raises(ValueError): + _ = Gr00tDataExporter.create( + save_root=temp_dir, + fps=30, + features=test_features, + modality_config=test_modality_config, + task="test_task", + vcodec="libx264", + ) + + +@pytest.mark.skipif(shutil.which("ffmpeg") is None, reason="ffmpeg not installed, skipping test") +def test_full_workflow(temp_dir, test_features, test_modality_config, test_data_collection_info): + """ + Test that simulates the complete workflow from the record_session.py example. + """ + NUM_EPISODES = 2 + FRAMES_PER_EPISODE = 3 + + # Create the exporter + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + robot_type="dummy", + ) + + # Create a small dataset + for episode_index in range(NUM_EPISODES): + for frame_index in range(FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + exporter.save_episode() + + # check modality config + modality_config_path = exporter.root / "meta" / "modality.json" + assert modality_config_path.exists(), f"{modality_config_path} does not exists." + with open(modality_config_path, "rb") as f: + actual_modality_config = json.load(f) + + assert ( + actual_modality_config == test_modality_config + ), f"Modality configs don't match.\nActual: {actual_modality_config}\nExpected: {test_modality_config}" + + # Verify results + for episode_idx in range(NUM_EPISODES): + video_path = exporter.root / exporter.meta.get_video_file_path( + episode_idx, "observation.images.ego_view" + ) + assert video_path.exists(), f"Video file not found: {video_path}" + + # Check that the expected number of episodes exists + episode_count = 0 + for path in exporter.root.glob("**/*.mp4"): + episode_count += 1 + assert episode_count == NUM_EPISODES, f"Expected {NUM_EPISODES} episodes, found {episode_count}" + + # Check the values of the dataset + dataset = LeRobotDataset( + repo_id="dataset", + root=temp_dir, + ) + for episode_idx in range(NUM_EPISODES): + for frame_idx in range(FRAMES_PER_EPISODE): + expected_frame = get_test_frame(frame_idx) + actual_frame = dataset[episode_idx * FRAMES_PER_EPISODE + frame_idx] + print(actual_frame["observation.images.ego_view"]) + actual_image_frame = actual_frame["observation.images.ego_view"].permute(1, 2, 0) * 255 + assert np.allclose( + actual_image_frame.numpy(), expected_frame["observation.images.ego_view"], atol=10 + ) # Allow some tolerance for video compression + assert np.allclose( + actual_frame["observation.state"], expected_frame["observation.state"] + ) + assert np.allclose(actual_frame["action"], expected_frame["action"]) + + # validate data_collection_info + assert dataset.meta.info["data_collection_info"] == test_data_collection_info.to_dict() + + +@pytest.mark.skipif(shutil.which("ffmpeg") is None, reason="ffmpeg not installed, skipping test") +def test_overwrite_existing_dataset_false( + temp_dir, test_features, test_modality_config, test_data_collection_info +): + """ + Test that appends to the existing dataset when overwrite_existing is set to false. + """ + # first dataset + FIRST_NUM_EPISODES = 2 + FIRST_FRAMES_PER_EPISODE = 3 + + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + robot_type="dummy", + ) + # !! `overwrite_existing` should always be set to false by default + # So we're deliberately not setting the overwrite_existing argument here. + # This test ensures that + # i. the default behavior is overwrite_existing=False + # ii. the dataset appends to the existing dataset (instead of overwriting) + + # Create a first dataset + for episode_index in range(FIRST_NUM_EPISODES): + for frame_index in range(FIRST_FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + exporter.save_episode() + + # second dataset + del exporter + SECOND_NUM_EPISODES = 3 + SECOND_FRAMES_PER_EPISODE = 2 + + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + robot_type="dummy", + ) + for episode_index in range(SECOND_NUM_EPISODES): + for frame_index in range(SECOND_FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + exporter.save_episode() + + # verify that there are + EXPECTED_NUM_EPISODES = FIRST_NUM_EPISODES + SECOND_NUM_EPISODES + assert len(list(exporter.root.glob("**/*.mp4"))) == EXPECTED_NUM_EPISODES + assert len(list(exporter.root.glob("**/*.parquet"))) == EXPECTED_NUM_EPISODES + + +def test_overwrite_existing_dataset_true( + temp_dir, test_features, test_modality_config, test_data_collection_info +): + """ + Test that overwrites to an existing dataset when overwrite_existing=True. + """ + # first dataset + FIRST_NUM_EPISODES = 2 + FIRST_FRAMES_PER_EPISODE = 3 + + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + robot_type="dummy", + ) + + # Create a first dataset + for episode_index in range(FIRST_NUM_EPISODES): + for frame_index in range(FIRST_FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + exporter.save_episode() + + # verify that the dataset is written to the disk + assert len(list(exporter.root.glob("**/*.mp4"))) == FIRST_NUM_EPISODES + assert len(list(exporter.root.glob("**/*.parquet"))) == FIRST_NUM_EPISODES + + # second dataset + SECOND_NUM_EPISODES = 3 + SECOND_FRAMES_PER_EPISODE = 2 + + # re-initialize the exporter + del exporter + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + robot_type="dummy", + overwrite_existing=True, + ) + for episode_index in range(SECOND_NUM_EPISODES): + for frame_index in range(SECOND_FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + exporter.save_episode() + + # verify that the dataset is overwritten + assert len(list(exporter.root.glob("**/*.mp4"))) == SECOND_NUM_EPISODES + assert len(list(exporter.root.glob("**/*.parquet"))) == SECOND_NUM_EPISODES + + +def test_save_episode_as_discarded_and_skip( + temp_dir, test_features, test_modality_config, test_data_collection_info +): + """ + Test that verifies the functionality of saving an episode as discarded and skipping an episode. + """ + FIRST_NUM_EPISODES = 10 + FIRST_FRAMES_PER_EPISODE = 3 + + exporter = Gr00tDataExporter.create( + save_root=temp_dir, + fps=20, + features=test_features, + modality_config=test_modality_config, + task="test_task", + data_collection_info=test_data_collection_info, + robot_type="dummy", + ) + + # Create a first dataset + saved_episodes = 0 + discarded_episode_indices = [] + for episode_index in range(FIRST_NUM_EPISODES): + for frame_index in range(FIRST_FRAMES_PER_EPISODE): + exporter.add_frame(get_test_frame(frame_index)) + if episode_index % 3 == 0: + exporter.save_episode_as_discarded() + discarded_episode_indices.append(saved_episodes) + saved_episodes += 1 + elif episode_index % 3 == 1: + exporter.skip_and_start_new_episode() + else: + exporter.save_episode() + saved_episodes += 1 + + # verify that the dataset is written to the disk + assert len(list(exporter.root.glob("**/*.mp4"))) == saved_episodes + assert len(list(exporter.root.glob("**/*.parquet"))) == saved_episodes + + dataset = LeRobotDataset( + repo_id="dataset", + root=temp_dir, + ) + + assert dataset.meta.info["discarded_episode_indices"] == discarded_episode_indices diff --git a/tests/replay_data/all_joints_raw_data_replay.pkl b/tests/replay_data/all_joints_raw_data_replay.pkl new file mode 100644 index 0000000..bb75024 --- /dev/null +++ b/tests/replay_data/all_joints_raw_data_replay.pkl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3fa5291a172b5c2620766180785be1e100903e8e0d62a8b17063e87407cd471a +size 9017749 diff --git a/tests/replay_data/calibration_data.pkl b/tests/replay_data/calibration_data.pkl new file mode 100644 index 0000000..62d8cc7 --- /dev/null +++ b/tests/replay_data/calibration_data.pkl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6576b146ef735acf4d9c949c8919122bbe33d2df2f0a51762727cd639a74459e +size 7967 diff --git a/tests/replay_data/g1_pnpbottle.parquet b/tests/replay_data/g1_pnpbottle.parquet new file mode 100644 index 0000000..3c66b39 --- /dev/null +++ b/tests/replay_data/g1_pnpbottle.parquet @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0da369134e22bef8bb0c56dce31bd1e2e7af29123c5478e793a389246526b6ad +size 604432 diff --git a/tests/replay_data/interpolation_data.pkl b/tests/replay_data/interpolation_data.pkl new file mode 100644 index 0000000..0916571 --- /dev/null +++ b/tests/replay_data/interpolation_data.pkl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1bf6fcfe6ac0d07fe2a4e11a3f170adf70f60232bbc1b30f0fc981ce2e5812c5 +size 22867 diff --git a/tests/replay_data/sin_wave_arm_replay.pkl b/tests/replay_data/sin_wave_arm_replay.pkl new file mode 100644 index 0000000..3ea7b6f --- /dev/null +++ b/tests/replay_data/sin_wave_arm_replay.pkl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e94e161345753ab685d3cd5d912a9ab8d0091d8ed6212e5e15617a39c5ca6070 +size 3092488 diff --git a/tests/replay_data/target_joints_data.pkl b/tests/replay_data/target_joints_data.pkl new file mode 100644 index 0000000..6f274d2 --- /dev/null +++ b/tests/replay_data/target_joints_data.pkl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:211b90245fbc10d5d38ebd40110316ff89630875eace991858fa358e2f3dc925 +size 189707 diff --git a/tests/sim/test_sim_data_collection.py b/tests/sim/test_sim_data_collection.py new file mode 100644 index 0000000..2665b22 --- /dev/null +++ b/tests/sim/test_sim_data_collection.py @@ -0,0 +1,64 @@ +from gr00t_wbc.control.main.teleop.configs.configs import SyncSimDataCollectionConfig +from gr00t_wbc.control.main.teleop.run_sync_sim_data_collection import ( + main as data_collection_main, +) + + +def test_sim_data_collection_unit(robot_name="G1", task_name="GroundOnly"): + """ + Fast CI unit test for simulation data collection (50 steps, no tracking checks). + + This test validates that: + 1. Data collection completes successfully + 2. Upper body joints are moving (velocity check) + + Note: This test runs for only 50 steps and does not perform end effector tracking validation + for faster CI execution. + """ + config = SyncSimDataCollectionConfig() + config.robot = robot_name + config.task_name = task_name + config.enable_visualization = False + config.enable_real_device = False + config.enable_onscreen = False + config.save_img_obs = True + config.ci_test = True + config.ci_test_mode = "unit" + config.replay_data_path = "tests/replay_data/all_joints_raw_data_replay.pkl" + config.remove_existing_dir = True + config.enable_gravity_compensation = True + res = data_collection_main(config) + assert res, "Data collection did not pass for all datasets" + + +def test_sim_data_collection_pre_merge(robot_name="G1", task_name="GroundOnly"): + """ + Pre-merge test for simulation data collection with end effector tracking validation (500 steps). + + This test validates that: + 1. Data collection completes successfully + 2. Upper body joints are moving (velocity check) + 3. End effector tracking error is within thresholds: + - G1 robots: + Max position error < 7cm (0.07m), Max rotation error < 17°, + Average position error < 5cm (0.05m), Average rotation error < 12° + """ + config = SyncSimDataCollectionConfig() + config.robot = robot_name + config.task_name = task_name + config.enable_visualization = False + config.enable_real_device = False + config.enable_onscreen = False + config.save_img_obs = True + config.ci_test = True + config.ci_test_mode = "pre_merge" + config.replay_data_path = "tests/replay_data/all_joints_raw_data_replay.pkl" + config.remove_existing_dir = True + config.enable_gravity_compensation = True + res = data_collection_main(config) + assert res, "Data collection did not pass for all datasets" + + +if __name__ == "__main__": + # Run unit tests for fast CI + test_sim_data_collection_unit("G1", "GroundOnly")