diff --git a/.gitignore b/.gitignore index b3a54b003..02a1c9a13 100644 --- a/.gitignore +++ b/.gitignore @@ -35,7 +35,6 @@ *.app # IDE stuff -*.vscode *.*~ TAGS @@ -90,3 +89,14 @@ core.* # Development and cache folders /__* + +# IDE +# Ignore .idea directories except for .idea/tools at the root +**/.idea/ +!/.idea/ +/.idea/* +!/.idea/tools/ +**/.vscode/ +# Don't ignore the top-level .vscode directory as it is +# used to configure VS Code settings +!.vscode \ No newline at end of file diff --git a/.idea/tools/runConfigurations/Replay Demos.xml b/.idea/tools/runConfigurations/Replay Demos.xml new file mode 100644 index 000000000..d9bda4231 --- /dev/null +++ b/.idea/tools/runConfigurations/Replay Demos.xml @@ -0,0 +1,27 @@ + + + + + \ No newline at end of file diff --git a/.idea/tools/setup_pycharm.py b/.idea/tools/setup_pycharm.py new file mode 100644 index 000000000..7abff7f9a --- /dev/null +++ b/.idea/tools/setup_pycharm.py @@ -0,0 +1,343 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script sets up the PyCharm settings for the Isaac Lab project. + +This script creates/updates the PyCharm configuration files to include the necessary Python paths +from Isaac Sim and Isaac Lab extensions. + +This is necessary because Isaac Sim 2022.2.1 onwards requires explicit path configuration +for proper code completion and navigation in PyCharm. +""" + +import re +import sys +import os +import pathlib +import xml.etree.ElementTree as ET +from xml.dom import minidom + + +ISAACLAB_ARENA_DIR = pathlib.Path(__file__).parents[2] +ISAACLAB_DIR = ISAACLAB_ARENA_DIR / "submodules" / "IsaacLab" +"""Path to the Isaac Lab directory.""" + +try: + import isaacsim # noqa: F401 + + isaacsim_dir = os.environ.get("ISAAC_PATH", "") +except ModuleNotFoundError or ImportError: + isaacsim_dir = os.path.join(ISAACLAB_DIR, "_isaac_sim") +except EOFError: + print("Unable to trigger EULA acceptance. This is likely due to the script being run in a non-interactive shell.") + print("Please run the script in an interactive shell to accept the EULA.") + print("Skipping the setup of the PyCharm settings...") + sys.exit(0) + +# check if the isaac-sim directory exists +if not os.path.exists(isaacsim_dir): + raise FileNotFoundError( + f"Could not find the isaac-sim directory: {isaacsim_dir}. There are two possible reasons for this:" + f"\n\t1. The Isaac Sim directory does not exist as a symlink at: {os.path.join(ISAACLAB_DIR, '_isaac_sim')}" + "\n\t2. The script could not import the 'isaacsim' package. This could be due to the 'isaacsim' package not " + "being installed in the Python environment.\n" + "\nPlease make sure that the Isaac Sim directory exists or that the 'isaacsim' package is installed." + ) + +ISAACSIM_DIR = isaacsim_dir +"""Path to the isaac-sim directory.""" + + +def prettify_xml(elem): + """Return a pretty-printed XML string for the Element.""" + rough_string = ET.tostring(elem, encoding='unicode') + reparsed = minidom.parseString(rough_string) + return reparsed.toprettyxml(indent=" ") + + +def get_python_paths_from_isaacsim(): + """Extract python paths from Isaac Sim's VSCode settings. + + Returns: + List of absolute paths to add to Python path. + """ + # isaac-sim settings + isaacsim_vscode_filename = os.path.join(ISAACSIM_DIR, ".vscode", "settings.json") + + # we use the isaac-sim settings file to get the python.analysis.extraPaths for kit extensions + # if this file does not exist, we will not add any extra paths + if os.path.exists(isaacsim_vscode_filename): + # read the path names from the isaac-sim settings file + with open(isaacsim_vscode_filename) as f: + vscode_settings = f.read() + # extract the path names + # search for the python.analysis.extraPaths section and extract the contents + settings = re.search( + r"\"python.analysis.extraPaths\": \[.*?\]", vscode_settings, flags=re.MULTILINE | re.DOTALL + ) + if settings: + settings = settings.group(0) + settings = settings.split('"python.analysis.extraPaths": [')[-1] + settings = settings.split("]")[0] + + # read the path names from the isaac-sim settings file + path_names = settings.split(",") + path_names = [path_name.strip().strip('"') for path_name in path_names] + path_names = [path_name for path_name in path_names if len(path_name) > 0] + + # convert to absolute paths + path_names = [os.path.abspath(os.path.join(ISAACSIM_DIR, path_name)) for path_name in path_names] + else: + path_names = [] + else: + path_names = [] + print( + f"[WARN] Could not find Isaac Sim VSCode settings: {isaacsim_vscode_filename}." + "\n\tThis will result in missing Python paths in the PyCharm configuration," + "\n\twhich limits the functionality of code completion and navigation." + "\n\tHowever, it does not affect the functionality of the Isaac Lab project." + "\n\tWe are working on a fix for this issue with the Isaac Sim team." + ) + + # add the path names that are in the Isaac Lab extensions directory + isaaclab_extensions = os.listdir(os.path.join(ISAACLAB_DIR, "source")) + path_names.extend([os.path.abspath(os.path.join(ISAACLAB_DIR, "source", ext)) for ext in isaaclab_extensions]) + + return path_names + + +def create_or_update_misc_xml(): + """Create or update .idea/misc.xml with the Python interpreter path.""" + misc_xml_filename = os.path.join(ISAACLAB_ARENA_DIR, ".idea", "misc.xml") + misc_xml_template_filename = os.path.join(ISAACLAB_ARENA_DIR, ".idea", "tools", "misc.template.xml") + + # Ensure .idea directory exists + os.makedirs(os.path.dirname(misc_xml_filename), exist_ok=True) + + # Read executable name + python_exe = sys.executable + + # Check if template exists, if so use it as base + if os.path.exists(misc_xml_template_filename): + tree = ET.parse(misc_xml_template_filename) + root = tree.getroot() + elif os.path.exists(misc_xml_filename): + tree = ET.parse(misc_xml_filename) + root = tree.getroot() + else: + root = ET.Element("project", version="4") + + # Find or create ProjectRootManager component + project_root_manager = root.find(".//component[@name='ProjectRootManager']") + if project_root_manager is None: + project_root_manager = ET.SubElement(root, "component", name="ProjectRootManager") + + # We make an exception for replacing the default interpreter if the + # path (/kit/python/bin/python3) indicates that we are using a local/container + # installation of IsaacSim. + if "kit/python/bin/python3" not in python_exe: + project_root_manager.set("version", "2") + project_root_manager.set("project-jdk-name", "Python") + project_root_manager.set("project-jdk-type", "Python SDK") + + # Add header comment + header_message = ( + "\n" + ) + + # Write the file + xml_string = prettify_xml(root) + # Insert header after XML declaration + xml_lines = xml_string.split('\n') + if xml_lines[0].startswith('\n" + ) + + # Insert header after XML declaration + lines = content.split('\n') + if lines[0].startswith('\n" + ) + + lines = content.split('\n') + if lines[0].startswith(' /dev/null 2>&1 + echo "PYTHONPATH=$PYTHONPATH" + echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" + """ + + result = subprocess.run( + ["bash", "-c", cmd], + capture_output=True, + text=True, + check=False + ) + + if result.returncode == 0: + for line in result.stdout.strip().split('\n'): + if '=' in line: + key, value = line.split('=', 1) + if key in ["PYTHONPATH", "LD_LIBRARY_PATH"]: + # Convert absolute paths to PyCharm variables where possible + value = value.replace(str(ISAAC_SIM_PATH), "$PROJECT_DIR$/submodules/IsaacLab/_isaac_sim") + value = value.replace(str(ISAACLAB_DIR), "$PROJECT_DIR$/submodules/IsaacLab") + env_vars[key] = value + else: + print(f"[WARNING] Failed to extract env vars from setup scripts") + except Exception as e: + print(f"[WARNING] Error extracting env vars: {e}") + + return env_vars + + +def update_run_configuration(config_file, env_vars): + """Update a run configuration XML file with environment variables.""" + try: + tree = ET.parse(config_file) + root = tree.getroot() + + # Find the configuration element + config = root.find(".//configuration") + if config is None: + print(f"[SKIP] No configuration element found in {config_file.name}") + return False + + # Check if it's a Python configuration + if config.get("type") != "PythonConfigurationType": + print(f"[SKIP] Not a Python configuration: {config_file.name}") + return False + + # Find or create envs element + envs = config.find("envs") + if envs is None: + # Find the right place to insert (after PARENT_ENVS option) + parent_envs_option = None + for i, child in enumerate(config): + if child.tag == "option" and child.get("name") == "PARENT_ENVS": + parent_envs_option = i + break + + if parent_envs_option is not None: + envs = ET.Element("envs") + config.insert(parent_envs_option + 1, envs) + else: + # If no PARENT_ENVS found, add at the beginning + envs = ET.Element("envs") + config.insert(0, envs) + + # Remove existing Isaac Lab related env vars to avoid duplicates + isaac_env_keys = {"ISAACLAB_PATH", "RESOURCE_NAME", "ISAAC_PATH", "CARB_APP_PATH", + "EXP_PATH", "PYTHONPATH", "LD_LIBRARY_PATH"} + for env_elem in list(envs.findall("env")): + if env_elem.get("name") in isaac_env_keys: + envs.remove(env_elem) + + # Keep PYTHONUNBUFFERED if it exists + pythonunbuffered_exists = any( + env.get("name") == "PYTHONUNBUFFERED" for env in envs.findall("env") + ) + if not pythonunbuffered_exists: + ET.SubElement(envs, "env", name="PYTHONUNBUFFERED", value="1") + + # Add Isaac Lab environment variables + for key, value in env_vars.items(): + ET.SubElement(envs, "env", name=key, value=value) + + # Remove "Setup IsaacLab Environment" before-launch task (no longer needed) + method_elem = config.find("method") + if method_elem is not None: + for option in list(method_elem.findall("option")): + if (option.get("name") == "RunConfigurationTask" and + option.get("run_configuration_name") == "Setup IsaacLab Environment"): + method_elem.remove(option) + print(f"[INFO] Removed obsolete before-launch task from {config_file.name}") + + # Write the file + xml_string = prettify_xml(root) + + # Check if it has a header comment + has_header = False + if config_file.exists(): + with open(config_file, 'r') as f: + first_lines = f.read(200) + if "automatically generated" in first_lines: + has_header = True + + # Preserve header if it exists + if has_header: + with open(config_file, 'r') as f: + lines = f.readlines() + header_lines = [] + for line in lines: + if line.strip().startswith('' in line: + header_lines.append(line) + break + elif header_lines: + header_lines.append(line) + + if header_lines: + # Insert header after XML declaration + xml_lines = xml_string.split('\n') + if xml_lines[0].startswith(' Reload All from Disk) for changes to take effect.") + print() + + return 0 + + +if __name__ == "__main__": + exit(main()) + diff --git a/.vscode/launch.json b/.vscode/launch.json index 51cbdd3fa..98ab20233 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,6 +1,10 @@ { "version": "0.2.0", "configurations": [ + + + + { "name": "Attach to debugpy session", @@ -16,6 +20,57 @@ "remoteRoot": "/workspaces/isaac_arena" } ] + }, + { + "name": "Python: Annotate Dataset", + "type": "python", + "request": "launch", + "args" : [ + "--mimic", + "--input_file", "${workspaceFolder}/isaaclab_arena/datasets/dataset_v1.hdf5", + "--output_file", "${workspaceFolder}/isaaclab_arena/datasets/dataset_v1_annotated.hdf5", + "fii_pick_place"], + "program": "${workspaceFolder}/isaaclab_arena/scripts/annotate_demos.py", + "console": "integratedTerminal" + }, + { + "name": "Python: Generate Dataset", + "type": "python", + "request": "launch", + "args" : [ + "--mimic", + "--input_file", "${workspaceFolder}/isaaclab_arena/datasets/dataset_v1_annotated.hdf5", + "--output_file", "${workspaceFolder}/isaaclab_arena/datasets/dataset_v1_generated.hdf5", + "--generation_num_trials", "100", + "--device", "cpu", + "fii_pick_place"], + "program": "${workspaceFolder}/isaaclab_arena/scripts/generate_dataset.py", + "console": "integratedTerminal" + }, + { + "name": "Python: Replay Dataset", + "type": "python", + "request": "launch", + "args" : [ + "--dataset_file", "${workspaceFolder}/isaaclab_arena/datasets/dataset_v1_annotated.hdf5", + "--enable_pinocchio", + "fii_pick_place"], + "program": "${workspaceFolder}/isaaclab_arena/scripts/replay_demos.py", + "console": "integratedTerminal" + }, + { + "name": "Python: Generate Dataset G1", + "type": "python", + "request": "launch", + "args" : ["--enable_cameras", + "--mimic", + "--input_file", "${workspaceFolder}/isaaclab_arena/datasets/arena_g1_loco_manipulation_dataset_annotated.hdf5", + "--output_file", "${workspaceFolder}/isaaclab_arena/datasets/arena_g1_loco_manipulation_dataset_generated.hdf5", + "--generation_num_trials", "100", + "--device", "cpu", + "galileo_g1_locomanip_pick_and_place", "--object", "brown_box", "--embodiment", "g1_wbc_pink"], + "program": "${workspaceFolder}/isaaclab_arena/scripts/generate_dataset.py", + "console": "integratedTerminal" } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..89959b66e --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,19 @@ +{ + "python.defaultInterpreterPath": "/isaac-sim/python.sh", + "python.analysis.extraPaths": [ + "${workspaceFolder}", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_assets", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_rl", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_mimic", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_tasks", + ], + "cursorpyright.analysis.extraPaths": [ + "${workspaceFolder}", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_assets", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_rl", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_mimic", + "${workspaceFolder}/submodules/IsaacLab/source/isaaclab_tasks", + ] +} \ No newline at end of file diff --git a/docker/run_docker.sh b/docker/run_docker.sh index 87da931e8..9701a4b90 100755 --- a/docker/run_docker.sh +++ b/docker/run_docker.sh @@ -140,12 +140,15 @@ else "-v" "./isaaclab_arena_g1:${WORKDIR}/isaaclab_arena_g1" "-v" "./isaaclab_arena_gr00t:${WORKDIR}/isaaclab_arena_gr00t" "-v" "./submodules/IsaacLab:${WORKDIR}/submodules/IsaacLab" + "-v" "/home/darrelldai/Projects/Python/IsaacLab-Arena/.vscode:/workspaces/isaaclab_arena/.vscode" $(add_volume_if_it_exists $DATASETS_HOST_MOUNT_DIRECTORY /datasets) $(add_volume_if_it_exists $MODELS_HOST_MOUNT_DIRECTORY /models) $(add_volume_if_it_exists $EVAL_HOST_MOUNT_DIRECTORY /eval) "-v" "$HOME/.bash_history:/home/$(id -un)/.bash_history" "-v" "$HOME/.config/osmo:/home/$(id -un)/.config/osmo" "-v" "$HOME/.cache:/home/$(id -un)/.cache" + "-v" "$HOME/.cursor:/home/$(id -un)/.cursor" + "-v" "$HOME/.cursor-server-root:/root/.cursor-server" "-v" "/tmp:/tmp" "-v" "/tmp/.X11-unix:/tmp/.X11-unix:rw" "-v" "/var/run/docker.sock:/var/run/docker.sock" diff --git a/isaaclab_arena.sh b/isaaclab_arena.sh new file mode 100755 index 000000000..f38bae96a --- /dev/null +++ b/isaaclab_arena.sh @@ -0,0 +1,294 @@ +# Path to your Isaac Sim installation (e.g., ~/.local/share/ov/pkg/isaac-sim-5.0.0) +export ISAAC_SIM_PATH="$HOME/isaacsim" + +# Path to this repository +export ISAACLAB_ARENA_PATH="$(pwd)" + +# Path to Isaac Lab (assumed to be in submodules) +export ISAACLAB_PATH="$ISAACLAB_ARENA_PATH/submodules/IsaacLab" + +# print the usage description +print_help () { + echo -e "\nusage: $(basename "$0") [-h] [-i] [-f] [-p] [-s] [-t] [-o] [-v] [-d] [-n] [-c] [-u] -- Utility to manage Isaac Lab." + echo -e "\noptional arguments:" + echo -e "\t-h, --help Display the help content." + echo -e "\t-i, --install [LIB] Install the extensions inside Isaac Arena Lab and learning frameworks as extra dependencies. Default is 'all'." + echo -e "\t-c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'." + echo -e "\t-u, --uv [NAME] Create the uv environment for Isaac Lab. Default name is 'env_isaaclab'." + echo -e "\n" >&2 +} + + +# extract isaac sim path +extract_isaacsim_path() { + # Use the sym-link path to Isaac Sim directory + local isaac_path=${ISAACLAB_PATH}/_isaac_sim + # If above path is not available, try to find the path using python + if [ ! -d "${isaac_path}" ]; then + # Use the python executable to get the path + local python_exe=$(extract_python_exe) + # Retrieve the path importing isaac sim and getting the environment path + if [ $(${python_exe} -m pip list | grep -c 'isaacsim-rl') -gt 0 ]; then + local isaac_path=$(${python_exe} -c "import isaacsim; import os; print(os.environ['ISAAC_PATH'])") + fi + fi + # check if there is a path available + if [ ! -d "${isaac_path}" ]; then + # throw an error if no path is found + echo -e "[ERROR] Unable to find the Isaac Sim directory: '${isaac_path}'" >&2 + echo -e "\tThis could be due to the following reasons:" >&2 + echo -e "\t1. Conda environment is not activated." >&2 + echo -e "\t2. Isaac Sim pip package 'isaacsim-rl' is not installed." >&2 + echo -e "\t3. Isaac Sim directory is not available at the default path: ${ISAACLAB_PATH}/_isaac_sim" >&2 + # exit the script + exit 1 + fi + # return the result + echo ${isaac_path} +} + +# extract the python from isaacsim +extract_python_exe() { + # check if using conda + if ! [[ -z "${CONDA_PREFIX}" ]]; then + # use conda python + local python_exe=${CONDA_PREFIX}/bin/python + elif ! [[ -z "${VIRTUAL_ENV}" ]]; then + # use uv virtual environment python + local python_exe=${VIRTUAL_ENV}/bin/python + else + # use kit python + local python_exe=${ISAACLAB_PATH}/_isaac_sim/python.sh + + if [ ! -f "${python_exe}" ]; then + # note: we need to check system python for cases such as docker + # inside docker, if user installed into system python, we need to use that + # otherwise, use the python from the kit + if [ $(python -m pip list | grep -c 'isaacsim-rl') -gt 0 ]; then + local python_exe=$(which python) + fi + fi + fi + # check if there is a python path available + if [ ! -f "${python_exe}" ]; then + echo -e "[ERROR] Unable to find any Python executable at path: '${python_exe}'" >&2 + echo -e "\tThis could be due to the following reasons:" >&2 + echo -e "\t1. Conda or uv environment is not activated." >&2 + echo -e "\t2. Isaac Sim pip package 'isaacsim-rl' is not installed." >&2 + echo -e "\t3. Python executable is not available at the default path: ${ISAACLAB_PATH}/_isaac_sim/python.sh" >&2 + exit 1 + fi + # return the result + echo ${python_exe} +} + +# find pip command based on virtualization +extract_pip_command() { + # detect if we're in a uv environment + if [ -n "${VIRTUAL_ENV}" ] && [ -f "${VIRTUAL_ENV}/pyvenv.cfg" ] && grep -q "uv" "${VIRTUAL_ENV}/pyvenv.cfg"; then + pip_command="uv pip install" + else + # retrieve the python executable + python_exe=$(extract_python_exe) + pip_command="${python_exe} -m pip install" + fi + + echo ${pip_command} +} + +extract_pip_uninstall_command() { + # detect if we're in a uv environment + if [ -n "${VIRTUAL_ENV}" ] && [ -f "${VIRTUAL_ENV}/pyvenv.cfg" ] && grep -q "uv" "${VIRTUAL_ENV}/pyvenv.cfg"; then + pip_uninstall_command="uv pip uninstall" + else + # retrieve the python executable + python_exe=$(extract_python_exe) + pip_uninstall_command="${python_exe} -m pip uninstall -y" + fi + + echo ${pip_uninstall_command} +} + +# setup uv environment for Isaac Lab Arena +setup_uv_env() { + # get environment name from input + local env_name="$1" + local python_path="$2" + + # check uv is installed + if ! command -v uv &>/dev/null; then + echo "[ERROR] uv could not be found. Please install uv and try again." + echo "[ERROR] uv can be installed here:" + echo "[ERROR] https://docs.astral.sh/uv/getting-started/installation/" + exit 1 + fi + + # check if _isaac_sim symlink exists and isaacsim-rl is not installed via pip + if [ ! -L "${ISAACLAB_PATH}/_isaac_sim" ] && ! python -m pip list | grep -q 'isaacsim-rl'; then + echo -e "[WARNING] _isaac_sim symlink not found at ${ISAACLAB_PATH}/_isaac_sim" + echo -e "\tThis warning can be ignored if you plan to install Isaac Sim via pip." + echo -e "\tIf you are using a binary installation of Isaac Sim, please ensure the symlink is created before setting up the conda environment." + fi + + # check if the environment exists + local env_path="${ISAACLAB_ARENA_PATH}/${env_name}" + if [ ! -d "${env_path}" ]; then + echo -e "[INFO] Creating uv environment named '${env_name}'..." + uv venv --clear --python "${python_path}" "${env_path}" + # Install pip so isaaclab.sh can use python -m pip + echo -e "[INFO] Installing pip in uv environment..." + uv pip install --python "${env_path}/bin/python" pip + else + echo "[INFO] uv environment '${env_name}' already exists." + fi + + # define root path for activation hooks + local isaaclab_root="${ISAACLAB_ARENA_PATH}" + + # cache current paths for later + cache_pythonpath=$PYTHONPATH + cache_ld_library_path=$LD_LIBRARY_PATH + + # ensure activate file exists + touch "${env_path}/bin/activate" + + # add variables to environment during activation + cat >> "${env_path}/bin/activate" <&2; + print_help + exit 0 +fi + +# pass the arguments +while [[ $# -gt 0 ]]; do + # read the key + case "$1" in + -i|--install) + # install the python packages in IsaacLab-Arena/submodules directory + echo "[INFO] Installing extensions inside the Isaac Lab Arena repository..." + python_exe=$(extract_python_exe) + pip_command=$(extract_pip_command) + pip_uninstall_command=$(extract_pip_uninstall_command) + export -f extract_python_exe + export -f extract_pip_command + export -f extract_pip_uninstall_command + if [ -z "$2" ]; then + framework_name="all" + elif [ "$2" = "none" ]; then + framework_name="none" + shift # past argument + else + framework_name=$2 + shift # past argument + fi + # Install IsaacLab + $ISAACLAB_PATH/isaaclab.sh -i $framework_name + # Install Isaac-GR00T + ${pip_command} -e "${ISAACLAB_ARENA_PATH}/submodules/Isaac-GR00T" + # Install IsaacLab Arena + ${pip_command} -e . + # unset local variables + unset extract_python_exe + unset extract_pip_command + unset extract_pip_uninstall_command + shift # past argument + ;; + -c|--conda) + # use default name if not provided + if [ -z "$2" ]; then + conda_env_name="env_isaaclab_arena" + else + conda_env_name=$2 + shift # past argument + fi + # setup the conda environment for Isaac Lab + $ISAACLAB_PATH/isaaclab.sh --conda ${conda_env_name} + shift # past argument + ;; + -u|--uv) + # use default name if not provided + if [ -z "$2" ]; then + echo "[INFO] Using default uv environment name: env_isaaclab_arena" + uv_env_name="env_isaaclab_arena" + else + echo "[INFO] Using uv environment name: $2" + uv_env_name=$2 + shift # past argument + fi + # setup the uv environment for Isaac Lab + setup_uv_env ${uv_env_name} + shift # past argument + ;; + -y|--pycharm) + # update the pycharm settings + update_pycharm_settings + shift # past argument + # exit neatly + break + ;; + -h|--help) + print_help + exit 0 + ;; + esac +done \ No newline at end of file diff --git a/isaaclab_arena/assets/object.py b/isaaclab_arena/assets/object.py index d08d9cb63..973421d17 100644 --- a/isaaclab_arena/assets/object.py +++ b/isaaclab_arena/assets/object.py @@ -9,6 +9,7 @@ from isaaclab_arena.assets.object_base import ObjectBase, ObjectType from isaaclab_arena.assets.object_utils import detect_object_type from isaaclab_arena.utils.pose import Pose +from isaaclab_arena.utils.usd_helpers import has_light, open_stage class Object(ObjectBase): @@ -26,7 +27,9 @@ def __init__( initial_pose: Pose | None = None, **kwargs, ): - assert name is not None and usd_path is not None + assert name is not None + if object_type is not ObjectType.SPAWNER: + assert usd_path is not None # Detect object type if not provided if object_type is None: object_type = detect_object_type(usd_path=usd_path) @@ -75,6 +78,9 @@ def _generate_articulation_cfg(self) -> ArticulationCfg: def _generate_base_cfg(self) -> AssetBaseCfg: assert self.object_type == ObjectType.BASE + with open_stage(self.usd_path) as stage: + if has_light(stage): + print("WARNING: Base object has lights, this may cause issues when using with multiple environments.") object_cfg = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/" + self.name, spawn=UsdFileCfg(usd_path=self.usd_path, scale=self.scale), @@ -82,6 +88,15 @@ def _generate_base_cfg(self) -> AssetBaseCfg: object_cfg = self._add_initial_pose_to_cfg(object_cfg) return object_cfg + def _generate_spawner_cfg(self) -> AssetBaseCfg: + assert self.object_type == ObjectType.SPAWNER + object_cfg = AssetBaseCfg( + prim_path=self.prim_path, + spawn=self.spawner_cfg, + ) + object_cfg = self._add_initial_pose_to_cfg(object_cfg) + return object_cfg + def _add_initial_pose_to_cfg( self, object_cfg: RigidObjectCfg | ArticulationCfg | AssetBaseCfg ) -> RigidObjectCfg | ArticulationCfg | AssetBaseCfg: diff --git a/isaaclab_arena/assets/object_base.py b/isaaclab_arena/assets/object_base.py index c2044b1b0..39ddbdaf5 100644 --- a/isaaclab_arena/assets/object_base.py +++ b/isaaclab_arena/assets/object_base.py @@ -19,6 +19,7 @@ class ObjectType(Enum): BASE = "base" RIGID = "rigid" ARTICULATION = "articulation" + SPAWNER = "spawner" class ObjectBase(Asset, ABC): @@ -53,6 +54,8 @@ def _init_object_cfg(self) -> RigidObjectCfg | ArticulationCfg | AssetBaseCfg: object_cfg = self._generate_articulation_cfg() elif self.object_type == ObjectType.BASE: object_cfg = self._generate_base_cfg() + elif self.object_type == ObjectType.SPAWNER: + object_cfg = self._generate_spawner_cfg() else: raise ValueError(f"Invalid object type: {self.object_type}") return object_cfg @@ -103,3 +106,7 @@ def _generate_articulation_cfg(self) -> ArticulationCfg: def _generate_base_cfg(self) -> AssetBaseCfg: # Subclasses must implement this method pass + + def _generate_spawner_cfg(self) -> AssetBaseCfg: + # Object Subclasses must implement this method + pass diff --git a/isaaclab_arena/assets/object_library.py b/isaaclab_arena/assets/object_library.py index 7618fd92e..2cee6a8b0 100644 --- a/isaaclab_arena/assets/object_library.py +++ b/isaaclab_arena/assets/object_library.py @@ -3,6 +3,10 @@ # # SPDX-License-Identifier: Apache-2.0 +from pathlib import Path + +import isaaclab.sim as sim_utils +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR from isaaclab_arena.affordances.openable import Openable @@ -11,7 +15,7 @@ from isaaclab_arena.assets.object_base import ObjectType from isaaclab_arena.assets.register import register_asset from isaaclab_arena.utils.pose import Pose - +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR class LibraryObject(Object): """ @@ -21,7 +25,7 @@ class LibraryObject(Object): name: str tags: list[str] - usd_path: str + usd_path: str | None = None object_type: ObjectType = ObjectType.RIGID scale: tuple[float, float, float] = (1.0, 1.0, 1.0) @@ -230,3 +234,66 @@ class BrownBox(LibraryObject): def __init__(self, prim_path: str | None = None, initial_pose: Pose | None = None): super().__init__(prim_path=prim_path, initial_pose=initial_pose) + + +@register_asset +class GroundPlane(LibraryObject): + """ + A ground plane. + """ + + name = "ground_plane" + tags = ["ground_plane"] + default_prim_path = "/World/GroundPlane" + object_type = ObjectType.SPAWNER + spawner_cfg = GroundPlaneCfg() + + def __init__(self, prim_path: str | None = default_prim_path, initial_pose: Pose | None = None): + super().__init__(prim_path=prim_path, initial_pose=initial_pose) + + +@register_asset +class DomeLight(LibraryObject): + """ + A dome light. + """ + + name = "dome_light" + tags = ["light"] + default_prim_path = "/World/defaultDomeLight" + object_type = ObjectType.SPAWNER + spawner_cfg = sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0) + + def __init__(self, prim_path: str | None = default_prim_path, initial_pose: Pose | None = None): + super().__init__(prim_path=prim_path, initial_pose=initial_pose) + +@register_asset +class IoBoard(LibraryObject): + """ + An IO board. + """ + + name = "object" + tags = ["object"] + usd_path = str(Path(__file__).parent / "object_library" / "6a_io_board.usd") + default_prim_path = "{ENV_REGEX_NS}/io_board" + scale = (1.0, 1.0, 1.0) + + def __init__(self, prim_path: str | None = None, initial_pose: Pose | None = None): + super().__init__(prim_path=prim_path, initial_pose=initial_pose) + +@register_asset +class PackingTable(LibraryObject): + """ + A packing table. + """ + + name = "packing_table" + tags = ["table"] + object_type = ObjectType.BASE + usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd" + default_prim_path = "{ENV_REGEX_NS}/PackingTable" + scale = (1.0, 1.0, 1.0) + + def __init__(self, prim_path: str | None = None, initial_pose: Pose | None = None): + super().__init__(prim_path=prim_path, initial_pose=initial_pose) \ No newline at end of file diff --git a/isaaclab_arena/assets/object_library/6a_io_board.usd b/isaaclab_arena/assets/object_library/6a_io_board.usd new file mode 100644 index 000000000..3436259b6 Binary files /dev/null and b/isaaclab_arena/assets/object_library/6a_io_board.usd differ diff --git a/isaaclab_arena/assets/object_reference.py b/isaaclab_arena/assets/object_reference.py index feb32d118..2b27716d5 100644 --- a/isaaclab_arena/assets/object_reference.py +++ b/isaaclab_arena/assets/object_reference.py @@ -23,6 +23,8 @@ def __init__(self, parent_asset: Asset, **kwargs): super().__init__(**kwargs) self.initial_pose_relative_to_parent = self._get_referenced_prim_pose_relative_to_parent(parent_asset) self.parent_asset = parent_asset + # Check that the object reference is not a spawner. + assert self.object_type != ObjectType.SPAWNER, "Object reference cannot be a spawner" self.object_cfg = self._init_object_cfg() def get_initial_pose(self) -> Pose: diff --git a/isaaclab_arena/embodiments/__init__.py b/isaaclab_arena/embodiments/__init__.py index 298988b15..1cbb7e958 100644 --- a/isaaclab_arena/embodiments/__init__.py +++ b/isaaclab_arena/embodiments/__init__.py @@ -3,6 +3,7 @@ # # SPDX-License-Identifier: Apache-2.0 +from .fii.fii import * from .franka.franka import * from .g1.g1 import * from .gr1t2.gr1t2 import * diff --git a/isaaclab_arena/embodiments/embodiment_library/Fiibot_W_1_V2_251016_Modified.usd b/isaaclab_arena/embodiments/embodiment_library/Fiibot_W_1_V2_251016_Modified.usd new file mode 100644 index 000000000..baca67413 Binary files /dev/null and b/isaaclab_arena/embodiments/embodiment_library/Fiibot_W_1_V2_251016_Modified.usd differ diff --git a/isaaclab_arena/embodiments/fii/__init__.py b/isaaclab_arena/embodiments/fii/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/isaaclab_arena/embodiments/fii/fii.py b/isaaclab_arena/embodiments/fii/fii.py new file mode 100644 index 000000000..09b7d181d --- /dev/null +++ b/isaaclab_arena/embodiments/fii/fii.py @@ -0,0 +1,650 @@ +from collections.abc import Sequence +from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase +from isaaclab_arena.assets.register import register_asset + +from isaaclab_arena.utils.pose import Pose +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.utils import configclass +from isaaclab.envs import ManagerBasedRLMimicEnv +import torch +import isaaclab.utils.math as PoseUtils +import numpy as np +import torch +import os +import math +import tempfile +from pathlib import Path + +from .swerve_ik import swerve_isosceles_ik +import isaaclab.controllers.utils as ControllerUtils + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import SceneEntityCfg +import isaaclab.envs.mdp as base_mdp + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.managers import EventTermCfg + +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.assets.articulation import Articulation +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.envs import ManagerBasedEnv + +from dataclasses import MISSING + +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from isaaclab.assets import ArticulationCfg +from isaaclab.utils import configclass + +from isaaclab.sensors import FrameTransformer, FrameTransformerCfg, OffsetCfg +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection + +def object_grasped( + env, + robot_cfg: SceneEntityCfg, + ee_frame_cfg: SceneEntityCfg, + object_cfg: SceneEntityCfg, + diff_threshold: float = 0.06, + gripper_open_val: float = 0.04, # 4cm open (adjust after testing) + gripper_threshold = 0.015 +) -> torch.Tensor: + """Check if an object is grasped by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + + object_pos = object.data.root_pos_w + + + any_grasped = torch.zeros(env.num_envs, device=env.device, dtype=torch.bool) + if hasattr(env.cfg.actions.upper_body_ik, "hand_joint_names"): + for i in range(len(env.cfg.actions.upper_body_ik.hand_joint_names)//2): + gripper_joint_ids, _ = robot.find_joints(env.cfg.actions.upper_body_ik.hand_joint_names[i*2:i*2+2]) + assert len(gripper_joint_ids) == 2, "Observations only support parallel gripper for now" + end_effector_pos = ee_frame.data.target_pos_w[:, i*2, :] + pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) + grasped = torch.logical_and( + pose_diff < diff_threshold, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[0]] + - torch.tensor(gripper_open_val, dtype=torch.float32).to(env.device) + ) + > gripper_threshold, + ) + end_effector_pos = ee_frame.data.target_pos_w[:, i*2+1, :] + pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) + grasped = torch.logical_and( + grasped, + pose_diff < diff_threshold, + ) + grasped = torch.logical_and( + grasped, + torch.abs( + robot.data.joint_pos[:, gripper_joint_ids[1]] + - torch.tensor(gripper_open_val, dtype=torch.float32).to(env.device) + ) + > gripper_threshold, + ) + any_grasped = any_grasped or grasped + return any_grasped + +@register_asset +class FiiEmbodiment(EmbodimentBase): + """ + Embodiment for the FII robot. + """ + + name = "fii" + + def __init__(self, enable_cameras: bool = False, initial_pose: Pose | None = None): + super().__init__(enable_cameras, initial_pose) + self.scene_config = FiiSceneCfg() + self.action_config = FiiActionsCfg() + self.observation_config = FiiObservationsCfg() + self.event_config = FiiEventCfg() + self.mimic_env = FiiMimicEnv + + # Convert USD to URDF for Pink IK controller + self.temp_urdf_dir = tempfile.gettempdir() + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene_config.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=False + ) + + # Set the URDF and mesh paths for the IK controller + self.action_config.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.action_config.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path + +#======================================================================= +# SCENE +#======================================================================= +@configclass +class FiiSceneCfg: + """Scene configuration for the FII embodiment.""" + + robot: ArticulationCfg = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + rot=(0.7071068, 0.0, 0.0, 0.7071068), + joint_pos={ + "jack_joint": 0.7, + "left_1_joint": 0.0, + "left_2_joint": 0.785398, + "left_3_joint": 0.0, + "left_4_joint": 1.570796, + "left_5_joint": 0.0, + "left_6_joint": -0.785398, + "left_7_joint": 0.0, + "right_1_joint": 0.0, + "right_2_joint": 0.785398, + "right_3_joint": 0.0, + "right_4_joint": 1.570796, + "right_5_joint": 0.0, + "right_6_joint": -0.785398, + "right_7_joint": 0.0, + } + ), + spawn=sim_utils.UsdFileCfg( + usd_path=str(Path(__file__).parent.parent / "embodiment_library" / "Fiibot_W_1_V2_251016_Modified.usd"), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ) + ), + actuators={ + "jack_joint": ImplicitActuatorCfg( + joint_names_expr=["jack_joint"], + damping=5000., + stiffness=500000. + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=["left_[0-9]_joint", "right_[0-9]_joint", "left_hand_.*", "right_hand_.*"], + damping=None, + stiffness=None + ), + "base": ImplicitActuatorCfg( + joint_names_expr=["walk_.*_joint", ".*_wheel_joint"], + damping=None, + stiffness=None + ), + "torso_and_head": ImplicitActuatorCfg( + joint_names_expr=["waist_joint", "head_.*_joint.*"], + damping=None, + stiffness=None + ), + }, + ) + + ee_frame: FrameTransformerCfg = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/robot/Fiibot_W_2_V2/base_link", + debug_vis=False, + target_frames=[ + # Left hand end-effector + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/robot/Fiibot_W_2_V2/left_hand_grip1_link", + name="left_hand_grip1", + offset=OffsetCfg(pos=[0.0, 0.0, 0.0]), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/robot/Fiibot_W_2_V2/left_hand_grip2_link", + name="left_hand_grip2", + offset=OffsetCfg(pos=[0.0, 0.0, 0.0]), + ), + # Right end-effector + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/robot/Fiibot_W_2_V2/right_hand_grip1_link", + name="right_hand_grip1", + offset=OffsetCfg(pos=[0.0, 0.0, 0.0]), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/robot/Fiibot_W_2_V2/right_hand_grip2_link", + name="right_hand_grip2", + offset=OffsetCfg(pos=[0.0, 0.0, 0.0]), + ), + ], + ) + +#======================================================================= +# ACTIONS +#======================================================================= + + +class FiibotLowerBodyAction(ActionTerm): + """Action term that is based on Agile lower body RL policy.""" + + cfg: "FiibotLowerBodyActionCfg" + """The configuration of the action term.""" + + _asset: Articulation + """The articulation asset to which the action term is applied.""" + + def __init__(self, cfg: "FiibotLowerBodyActionCfg", env: ManagerBasedEnv): + super().__init__(cfg, env) + + self._env = env + + self._joint_names = [ + "walk_mid_top_joint", + "walk_left_bottom_joint", + "walk_right_bottom_joint", + "jack_joint", + "front_wheel_joint", + "left_wheel_joint", + "right_wheel_joint" + ] + + self._joint_ids = [ + self._asset.data.joint_names.index(joint_name) + for joint_name in self._joint_names + ] + + self._joint_pos_target = torch.zeros(self.num_envs, 7, device=self.device) + self._joint_vel_target = torch.zeros(self.num_envs, 3, device=self.device) + + @property + def action_dim(self) -> int: + """Lower Body Action: [vx, vy, wz, jack_joint_height]""" + return 4 + + @property + def raw_actions(self) -> torch.Tensor: + return self._joint_pos_target + + @property + def processed_actions(self) -> torch.Tensor: + return self._joint_pos_target + + def process_actions(self, actions: torch.Tensor): + + ik_out = swerve_isosceles_ik( + vx=float(actions[0, 0]), + vy=float(actions[0, 1]), + wz=float(actions[0, 2]), + L1=0.30438, + d=0.17362, + w=0.25, + R=0.06 + ) + + self._joint_pos_target[:, 0] = ik_out['wheel1']['angle_rad'] + self._joint_pos_target[:, 1] = ik_out['wheel2']['angle_rad'] + self._joint_pos_target[:, 2] = ik_out['wheel3']['angle_rad'] + self._joint_pos_target[:, 3] = float(actions[0, 3]) + + self._joint_vel_target[:, 0] = ik_out['wheel1']['omega'] + self._joint_vel_target[:, 1] = ik_out['wheel2']['omega'] + self._joint_vel_target[:, 2] = ik_out['wheel3']['omega'] + + def apply_actions(self): + + self._joint_pos_target[:, 4:] = self._joint_pos_target[:, 4:] + self._env.physics_dt * self._joint_vel_target + + self._asset.set_joint_position_target( + target=self._joint_pos_target, + joint_ids=self._joint_ids + ) + + + +@configclass +class FiibotLowerBodyActionCfg(ActionTermCfg): + class_type: type[ActionTerm] = FiibotLowerBodyAction + + +@configclass +class FiiActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + # "waist_joint", + "left_1_joint", + "left_2_joint", + "left_3_joint", + "left_4_joint", + "left_5_joint", + "left_6_joint", + "left_7_joint", + "right_1_joint", + "right_2_joint", + "right_3_joint", + "right_4_joint", + "right_5_joint", + "right_6_joint", + "right_7_joint" + ], + hand_joint_names=[ + "left_hand_grip1_joint", + "left_hand_grip2_joint", + "right_hand_grip1_joint", + "right_hand_grip2_joint" + ], + target_eef_link_names={ + "left_wrist": "Fiibot_W_2_V2_left_7_Link", + "right_wrist": "Fiibot_W_2_V2_right_7_Link", + }, + asset_name="robot", + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=4, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + "Fiibot_W_2_V2_left_7_Link", + base_link_frame_name="Root", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=10, + gain=0.1, + ), + LocalFrameTask( + "Fiibot_W_2_V2_right_7_Link", + base_link_frame_name="Root", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=10, + gain=0.1, + ) + ], + fixed_input_tasks=[], + ) + ) + + lower_body_ik = FiibotLowerBodyActionCfg( + asset_name="robot" + ) + + +#======================================================================= +# OBSERVATIONS +#======================================================================= +@configclass +class FiiEventCfg: + """Configuration for FII events.""" + + reset_robot_joints = EventTermCfg( + func=base_mdp.reset_scene_to_default, + mode="reset", + params={"reset_joint_targets": True}, + ) + + +@configclass +class FiiObservationsCfg: + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_7_Link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_7_Link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_7_Link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_7_Link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [ + "left_hand_grip1_joint", + "left_hand_grip2_joint", + "right_hand_grip1_joint", + "right_hand_grip2_joint" + ]}) + + # Note: object_obs function hardcodes env.scene["object"], which doesn't exist in our scene + # We already have object_pos and object_rot observations above, so this is redundant + # object = ObsTerm( + # func=manip_mdp.object_obs, + # params={"left_eef_link_name": "left_7_Link", "right_eef_link_name": "right_7_Link"}, + # ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp = ObsTerm( + func=object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), # ← Now this works! + "object_cfg": SceneEntityCfg("object"), + "gripper_open_val": 0.04, + "gripper_threshold": 0.015, + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + +class FiiMimicEnv(ManagerBasedRLMimicEnv): + """Configuration for Fii Mimic.""" + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + eef_pos_name = f"{eef_name}_eef_pos" + eef_quat_name = f"{eef_name}_eef_quat" + + target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_wrist_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns + an environment action to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + noise: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"].clone()) + target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"].clone()) + + target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) + target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) + + # gripper actions + left_gripper_action = gripper_action_dict["left"] + right_gripper_action = gripper_action_dict["right"] + + if action_noise_dict is not None: + pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) + pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) + quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) + quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) + + target_left_eef_pos += pos_noise_left + target_right_eef_pos += pos_noise_right + target_left_eef_rot_quat += quat_noise_left + target_right_eef_rot_quat += quat_noise_right + + return torch.cat( + ( + target_left_eef_pos, + target_left_eef_rot_quat, + target_right_eef_pos, + target_right_eef_rot_quat, + left_gripper_action, + right_gripper_action, + torch.zeros(3), + torch.ones(1)*0.7, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + + target_poses = {} + + target_left_wrist_position = action[:, 0:3] + target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) + target_poses["left"] = target_pose_left + + target_right_wrist_position = action[:, 7:10] + target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) + target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) + target_poses["right"] = target_pose_right + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + + """ + Shape of actions: + left_wrist_pos shape: (3,) + left_wrist_quat shape: (4,) + right_wrist_pos shape: (3,) + right_wrist_quat shape: (4,) + left_gripper_action shape: (2,) + right_gripper_action shape: (2,) + navigate_cmd shape: (3,) + base_height_cmd shape: (1,) + """ + return {"left": actions[:, 14:16], "right": actions[:, 16:18]} + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Gets the pose of each object relevant to Isaac Lab Mimic data generation in the current scene. + + Args: + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A dictionary that maps object names to object pose matrix in pelvis frame (4x4 torch.Tensor) + """ + if env_ids is None: + env_ids = slice(None) + + # Get base link inverse transform to convert from world to base link frame + base_link_pose_w = self.scene["robot"].data.body_link_state_w[ + :, self.scene["robot"].data.body_names.index("base_link"), : + ] + base_link_position_w = base_link_pose_w[:, :3] - self.scene.env_origins + base_link_rot_mat_w = PoseUtils.matrix_from_quat(base_link_pose_w[:, 3:7]) + base_link_pose_mat_w = PoseUtils.make_pose(base_link_position_w, base_link_rot_mat_w) + base_link_pose_inv = PoseUtils.pose_inv(base_link_pose_mat_w) + + rigid_object_states = self.scene.get_state(is_relative=True)["rigid_object"] + object_pose_matrix = dict() + for obj_name, obj_state in rigid_object_states.items(): + object_pose_mat_w = PoseUtils.make_pose( + obj_state["root_pose"][env_ids, :3], PoseUtils.matrix_from_quat(obj_state["root_pose"][env_ids, 3:7]) + ) + object_pose_base_link_frame = torch.matmul(base_link_pose_inv, object_pose_mat_w) + object_pose_matrix[obj_name] = object_pose_base_link_frame + + return object_pose_matrix + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask term signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask term signal annotation. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + if env_ids is None: + env_ids = slice(None) + + signals = dict() + + subtask_terms = self.obs_buf["subtask_terms"] + if "grasp" in subtask_terms: + signals["grasp"] = subtask_terms["grasp"][env_ids] + + # Handle multiple grasp signals + for i in range(0, len(self.cfg.subtask_configs)): + grasp_key = f"grasp_{i + 1}" + if grasp_key in subtask_terms: + signals[grasp_key] = subtask_terms[grasp_key][env_ids] + # final subtask signal is not needed + return signals \ No newline at end of file diff --git a/isaaclab_arena/embodiments/fii/swerve_ik.py b/isaaclab_arena/embodiments/fii/swerve_ik.py new file mode 100644 index 000000000..9fde10d0a --- /dev/null +++ b/isaaclab_arena/embodiments/fii/swerve_ik.py @@ -0,0 +1,74 @@ +import math +from typing import Dict, Tuple + +def swerve_isosceles_ik( + vx: float, + vy: float, + wz: float, + L1: float, + d: float, + w: float, + R: float, +) -> Dict[str, Dict[str, float]]: + """ + Inverse kinematics for a 3-module swerve (independent steering + drive) + in an isosceles layout with +x pointing to Wheel 1. + + Geometry (meters): + r1 = [ L1, 0 ] + r2 = [ -d, w ] + r3 = [ -d, -w ] + + Inputs: + vx : chassis linear velocity in x (robot frame) [m/s] + vy : chassis linear velocity in y (robot frame) [m/s] + wz : chassis yaw rate about +z [rad/s] (CCW +) + L1 : distance from center to Wheel 1 along +x [m] + d : x offset magnitude of base wheels (W2/W3) at x = -d [m] + w : half spacing of base wheels (W2/W3) at y = ±w [m] + R : wheel radius [m] (for converting linear speed to wheel angular speed) + + Outputs (per wheel i in {1,2,3}): + angle_rad : steering angle α_i = atan2(v_i_y, v_i_x) [rad] (−π, π] + angle_deg : same, degrees + speed : wheel linear speed s_i = ||v_i|| [m/s] + omega : wheel angular speed φ̇_i = s_i / R [rad/s] + + Equations (standard rigid-body velocity addition): + v_i = [vx, vy]^T + wz * [-y_i, x_i]^T + α_i = atan2(v_i_y, v_i_x) + s_i = sqrt(v_i_x^2 + v_i_y^2) + φ̇_i = s_i / R + + References: + - WPILib swerve kinematics (maps chassis speeds → module angles & speeds). :contentReference[oaicite:0]{index=0} + - Rigid-body relation v_P = v_O + ω × r (planar form used above). :contentReference[oaicite:1]{index=1} + - Community derivations / implementation notes (angle optimization, etc.). :contentReference[oaicite:2]{index=2} + """ + + def module_state(xi: float, yi: float) -> Tuple[float, float, float, float]: + vix = vx + wz * (-yi) + viy = vy + wz * ( xi) + angle = math.atan2(viy, vix) # [-pi, pi] + speed = math.hypot(vix, viy) + omega = speed / R if R > 0 else float("inf") + return angle, math.degrees(angle), speed, omega + + # Module positions (isosceles) + r = { + "wheel1": ( L1, 0.0), + "wheel2": (-d , w ), + "wheel3": (-d , -w ), + } + + out = {} + for name, (xi, yi) in r.items(): + angle_rad, angle_deg, speed, omega = module_state(xi, yi) + out[name] = { + "angle_rad": angle_rad, + "angle_deg": angle_deg, + "speed": speed, + "omega": omega, + } + return out + diff --git a/isaaclab_arena/examples/example_environments/cli.py b/isaaclab_arena/examples/example_environments/cli.py index 3e476bc66..516d43e9e 100644 --- a/isaaclab_arena/examples/example_environments/cli.py +++ b/isaaclab_arena/examples/example_environments/cli.py @@ -20,6 +20,8 @@ ) from isaaclab_arena.examples.example_environments.press_button_environment import PressButtonEnvironment +from isaaclab_arena.examples.example_environments.fii_pick_place_environment import FiiPickPlaceEnvironment + # NOTE(alexmillane, 2025.09.04): There is an issue with type annotation in this file. # We cannot annotate types which require the simulation app to be started in order to # import, because this file is used to retrieve CLI arguments, so it must be imported @@ -34,6 +36,7 @@ GalileoPickAndPlaceEnvironment.name: GalileoPickAndPlaceEnvironment, GalileoG1LocomanipPickAndPlaceEnvironment.name: GalileoG1LocomanipPickAndPlaceEnvironment, PressButtonEnvironment.name: PressButtonEnvironment, + FiiPickPlaceEnvironment.name: FiiPickPlaceEnvironment, } diff --git a/isaaclab_arena/examples/example_environments/fii_pick_place_environment.py b/isaaclab_arena/examples/example_environments/fii_pick_place_environment.py new file mode 100644 index 000000000..3f2415ec6 --- /dev/null +++ b/isaaclab_arena/examples/example_environments/fii_pick_place_environment.py @@ -0,0 +1,40 @@ +import argparse + +from isaaclab_arena.examples.example_environments.example_environment_base import ExampleEnvironmentBase +from isaaclab_arena.utils.pose import Pose + +class FiiPickPlaceEnvironment(ExampleEnvironmentBase): + name: str = "fii_pick_place" + + def get_env(self, args_cli: argparse.Namespace): + from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment + from isaaclab_arena.scene.scene import Scene + from isaaclab_arena.tasks.fii_pick_and_place_task import FiiPickAndPlaceTask + from isaaclab.assets import AssetBaseCfg + import isaaclab.sim as sim_utils + object = self.asset_registry.get_asset_by_name("object")(initial_pose=Pose(position_xyz=(0.0, 0.75, 1.0))) + packing_table = self.asset_registry.get_asset_by_name("packing_table")(initial_pose=Pose(position_xyz=(0.0, 0.85, 0.))) + embodiment = self.asset_registry.get_asset_by_name("fii")() + ground = self.asset_registry.get_asset_by_name("ground_plane")() + dome_light = self.asset_registry.get_asset_by_name("dome_light")() + + if args_cli.teleop_device is not None: + teleop_device = self.device_registry.get_device_by_name(args_cli.teleop_device)() + else: + teleop_device = None + + scene = Scene(assets=[object, packing_table, ground, dome_light]) + + isaaclab_arena_environment = IsaacLabArenaEnvironment( + name=self.name, + embodiment=embodiment, + scene=scene, + task=FiiPickAndPlaceTask(object, packing_table, episode_length_s=20.0), + teleop_device=teleop_device, + ) + return isaaclab_arena_environment + + @staticmethod + def add_cli_args(parser: argparse.ArgumentParser) -> None: + parser.add_argument("--embodiment", type=str, default="fii") + parser.add_argument("--teleop_device", type=str, default=None) \ No newline at end of file diff --git a/isaaclab_arena/scripts/annotate_demos.py b/isaaclab_arena/scripts/annotate_demos.py index 23ccbe98b..c281f22f9 100644 --- a/isaaclab_arena/scripts/annotate_demos.py +++ b/isaaclab_arena/scripts/annotate_demos.py @@ -361,7 +361,7 @@ def annotate_episode_in_auto_mode( annotated_episode = env.recorder_manager.get_episode(0) subtask_term_signal_dict = annotated_episode.data["obs"]["datagen_info"]["subtask_term_signals"] for signal_name, signal_flags in subtask_term_signal_dict.items(): - if not torch.any(signal_flags): + if not torch.any(torch.tensor(signal_flags, dtype=torch.bool)): is_episode_annotated_successfully = False print(f'\tDid not detect completion for the subtask "{signal_name}".') return is_episode_annotated_successfully diff --git a/isaaclab_arena/tasks/fii_pick_and_place_task.py b/isaaclab_arena/tasks/fii_pick_and_place_task.py new file mode 100644 index 000000000..e056c38e0 --- /dev/null +++ b/isaaclab_arena/tasks/fii_pick_and_place_task.py @@ -0,0 +1,234 @@ +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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 +from dataclasses import MISSING + +from isaaclab.envs.common import ViewerCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from isaaclab_arena.assets.asset import Asset +from isaaclab_arena.tasks.task_base import TaskBase +from isaaclab.envs.common import ViewerCfg +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig + +import numpy as np + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.managers import SceneEntityCfg +import isaaclab.envs.mdp as base_mdp +from isaaclab.envs.common import ViewerCfg + +from isaaclab.managers import TerminationTermCfg as DoneTerm + +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +class FiiPickAndPlaceTask(TaskBase): + + def __init__( + self, + object: Asset, + packing_table: Asset, + episode_length_s: float | None = None, + ): + super().__init__(episode_length_s=episode_length_s) + self.object = object + self.packing_table = packing_table + self.termination_cfg = FiibotTerminationsCfg() + self.viewer_cfg = ViewerCfg( + eye=(0.0, 3.0, 1.5), lookat=(0.0, 0.0, 0.7), origin_type="asset_body", asset_name="robot", body_name="base_link" + ) + + def get_scene_cfg(self): + pass + + def get_termination_cfg(self): + return self.termination_cfg + + def get_events_cfg(self): + pass + + def get_viewer_cfg(self) -> ViewerCfg: + return self.viewer_cfg + def get_metrics(self): + pass + def get_prompt(self): + pass + def get_mimic_env_cfg(self, embodiment_name: str): + return FiiPickAndPlaceMimicEnvCfg( + object_name=self.object.name + ) +#======================================================================= +# TERMINATIONS +#======================================================================= +@configclass +class FiibotTerminationsCfg: + + time_out = DoneTerm(func=base_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={ + "object_cfg": SceneEntityCfg("object"), + "task_link_name": "right_7_Link", + "right_wrist_max_x": 0.26, + "min_x": 0.40, + "max_x": 0.85, + "min_y": 0.35, + "max_y": 0.8, + "max_height": 1.10, + "min_vel": 0.20, + }) + +@configclass +class FiiPickAndPlaceMimicEnvCfg(MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Fii Pick and Place env. + """ + + embodiment_name: str = "fii" + + object_name: str = "object" + + def __post_init__(self): + # post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "demo_src_fii_pick_and_place_isaac_lab_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = False + self.datagen_config.generation_num_trials = 100 + self.datagen_config.generation_select_src_per_subtask = False + self.datagen_config.generation_select_src_per_arm = False + self.datagen_config.generation_relative = False + self.datagen_config.generation_joint_pos = False + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the pick and place task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + # TODO(alexmillane, 2025.09.02): This is currently broken. FIX. + # We need a way to pass in a reference to an object that exists in the + # scene. + object_ref=self.object_name, + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["left"] = subtask_configs + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref=self.object_name, + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="grasp", + # Specifies time offsets for data generation when splitting a trajectory into + # subtask segments. Random offsets are added to the termination boundary. + subtask_term_offset_range=(10, 20), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + # TODO(alexmillane, 2025.09.02): This is currently broken. FIX. + # We need a way to pass in a reference to an object that exists in the + # scene. + object_ref=self.object_name, + # End of final subtask does not need to be detected + subtask_term_signal="drop", + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + # TODO(alexmillane, 2025.09.02): This is currently broken. FIX. + # We need a way to pass in a reference to an object that exists in the + # scene. + object_ref=self.object_name, + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["right"] = subtask_configs \ No newline at end of file diff --git a/isaaclab_arena/teleop_devices/__init__.py b/isaaclab_arena/teleop_devices/__init__.py index 99d4b4b32..4c1efe496 100644 --- a/isaaclab_arena/teleop_devices/__init__.py +++ b/isaaclab_arena/teleop_devices/__init__.py @@ -6,3 +6,4 @@ from .avp_handtracking import * from .keyboard import * from .spacemouse import * +from .handtracking import * diff --git a/isaaclab_arena/teleop_devices/handtracking.py b/isaaclab_arena/teleop_devices/handtracking.py new file mode 100644 index 000000000..7f2f08917 --- /dev/null +++ b/isaaclab_arena/teleop_devices/handtracking.py @@ -0,0 +1,208 @@ +# +# 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 isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg + +from isaaclab_arena.assets.register import register_device +from isaaclab_arena.teleop_devices.teleop_device_base import TeleopDeviceBase +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg + +import numpy as np +import torch +from dataclasses import dataclass +from typing import Final + +import isaaclab.sim as sim_utils +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +import isaaclab.utils.math as PoseUtils + + +# Register FiiRetargeter in the factory's RETARGETER_MAP +def _register_fii_retargeter(): + """Register FiiRetargeter with the teleop device factory.""" + try: + from isaaclab.devices.teleop_device_factory import RETARGETER_MAP + # Forward declare to avoid circular import + RETARGETER_MAP[type('FiiRetargeterCfg', (), {})] = None # Placeholder + except ImportError: + pass # Factory might not be loaded yet + + +@register_device +class HandTrackingTeleopDevice(TeleopDeviceBase): + """ + Teleop device for hand tracking. + """ + + name = "handtracking" + + def __init__(self, sim_device: str | None = None): + super().__init__(sim_device=sim_device) + self.xr = XrCfg(anchor_pos=(0., 0., 0.25), anchor_rot=(1.0, 0.0, 0.0, 0.0)) + + def get_teleop_device_cfg(self, embodiment: object | None = None): + return DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + FiiRetargeterCfg( + sim_device=self.sim_device, + num_open_xr_hand_joints=2 * 26, + enable_visualization=True + ) + ], + sim_device=self.sim_device, + xr_cfg=self.xr, + ), + } + ) + +@dataclass +class FiiRetargeterCfg(RetargeterCfg): + enable_visualization: bool = True + num_open_xr_hand_joints: int = 100 + + +class FiiRetargeter(RetargeterBase): + + def __init__(self, cfg: FiiRetargeterCfg): + """Initialize the retargeter.""" + self.cfg = cfg + self._sim_device = cfg.sim_device + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_hand_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + + base_vel = torch.tensor([0.0, 0.0, 0.0], dtype=torch.float32, device=self._sim_device) + base_height = torch.tensor([0.7], dtype=torch.float32, device=self._sim_device) + + left_eef_pose = torch.tensor([-0.3, 0.3, 0.72648, 1.0, 0., 0., 0.], dtype=torch.float32, device=self._sim_device) + right_eef_pose = torch.tensor([-0.3, 0.3, 0.72648, 1.0, 0., 0., 0.], dtype=torch.float32, device=self._sim_device) + + + left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] + right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + if left_wrist is not None: + left_eef_pose = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + if right_wrist is not None: + right_eef_pose = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + + # left_wrist_tensor = torch.tensor( + # self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + # ) + # right_wrist_tensor = torch.tensor( + # self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + # ) + + gripper_value_left = self._hand_data_to_gripper_values(data[OpenXRDevice.TrackingTarget.HAND_LEFT]) + gripper_value_right = self._hand_data_to_gripper_values(data[OpenXRDevice.TrackingTarget.HAND_RIGHT]) + + return torch.cat([left_eef_pose, right_eef_pose, gripper_value_left, gripper_value_right, base_vel, base_height]) + + def _hand_data_to_gripper_values(self, hand_data): + thumb_tip = hand_data["thumb_tip"] + index_tip = hand_data["index_tip"] + + distance = np.linalg.norm(thumb_tip[:3] - index_tip[:3]) + + finger_dist_closed = 0.00 + finger_dist_open = 0.06 + + gripper_value_closed = 0.06 + gripper_value_open = 0.00 + + t = np.clip((distance - finger_dist_closed) / (finger_dist_open - finger_dist_closed), 0, 1) + # t = 1 -> open + # t = 0 -> closed + gripper_joint_value = (1.0 - t) * gripper_value_closed + t * gripper_value_open + + return torch.tensor([gripper_joint_value, gripper_joint_value], dtype=torch.float32, device=self._sim_device) + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + wrist: Wrist pose data from OpenXR. + is_left: True for the left hand, False for the right hand. + + Returns: + Retargeted wrist pose in USD control frame. + """ + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + + if is_left: + # Corresponds to a rotation of (0, 90, 90) in euler angles (x,y,z) + combined_quat = torch.tensor([0, 0.7071, 0, 0.7071], dtype=torch.float32) + # combined_quat = torch.tensor([0, 0, 0, 1], dtype=torch.float32) + # combined_quat = torch.tensor([0.5, 0.5, 0.5, 0.5], dtype=torch.float32) + # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) + # combined_quat = torch.tensor([0, -0.7071, 0, 0.7071], dtype=torch.float32) + else: + # Corresponds to a rotation of (0, -90, -90) in euler angles (x,y,z) + combined_quat = torch.tensor([0, -0.7071, 0, 0.7071], dtype=torch.float32) + # combined_quat = torch.tensor([0, 0, 0, -1], dtype=torch.float32) + # combined_quat = torch.tensor([0.5, -0.5, -0.5, 0.5], dtype=torch.float32) + # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) + # combined_quat = torch.tensor([0, 0.7071, 0, 0.7071], dtype=torch.float32) + + offset = torch.tensor([0.0, 0.25, -0.15]) + transform_pose = PoseUtils.make_pose(offset, PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +# Register the FII retargeter with the factory after class definition +try: + from isaaclab.devices.teleop_device_factory import RETARGETER_MAP + RETARGETER_MAP[FiiRetargeterCfg] = FiiRetargeter +except ImportError: + pass # Factory not available yet \ No newline at end of file diff --git a/isaaclab_arena/tests/test_asset_registry.py b/isaaclab_arena/tests/test_asset_registry.py index a6e1c2b57..82e36d219 100644 --- a/isaaclab_arena/tests/test_asset_registry.py +++ b/isaaclab_arena/tests/test_asset_registry.py @@ -26,6 +26,12 @@ def _test_default_assets_registered(simulation_app): num_assets = len(asset_registry.get_assets_by_tag("object")) print(f"Number of pick up object assets registered: {num_assets}") assert num_assets > 0 + num_ground_plane_assets = len(asset_registry.get_assets_by_tag("ground_plane")) + print(f"Number of ground plane assets registered: {num_ground_plane_assets}") + assert num_ground_plane_assets > 0 + num_light_assets = len(asset_registry.get_assets_by_tag("light")) + print(f"Number of light assets registered: {num_light_assets}") + assert num_light_assets > 0 return True @@ -70,6 +76,15 @@ def _test_all_assets_in_registry(simulation_app): asset.set_initial_pose(pose) objects_in_registry.append(asset) objects_in_registry_names.append(asset.name) + # Add lights + for asset_cls in asset_registry.get_assets_by_tag("light"): + asset = asset_cls() + objects_in_registry.append(asset) + objects_in_registry_names.append(asset.name) + # Add ground plane + ground_plane = asset_registry.get_asset_by_name("ground_plane")() + objects_in_registry.append(ground_plane) + objects_in_registry_names.append(ground_plane.name) assert len(objects_in_registry) > 0 scene = Scene(assets=[background, *objects_in_registry]) diff --git a/isaaclab_arena/utils/usd_helpers.py b/isaaclab_arena/utils/usd_helpers.py index 5ec0e4479..f9661f867 100644 --- a/isaaclab_arena/utils/usd_helpers.py +++ b/isaaclab_arena/utils/usd_helpers.py @@ -5,7 +5,7 @@ from contextlib import contextmanager -from pxr import Usd, UsdPhysics +from pxr import Usd, UsdLux, UsdPhysics def get_all_prims( @@ -34,6 +34,24 @@ def get_all_prims( return prims_list +def has_light(stage: Usd.Stage) -> bool: + """Check if the stage has a light""" + LIGHT_TYPES = ( + UsdLux.SphereLight, + UsdLux.RectLight, + UsdLux.DomeLight, + UsdLux.DistantLight, + UsdLux.DiskLight, + ) + has_light = False + all_prims = get_all_prims(stage) + for prim in all_prims: + if any(prim.IsA(t) for t in LIGHT_TYPES): + has_light = True + break + return has_light + + def is_articulation_root(prim: Usd.Prim) -> bool: """Check if prim is articulation root""" return prim.HasAPI(UsdPhysics.ArticulationRootAPI) diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf new file mode 100644 index 000000000..e057336b6 --- /dev/null +++ b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf @@ -0,0 +1,1497 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/head_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/head_link.STL new file mode 100644 index 000000000..2ee5fba15 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/head_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 000000000..69de84901 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 000000000..8864e9f98 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_ankle_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link.STL new file mode 100644 index 000000000..1a96d99ba Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link_merge.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link_merge.STL new file mode 100644 index 000000000..c3b9dad9b Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_elbow_link_merge.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_0_link.STL new file mode 100644 index 000000000..8069369af Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_1_link.STL new file mode 100644 index 000000000..89d231d7e Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_index_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_0_link.STL new file mode 100644 index 000000000..8069369af Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_1_link.STL new file mode 100644 index 000000000..89d231d7e Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_middle_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_palm_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_palm_link.STL new file mode 100644 index 000000000..7d595ed8f Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_palm_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_0_link.STL new file mode 100644 index 000000000..3028bb4d6 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_1_link.STL new file mode 100644 index 000000000..d1c080c86 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_2_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_2_link.STL new file mode 100644 index 000000000..8b32e9663 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hand_thumb_2_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 000000000..5b751c767 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_roll_link.STL new file mode 100644 index 000000000..778437ffe Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 000000000..383093ab9 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_hip_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_knee_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_knee_link.STL new file mode 100644 index 000000000..f2e98e54e Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_knee_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_rubber_hand.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_rubber_hand.STL new file mode 100644 index 000000000..c44830f1f Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_rubber_hand.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 000000000..e698311fb Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 000000000..80bca84ac Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 000000000..281e69905 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_shoulder_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 000000000..82cc224a8 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 000000000..f3c263a7a Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_rubber_hand.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_rubber_hand.STL new file mode 100644 index 000000000..8fa435b66 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_roll_rubber_hand.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 000000000..31be4fd45 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/left_wrist_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/logo_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/logo_link.STL new file mode 100644 index 000000000..e97920985 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/logo_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis.STL new file mode 100644 index 000000000..691a779b9 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis_contour_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis_contour_link.STL new file mode 100644 index 000000000..42434339a Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/pelvis_contour_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 000000000..e77d8a2fe Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 000000000..d4261dd7c Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_ankle_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link.STL new file mode 100644 index 000000000..f259e3812 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link_merge.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link_merge.STL new file mode 100644 index 000000000..83ce0ba0c Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_elbow_link_merge.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_0_link.STL new file mode 100644 index 000000000..f87ad3212 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_1_link.STL new file mode 100644 index 000000000..6dea51ad9 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_index_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_0_link.STL new file mode 100644 index 000000000..f87ad3212 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_1_link.STL new file mode 100644 index 000000000..6dea51ad9 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_middle_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_palm_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_palm_link.STL new file mode 100644 index 000000000..5ae00a783 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_palm_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_0_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_0_link.STL new file mode 100644 index 000000000..1cae7f18e Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_0_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_1_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_1_link.STL new file mode 100644 index 000000000..c141fbf5a Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_1_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_2_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_2_link.STL new file mode 100644 index 000000000..e942923c5 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hand_thumb_2_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 000000000..998a0a0f5 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_roll_link.STL new file mode 100644 index 000000000..47b2eebdd Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 000000000..371856427 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_hip_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_knee_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_knee_link.STL new file mode 100644 index 000000000..76d21a3d8 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_knee_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_rubber_hand.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_rubber_hand.STL new file mode 100644 index 000000000..0aacffbb8 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_rubber_hand.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 000000000..3f5b4ed47 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 000000000..179d61753 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 000000000..2ba6076a8 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_shoulder_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_pitch_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 000000000..da194543c Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_pitch_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 000000000..26868d228 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_rubber_hand.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_rubber_hand.STL new file mode 100644 index 000000000..c365aa9d8 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_roll_rubber_hand.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 000000000..d78890286 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/right_wrist_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_link.STL new file mode 100644 index 000000000..75d82f578 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_rod_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_rod_link.STL new file mode 100644 index 000000000..6747f3f93 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_L_rod_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_link.STL new file mode 100644 index 000000000..5cb5958ba Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_rod_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_rod_link.STL new file mode 100644 index 000000000..95cf415f7 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_constraint_R_rod_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link.STL new file mode 100644 index 000000000..17745af9c Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link_rev_1_0.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link_rev_1_0.STL new file mode 100644 index 000000000..8a759a709 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/torso_link_rev_1_0.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_L.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_L.STL new file mode 100644 index 000000000..911410fa5 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_L.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_R.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_R.STL new file mode 100644 index 000000000..babe79492 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_constraint_R.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link.STL new file mode 100644 index 000000000..65831abd2 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link_rev_1_0.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link_rev_1_0.STL new file mode 100644 index 000000000..a64f330c5 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_roll_link_rev_1_0.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_support_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_support_link.STL new file mode 100644 index 000000000..63660fb6e Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_support_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link.STL new file mode 100644 index 000000000..7d36b02cc Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link_rev_1_0.STL b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link_rev_1_0.STL new file mode 100644 index 000000000..0fabb63d3 Binary files /dev/null and b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/meshes/waist_yaw_link_rev_1_0.STL differ diff --git a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/utils/g1.py b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/utils/g1.py index 06dda3540..22f425289 100644 --- a/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/utils/g1.py +++ b/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/utils/g1.py @@ -32,12 +32,12 @@ def instantiate_g1_robot_model( """ robot_model_config = { - "asset_path": f"{ISAACLAB_NUCLEUS_DIR}/Arena/wbc_policy/robot_model/g1/", - "urdf_path": f"{ISAACLAB_NUCLEUS_DIR}/Arena/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf", + "asset_path": f"/workspaces/isaaclab_arena/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/", + "urdf_path": f"/workspaces/isaaclab_arena/isaaclab_arena_g1/g1_whole_body_controller/wbc_policy/robot_model/g1/g1_29dof_with_hand.urdf", } - asset_path_local = retrieve_file_path(robot_model_config["asset_path"], force_download=True) - urdf_path_local = retrieve_file_path(robot_model_config["urdf_path"], force_download=True) + asset_path_local = robot_model_config["asset_path"] + urdf_path_local = robot_model_config["urdf_path"] assert waist_location in [ "lower_body", diff --git a/setup.py b/setup.py index efd26a653..206069407 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,16 @@ version=ISAACLAB_ARENA_VERSION_NUMBER, description="Isaac Lab - Arena. An Isaac Lab extension for robotic policy evaluation. ", packages=find_packages(include=["isaaclab_arena*", "isaaclab_arena_g1*", "isaaclab_arena_gr00t*"]), + install_requires=[ + "pytest", + "jupyter", + "typing_extensions", + "onnxruntime", + "qpsolvers==4.8.1", + "vuer[all]", + "lightwheel-sdk", + "huggingface-hub[cli]", + ], python_requires=">=3.10", zip_safe=False, )